Merge pull request #3234 from UAVenture/altitude_message

WIP: Altitude message

msg/vehicle_global_position.msg

@@ -18,3 +18,4 @@ float32 epv			# Standard deviation of position vertically
 float32 terrain_alt		# Terrain altitude in m, WGS84
 bool terrain_alt_valid		# Terrain altitude estimate is valid
 bool dead_reckoning		# True if this position is estimated through dead-reckoning
+float32 pressure_alt		# Pressure altitude

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1012,6 +1012,9 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 		_global_pos.terrain_alt_valid = false;
 	}
 
+	/* baro altitude */
+	_global_pos.pressure_alt = _ekf->baroHgt;
+
 	_global_pos.yaw = _local_pos.yaw;
 
 	const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -711,6 +711,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().terrain_alt = 0;
 		_pub_gpos.get().terrain_alt_valid = false;
 		_pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout;
+		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
 		_pub_gpos.update();
 	}
 }

src/modules/mavlink/mavlink_main.cpp

@@ -1689,6 +1689,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
+		configure_stream("ALTITUDE", 1.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1715,6 +1716,7 @@ Mavlink::task_main(int argc, char *argv[])
 		//camera trigger is rate limited at the source, do not limit here
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
+		configure_stream("ALTITUDE", 10.0f);
 		break;
 
 	case MAVLINK_MODE_OSD:
@@ -1730,6 +1732,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("RC_CHANNELS", 5.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
+		configure_stream("ALTITUDE", 1.0f);
 		break;
 
 	case MAVLINK_MODE_MAGIC:
@@ -1760,6 +1763,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_RAW_INT", 20.0f);
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
+		configure_stream("ALTITUDE", 10.0f);
 
 	default:
 		break;

src/modules/mavlink/mavlink_messages.cpp

@@ -2545,6 +2545,91 @@ protected:
 	}
 };
 
+class MavlinkStreamAltitude : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamAltitude::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "ALTITUDE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ALTITUDE;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamAltitude(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_ALTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	MavlinkOrbSubscription *_global_pos_sub;
+	uint64_t _global_pos_time;
+
+	MavlinkOrbSubscription *_local_pos_sub;
+	uint64_t _local_pos_time;
+
+	MavlinkOrbSubscription *_home_sub;
+	uint64_t _home_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamAltitude(MavlinkStreamAltitude &);
+	MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &);
+
+protected:
+	explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_global_pos_time(0),
+		_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
+		_local_pos_time(0),
+		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
+		_home_time(0)
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_global_position_s global_pos;
+		struct vehicle_local_position_s local_pos;
+		struct home_position_s home;
+
+		bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos);
+		updated |= _local_pos_sub->update(&_local_pos_time, &local_pos);
+		updated |= _home_sub->update(&_home_time, &home);
+
+		if (updated) {
+			mavlink_altitude_t msg;
+
+			msg.time_usec = hrt_absolute_time();
+
+			msg.altitude_monotonic = global_pos.pressure_alt;
+			msg.altitude_amsl = global_pos.alt;
+			msg.altitude_local = -local_pos.z;
+			msg.altitude_relative = home.alt;
+
+			if (global_pos.terrain_alt_valid) {
+				msg.altitude_terrain = global_pos.terrain_alt;
+				msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt;
+			} else {
+				msg.altitude_terrain = -1001;
+				msg.bottom_clearance = -1;
+			}
+
+			_mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg);
+		}
+	}
+};
+
 const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
@@ -2582,5 +2667,6 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static),
 	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
 	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static),
+	new StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static),
 	nullptr
 };

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -1338,6 +1338,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					global_pos.terrain_alt_valid = false;
 				}
 
+				global_pos.pressure_alt = sensor.baro_alt_meter[0];
+
 				if (vehicle_global_position_pub == NULL) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);