Mavlink: GLOBAL_POSITION for aux positioning (#26307)

* mavlink: add GLOBAL_POSITION message handling
- Add handler for incoming GLOBAL_POSITION MAVLink messages
- Publish received positions to aux_global_position uORB topic
- Update GLOBAL_POSITION stream to use aux_global_position topic
* correctly handle multi AGP in mavlink pub
* move from GLOBAL_POSITION to GLOBAL_POSITION_SENSOR
* mavlink: update submodule to include GLOBAL_POSITION_SENSOR in common.xml

Author: haumarco

src/modules/mavlink/mavlink

@@ -1436,9 +1436,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f);
 		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
-#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
-		configure_stream_local("GLOBAL_POSITION", 5.0f);
-#endif
+		configure_stream_local("GLOBAL_POSITION_SENSOR", 5.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
 #if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
 		configure_stream_local("GNSS_INTEGRITY", 1.0f);
@@ -1779,9 +1777,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
-#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
-		configure_stream_local("GLOBAL_POSITION", 10.0f);
-#endif
+		configure_stream_local("GLOBAL_POSITION_SENSOR", 10.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 #if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
@@ -1847,9 +1843,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 0.5f);
 		configure_stream_local("GLOBAL_POSITION_INT", 2.0f);
-#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
-		configure_stream_local("GLOBAL_POSITION", 2.0f);
-#endif
+		configure_stream_local("GLOBAL_POSITION_SENSOR", 2.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS2_RAW", 2.0f);
 		configure_stream_local("GPS_RAW_INT", 2.0f);

src/modules/mavlink/mavlink_main.cpp

@@ -75,9 +75,7 @@
 #include "streams/ESTIMATOR_STATUS.hpp"
 #include "streams/EXTENDED_SYS_STATE.hpp"
 #include "streams/FLIGHT_INFORMATION.hpp"
-#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
-#include "streams/GLOBAL_POSITION.hpp"
-#endif //MAVLINK_MSG_ID_GLOBAL_POSITION
+#include "streams/GLOBAL_POSITION_SENSOR.hpp"
 #include "streams/GLOBAL_POSITION_INT.hpp"
 #if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
 #include "streams/GNSS_INTEGRITY.hpp"
@@ -519,9 +517,9 @@ static const StreamListItem streams_list[] = {
 #if defined(CURRENT_MODE_HPP)
 	create_stream_list_item<MavlinkStreamCurrentMode>(),
 #endif // CURRENT_MODE_HPP
-#if defined(GLOBAL_POSITION_HPP)
-	create_stream_list_item<MavlinkStreamGLobalPosition>(),
-#endif // GLOBAL_POSITION_HPP
+#if defined(GLOBAL_POSITION_SENSOR_HPP)
+	create_stream_list_item<MavlinkStreamGlobalPositionSensor>(),
+#endif // GLOBAL_POSITION_SENSOR_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/mavlink_messages.cpp

@@ -213,6 +213,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_follow_target(msg);
 		break;
 
+	case MAVLINK_MSG_ID_GLOBAL_POSITION_SENSOR:
+		handle_message_global_position_sensor(msg);
+		break;
+
 	case MAVLINK_MSG_ID_LANDING_TARGET:
 		handle_message_landing_target(msg);
 		break;
@@ -2492,6 +2496,31 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	_sensor_gps_pub.publish(gps);
 }
 
+void
+MavlinkReceiver::handle_message_global_position_sensor(mavlink_message_t *msg)
+{
+	mavlink_global_position_sensor_t global_pos;
+	mavlink_msg_global_position_sensor_decode(msg, &global_pos);
+
+	aux_global_position_s aux_global_position{};
+	const hrt_abstime now = hrt_absolute_time();
+	aux_global_position.timestamp = now;
+	aux_global_position.timestamp_sample = now;
+
+	aux_global_position.id = global_pos.id;
+	aux_global_position.source = global_pos.source;
+
+	aux_global_position.lat = global_pos.lat * 1e-7;
+	aux_global_position.lon = global_pos.lon * 1e-7;
+	aux_global_position.alt = global_pos.alt;
+
+	aux_global_position.lat_lon_reset_counter = 0;
+	aux_global_position.eph = global_pos.eph;
+	aux_global_position.epv = global_pos.epv;
+
+	_aux_global_position_pub.publish(aux_global_position);
+}
+
 void
 MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.cpp

@@ -111,6 +111,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/velocity_limits.h>
+#include <uORB/topics/aux_global_position.h>
 
 #if !defined(CONSTRAINED_FLASH)
 # include <uORB/topics/debug_array.h>
@@ -208,7 +209,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
 	void handle_message_gimbal_device_information(mavlink_message_t *msg);
 	void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg);
-
+	void handle_message_global_position_sensor(mavlink_message_t *msg);
 #if !defined(CONSTRAINED_FLASH)
 	void handle_message_debug(mavlink_message_t *msg);
 	void handle_message_debug_float_array(mavlink_message_t *msg);
@@ -329,7 +330,6 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 	uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
-
 #if !defined(CONSTRAINED_FLASH)
 	uORB::Publication<debug_array_s>			_debug_array_pub {ORB_ID(debug_array)};
 	uORB::Publication<debug_key_value_s>			_debug_key_value_pub{ORB_ID(debug_key_value)};
@@ -339,6 +339,7 @@ class MavlinkReceiver : public ModuleParams
 
 	// ORB publications (multi)
 	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<aux_global_position_s>		_aux_global_position_pub{ORB_ID(aux_global_position)};
 	uORB::PublicationMulti<gps_inject_data_s>		_gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	uORB::PublicationMulti<input_rc_s>			_rc_pub{ORB_ID(input_rc)};
 	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_input_pub{ORB_ID(manual_control_input)};

src/modules/mavlink/mavlink_receiver.h

@@ -31,75 +31,79 @@
  *
  ****************************************************************************/
 
-#ifndef GLOBAL_POSITION_HPP
-#define GLOBAL_POSITION_HPP
+#ifndef GLOBAL_POSITION_SENSOR_HPP
+#define GLOBAL_POSITION_SENSOR_HPP
 
 #include <stdint.h>
 
 #include <uORB/topics/aux_global_position.h>
 
-class MavlinkStreamGLobalPosition : public MavlinkStream
+class MavlinkStreamGlobalPositionSensor : public MavlinkStream
 {
 public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGLobalPosition(mavlink); }
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGlobalPositionSensor(mavlink); }
 
