EKF2: support multiple AGP sources

msg/CMakeLists.txt

@@ -248,6 +248,7 @@ set(msg_files
 	versioned/AirspeedValidated.msg
 	versioned/ArmingCheckReply.msg
 	versioned/ArmingCheckRequest.msg
+	versioned/AuxGlobalPosition.msg
 	versioned/BatteryStatus.msg
 	versioned/ConfigOverrides.msg
 	versioned/FixedWingLateralSetpoint.msg

msg/px4_msgs_old/msg/VehicleGlobalPositionV0.msg

@@ -0,0 +1,36 @@
+# Fused global position in WGS84.
+# This struct contains global position estimation. It is not the raw GPS
+# measurement (@see vehicle_gps_position). This topic is usually published by the position
+# estimator, which will take more sources of information into account than just GPS,
+# e.g. control inputs of the vehicle in a Kalman-filter implementation.
+#
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
+float64 lat			# Latitude, (degrees)
+float64 lon			# Longitude, (degrees)
+float32 alt			# Altitude AMSL, (meters)
+float32 alt_ellipsoid		# Altitude above ellipsoid, (meters)
+
+bool lat_lon_valid
+bool alt_valid
+
+float32 delta_alt 	# Reset delta for altitude
+float32 delta_terrain   # Reset delta for terrain
+uint8 lat_lon_reset_counter	# Counter for reset events on horizontal position coordinates
+uint8 alt_reset_counter 	# Counter for reset events on altitude
+uint8 terrain_reset_counter     # Counter for reset events on terrain
+
+float32 eph			# Standard deviation of horizontal position error, (metres)
+float32 epv			# Standard deviation of vertical position error, (metres)
+
+float32 terrain_alt		# Terrain altitude WGS84, (metres)
+bool terrain_alt_valid		# Terrain altitude estimate is valid
+
+bool dead_reckoning		# True if this position is estimated through dead-reckoning
+
+# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
+# TOPICS estimator_global_position

msg/translation_node/translations/all_translations.h

@@ -7,6 +7,7 @@
 #include <translation_util.h>
 
 #include "translation_airspeed_validated_v1.h"
+#include "translation_aux_global_position_v1.h"
 #include "translation_arming_check_reply_v1.h"
 #include "translation_arming_check_request_v1.h"
 #include "translation_battery_status_v1.h"

msg/translation_node/translations/translation_aux_global_position_v1.h

@@ -0,0 +1,58 @@
+/****************************************************************************
+ * Copyright (c) 2026 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <cmath>
+
+// Translate aux_global_position: VehicleGlobalPosition v0 <--> AuxGlobalPosition v1
+#include <px4_msgs_old/msg/vehicle_global_position_v0.hpp>
+#include <px4_msgs/msg/aux_global_position.hpp>
+
+class AuxGlobalPositionV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::VehicleGlobalPositionV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::AuxGlobalPosition;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/in/aux_global_position";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		msg_newer.timestamp = msg_older.timestamp;
+		msg_newer.timestamp_sample = msg_older.timestamp_sample;
+		msg_newer.id = 1;
+		msg_newer.source = MessageNewer::SOURCE_VISION;
+		msg_newer.lat = msg_older.lat;
+		msg_newer.lon = msg_older.lon;
+		msg_newer.alt = msg_older.alt_valid ? msg_older.alt : NAN;
+		msg_newer.eph = msg_older.eph;
+		msg_newer.epv = msg_older.epv;
+		msg_newer.lat_lon_reset_counter = msg_older.lat_lon_reset_counter;
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		msg_older.timestamp = msg_newer.timestamp;
+		msg_older.timestamp_sample = msg_newer.timestamp_sample;
+		msg_older.lat = msg_newer.lat;
+		msg_older.lon = msg_newer.lon;
+		msg_older.alt = std::isnan(msg_newer.alt) ? 0.0f : msg_newer.alt;
+		msg_older.alt_ellipsoid = 0.0f;
+		msg_older.lat_lon_valid = true;
+		msg_older.alt_valid = !std::isnan(msg_newer.alt);
+		msg_older.delta_alt = 0.0f;
+		msg_older.delta_terrain = 0.0f;
+		msg_older.lat_lon_reset_counter = msg_newer.lat_lon_reset_counter;
+		msg_older.alt_reset_counter = 0;
+		msg_older.terrain_reset_counter = 0;
+		msg_older.eph = std::isnan(msg_newer.eph) ? 0.0f : msg_newer.eph;
+		msg_older.epv = std::isnan(msg_newer.epv) ? 0.0f : msg_newer.epv;
+		msg_older.terrain_alt = 0.0f;
+		msg_older.terrain_alt_valid = false;
+		msg_older.dead_reckoning = false;
+	}
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(AuxGlobalPositionV1Translation);

msg/versioned/AuxGlobalPosition.msg

@@ -0,0 +1,31 @@
+# Auxiliary global position
+#
+# This message provides global position data from an external source such as
+# pseudolites, visual navigation, or other positioning system.
+
+uint32 MESSAGE_VERSION = 1
+
+uint64 timestamp # [us] Time since system start
+uint64 timestamp_sample # [us] Timestamp of the raw data
+
+uint8 id # [-] Unique identifier for the AGP source
+uint8 source # [@enum SOURCE] Source type of the position data (based on mavlink::GLOBAL_POSITION_SRC)
+uint8 SOURCE_UNKNOWN = 0 # Unknown source
+uint8 SOURCE_GNSS = 1 # GNSS
+uint8 SOURCE_VISION = 2 # Vision
+uint8 SOURCE_PSEUDOLITES = 3 # Pseudolites
+uint8 SOURCE_TERRAIN = 4 # Terrain
+uint8 SOURCE_MAGNETIC = 5 # Magnetic
+uint8 SOURCE_ESTIMATOR = 6 # Estimator
+
+# lat, lon: required for horizontal position fusion, alt: required for vertical position fusion
+float64 lat # [deg] Latitude in WGS84
+float64 lon # [deg] Longitude in WGS84
+float32 alt # [m] [@invalid NaN] Altitude above mean sea level (AMSL)
+
+float32 eph # [m] [@invalid NaN] Std dev of horizontal position, lower bounded by NOISE param
+float32 epv # [m] [@invalid NaN] Std dev of vertical position, lower bounded by NOISE param
+
+uint8 lat_lon_reset_counter # [-] Counter for reset events on horizontal position coordinates
+
+# TOPICS aux_global_position

msg/versioned/VehicleGlobalPosition.msg

@@ -34,4 +34,3 @@ bool dead_reckoning		# True if this position is estimated through dead-reckoning
 
 # TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position
 # TOPICS estimator_global_position
-# TOPICS aux_global_position

src/modules/ekf2/CMakeLists.txt

@@ -141,7 +141,10 @@ if(CONFIG_EKF2_AIRSPEED)
 endif()
 
 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
+	list(APPEND EKF_SRCS
+		EKF/aid_sources/aux_global_position/aux_global_position.cpp
+		EKF/aid_sources/aux_global_position/aux_global_position_control.cpp
+	)
 	list(APPEND EKF_MODULE_PARAMS params_aux_global_position.yaml)
 endif()
 

src/modules/ekf2/EKF/CMakeLists.txt

@@ -60,10 +60,6 @@ if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp)
 endif()
 
-if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp)
-endif()
-
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp)
 endif()