-	static constexpr const char *get_name_static() { return "GLOBAL_POSITION"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GLOBAL_POSITION; }
+	static constexpr const char *get_name_static() { return "GLOBAL_POSITION_SENSOR"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GLOBAL_POSITION_SENSOR; }
 
 	const char *get_name() const override { return get_name_static(); }
 	uint16_t get_id() override { return get_id_static(); }
 
 	unsigned get_size() override
 	{
-		return _aux_global_position_sub.advertised() ? (MAVLINK_MSG_ID_GLOBAL_POSITION_LEN +
+		return _aux_global_position_sub.advertised() ? (MAVLINK_MSG_ID_GLOBAL_POSITION_SENSOR_LEN +
 				MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	explicit MavlinkStreamGLobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+	explicit MavlinkStreamGlobalPositionSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)};
+	uORB::SubscriptionMultiArray<aux_global_position_s, 4> _aux_global_position_sub{ORB_ID::aux_global_position};
 
 	bool send() override
 	{
-		vehicle_global_position_s pos{};
+		aux_global_position_s pos{};
+		bool sent = false;
 
-		if (_aux_global_position_sub.update(&pos)) {
-			mavlink_global_position_t msg{};
+		for (int i = 0; i < _aux_global_position_sub.size(); i++) {
+			if (_aux_global_position_sub[i].update(&pos)) {
+				mavlink_global_position_sensor_t msg{};
 
-			msg.id = UINT8_C(1);
-			msg.time_usec = pos.timestamp;
-			msg.source = GLOBAL_POSITION_UNKNOWN;
-			msg.flags = 0;
+				msg.target_system = 0;
+				msg.target_component = 0;
+				msg.id = pos.id;
+				msg.time_usec = pos.timestamp;
+				msg.source = pos.source;
 
-			if (PX4_ISFINITE(pos.lat)) {
-				msg.lat = static_cast<int32_t>(pos.lat * 1e7);
+				if (PX4_ISFINITE(pos.lat)) {
+					msg.lat = static_cast<int32_t>(pos.lat * 1e7);
 
-			} else {
-				msg.lat = INT32_MAX;
-			}
+				} else {
+					msg.lat = INT32_MAX;
+				}
 
-			if (PX4_ISFINITE(pos.lon)) {
-				msg.lon = static_cast<int32_t>(pos.lon * 1e7);
+				if (PX4_ISFINITE(pos.lon)) {
+					msg.lon = static_cast<int32_t>(pos.lon * 1e7);
 
-			} else {
-				msg.lon = INT32_MAX;
-			}
+				} else {
+					msg.lon = INT32_MAX;
+				}
 
-			msg.alt = pos.alt;
-			msg.alt_ellipsoid = pos.alt_ellipsoid;
+				msg.alt = pos.alt;
+				msg.alt_ellipsoid = pos.alt;
 
-			msg.eph = pos.eph;
-			msg.epv = pos.epv;
+				msg.eph = pos.eph;
+				msg.epv = pos.epv;
 
 
-			mavlink_msg_global_position_send_struct(_mavlink->get_channel(), &msg);
+				mavlink_msg_global_position_sensor_send_struct(_mavlink->get_channel(), &msg);
 
-			return true;
+				sent = true;
+			}
 		}
 
-		return false;
+		return sent;
 	}
 };
 
-#endif // GLOBAL_POSITION_HPP
+#endif // GLOBAL_POSITION_SENSOR_HPP