ekf2: move AGP subs to manager class to always route messages to correct source

with set ID on boot!

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/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 = 0
+
+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()

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -32,158 +32,155 @@
  ****************************************************************************/
 
 #include "ekf.h"
-
-#include "aid_sources/aux_global_position/aux_global_position.hpp"
+#include <aid_sources/aux_global_position/aux_global_position.hpp>
+#include <aid_sources/aux_global_position/aux_global_position_control.hpp>
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
+AuxGlobalPosition::AuxGlobalPosition() : ModuleParams(nullptr)
+{
+	for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
+		_id_param_values[slot] = getAgpParamInt32("ID", slot);
+
+		if (_id_param_values[slot] != 0) {
+			_sources[slot] = new AgpSource(slot, this);
+			_n_sources++;
+		}
+	}
+
+	// Only subscribe to uORB instances if there are configured sources
+	if (_n_sources > 0) {
+		for (int i = 0; i < MAX_AGP_IDS; i++) {
+			_agp_sub[i] = uORB::Subscription(ORB_ID(aux_global_position), i);
+		}
+	}
+}
+
+AuxGlobalPosition::~AuxGlobalPosition()
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		delete _sources[i];
+	}
+}
+
 void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
-
-#if defined(MODULE_NAME)
-
-	if (_aux_global_position_sub.updated()) {
-
-		vehicle_global_position_s aux_global_position{};
-		_aux_global_position_sub.copy(&aux_global_position);
-
-		const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(_param_ekf2_agp_delay.get() * 1000);
-
-		AuxGlobalPositionSample sample{};
-		sample.time_us = time_us;
-		sample.latitude = aux_global_position.lat;
-		sample.longitude = aux_global_position.lon;
-		sample.altitude_amsl = aux_global_position.alt;
-		sample.eph = aux_global_position.eph;
-		sample.epv = aux_global_position.epv;
-		sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
-
-		_aux_global_position_buffer.push(sample);
-
-		_time_last_buffer_push = imu_delayed.time_us;
+	// If there are no sources configured, we also don't subscribe to any uORB topic
+	// and skip the update completely.
+	if (_n_sources == 0) {
+		return;
 	}
 
-#endif // MODULE_NAME
+	for (int instance = 0; instance < MAX_AGP_IDS; instance++) {
+		if (_agp_sub[instance].updated()) {
+			aux_global_position_s msg{};
+			_agp_sub[instance].copy(&msg);
 
-	AuxGlobalPositionSample sample;
+			int slot = _instance_slot_map[instance];
 
-	if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
-
-		if (!(_param_ekf2_agp_ctrl.get() & static_cast<int32_t>(Ctrl::kHPos))) {
-			return;
-		}
-
-		estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position;
-		const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
-		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
-
-		// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
-		float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
-		const float pos_var = sq(pos_noise);
-		const Vector2f pos_obs_var(pos_var, pos_var);
-
-		ekf.updateAidSourceStatus(aid_src,
-					  sample.time_us,                                      // sample timestamp
-					  matrix::Vector2d(sample.latitude, sample.longitude), // observation
-					  pos_obs_var,                                         // observation variance
-					  innovation,                                          // innovation
-					  Vector2f(ekf.getPositionVariance()) + pos_obs_var,   // innovation variance
-					  math::max(_param_ekf2_agp_gate.get(), 1.f));         // innovation gate
-
-		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
-						 && ekf.control_status_flags().yaw_align;
-		const bool continuing_conditions = starting_conditions
-						   && ekf.global_origin_valid();
-
-		switch (_state) {
-		case State::kStopped:
-
-		/* FALLTHROUGH */
-		case State::kStarting:
-			if (starting_conditions) {
-				_state = State::kStarting;
-
-				if (ekf.global_origin_valid()) {
-					const bool fused = ekf.fuseHorizontalPosition(aid_src);
-					bool reset = false;
-
-					if (!fused && isResetAllowed(ekf)) {
-						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
-						reset = true;
-					}
-
-					if (fused || reset) {
-						ekf.enableControlStatusAuxGpos();
-						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
-						_state = State::kActive;
-					}
-
-				} else {
-					// Try to initialize using measurement
-					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
-								      sq(sample.epv))) {
-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
-						ekf.enableControlStatusAuxGpos();
-						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
-						_state = State::kActive;
-					}
-				}
+			if (slot < 0) {
+				slot = mapSensorIdToSlot(msg.id);
+				_instance_slot_map[instance] = static_cast<int8_t>(slot);
 			}
 
-			break;
-
-		case State::kActive:
-			if (continuing_conditions) {
-				ekf.fuseHorizontalPosition(aid_src);
-
-				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
-				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
-					if (isResetAllowed(ekf)) {
-
-						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
-
-						ekf.resetAidSourceStatusZeroInnovation(aid_src);
-
-						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
-
-					} else {
-						ekf.disableControlStatusAuxGpos();
-						_state = State::kStopped;
-					}
-				}
-
-			} else {
-				ekf.disableControlStatusAuxGpos();
-				_state = State::kStopped;
+			if (slot >= 0 && _sources[slot]) {
+				_sources[slot]->bufferData(msg, imu_delayed);
 			}
-
-			break;
-
-		default:
-			break;
 		}
+	}
 
-#if defined(MODULE_NAME)
-		aid_src.timestamp = hrt_absolute_time();
-		_estimator_aid_src_aux_global_position_pub.publish(aid_src);
-
-		_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
-#endif // MODULE_NAME
-
-	} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
-		ekf.disableControlStatusAuxGpos();
-		_state = State::kStopped;
-		ECL_WARN("Aux global position data stopped");
+	for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
+		if (_sources[slot]) {
+			if (_sources[slot]->update(ekf, imu_delayed)) {
+				// Only update one source per update cycle
+				break;
+			}
+		}
 	}
 }
 
-bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
+void AuxGlobalPosition::paramsUpdated()
 {
-	return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
-		&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
-	       || ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
-		   && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i]) {
+			_sources[i]->updateParams();
+		}
+	}
 }
 
-#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+float AuxGlobalPosition::testRatioFiltered() const
+{
+	float max_ratio = 0.f;
+
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i] && _sources[i]->isFusing()) {
+			max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered());
+		}
+	}
+
+	return max_ratio;
+}
+
+bool AuxGlobalPosition::anySourceFusing() const
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i] && _sources[i]->isFusing()) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instance) const
+{
+	char param_name[20] {};
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
+
+	int32_t value = 0;
+
+	if (param_get(param_find(param_name), &value) != 0) {
+		PX4_ERR("failed to get %s", param_name);
+	}
+
+	return value;
+}
+
+bool AuxGlobalPosition::setAgpParamInt32(const char *param_suffix, int instance, int32_t value)
+{
+	char param_name[20] {};
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
+
+	return param_set_no_notification(param_find(param_name), &value) == PX4_OK;
+}
+
+int32_t AuxGlobalPosition::getIdParam(int instance)
+{
+	return _id_param_values[instance];
+}
+
+void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id)
+{
+	setAgpParamInt32("ID", instance, sensor_id);
+	_id_param_values[instance] = sensor_id;
+}
+
+int AuxGlobalPosition::mapSensorIdToSlot(int32_t sensor_id)
+{
+	for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
+		if (getIdParam(slot) == sensor_id) {
+			return slot;
+		}
+	}
+
+	for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
+		if (getIdParam(slot) == 0) {
+			setIdParam(slot, sensor_id);
+			return slot;
+		}
+	}
+
+	return -1;
+}
+
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp

@@ -34,38 +34,26 @@
 #ifndef EKF_AUX_GLOBAL_POSITION_HPP
 #define EKF_AUX_GLOBAL_POSITION_HPP
 
-// interface?
-//  - ModuleParams
-//  - Base class EKF
-//  -  bool update(imu)
-//   how to get delay?
-//  WelfordMean for init?
-//  WelfordMean for rate
-
 #include "../../common.h"
-#include <lib/ringbuffer/TimestampedRingBuffer.hpp>
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
-#if defined(MODULE_NAME)
-# include <px4_platform_common/module_params.h>
-# include <uORB/PublicationMulti.hpp>
-# include <uORB/Subscription.hpp>
-# include <uORB/topics/estimator_aid_source2d.h>
-# include <uORB/topics/vehicle_global_position.h>
-#endif // MODULE_NAME
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/log.h>
+#include <lib/parameters/param.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/aux_global_position.h>
+#include <aid_sources/aux_global_position/aux_global_position_control.hpp>
 
 class Ekf;
 
 class AuxGlobalPosition : public ModuleParams
 {
 public:
-	AuxGlobalPosition() : ModuleParams(nullptr)
-	{
-		_estimator_aid_src_aux_global_position_pub.advertise();
-	}
+	static constexpr uint8_t MAX_AGP_IDS = 4;
 
-	~AuxGlobalPosition() = default;
+	AuxGlobalPosition();
+	~AuxGlobalPosition();
 
 	void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
 
@@ -74,70 +62,29 @@ public:
 		updateParams();
 	}
 
-	float test_ratio_filtered() const { return _test_ratio_filtered; }
+	/**
+	 * Returns the maximum filtered test ratio across all active AGP sources.
+	 */
+	float testRatioFiltered() const;
+	bool anySourceFusing() const;
+	int32_t getIdParam(int instance);
+	void setIdParam(int instance, int32_t sensor_id);
+	int mapSensorIdToSlot(int32_t sensor_id);
+	void paramsUpdated();
 
 private:
-	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const
-	{
-		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us);
-	}
+	AgpSource *_sources[MAX_AGP_IDS] {};
+	uORB::Subscription _agp_sub[MAX_AGP_IDS];
+	int8_t _instance_slot_map[MAX_AGP_IDS] {-1, -1, -1, -1};
+	uint8_t _n_sources{0};
 
-	bool isResetAllowed(const Ekf &ekf) const;
+	int32_t getAgpParamInt32(const char *param_suffix, int instance) const;
+	bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value);
 
-	struct AuxGlobalPositionSample {
-		uint64_t time_us{};     ///< timestamp of the measurement (uSec)
-		double latitude{};
-		double longitude{};
-		float altitude_amsl{};
-		float eph{};
-		float epv{};
-		uint8_t lat_lon_reset_counter{};
-	};
+	int32_t _id_param_values[MAX_AGP_IDS] {};
 
-	estimator_aid_source2d_s _aid_src_aux_global_position{};
-	TimestampedRingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
-	uint64_t _time_last_buffer_push{0};
-
-	enum class Ctrl : uint8_t {
-		kHPos  = (1 << 0),
-		kVPos  = (1 << 1)
-	};
-
-	enum class Mode : uint8_t {
-		kAuto           = 0,   	///< Reset on fusion timeout if no other source of position is available
-		kDeadReckoning = 1   	///< Reset on fusion timeout if no source of velocity is availabl
-	};
-
-	enum class State {
-		kStopped,
-		kStarting,
-		kActive,
-	};
-
-	State _state{State::kStopped};
-
-	float _test_ratio_filtered{INFINITY};
-
-#if defined(MODULE_NAME)
-	struct reset_counters_s {
-		uint8_t lat_lon{};
-	};
-	reset_counters_s _reset_counters{};
-
-	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)};
-	uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::EKF2_AGP_CTRL>) _param_ekf2_agp_ctrl,
-		(ParamInt<px4::params::EKF2_AGP_MODE>) _param_ekf2_agp_mode,
-		(ParamFloat<px4::params::EKF2_AGP_DELAY>) _param_ekf2_agp_delay,
-		(ParamFloat<px4::params::EKF2_AGP_NOISE>) _param_ekf2_agp_noise,
-		(ParamFloat<px4::params::EKF2_AGP_GATE>) _param_ekf2_agp_gate
-	)
-
-#endif // MODULE_NAME
 };
 
-#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME
 
 #endif // !EKF_AUX_GLOBAL_POSITION_HPP

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+#include <aid_sources/aux_global_position/aux_global_position_control.hpp>
+#include <aid_sources/aux_global_position/aux_global_position.hpp>
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+
+AgpSource::AgpSource(int slot, AuxGlobalPosition *manager)
+	: _manager(manager)
+	, _slot(slot)
+{
+	initParams();
+	advertise();
+}
+
+void AgpSource::initParams()
+{
+	char param_name[20] {};
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", _slot);
+	_param_handles.ctrl = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_MODE", _slot);
+	_param_handles.mode = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_DELAY", _slot);
+	_param_handles.delay = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_NOISE", _slot);
+	_param_handles.noise = param_find(param_name);
+
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_GATE", _slot);
+	_param_handles.gate = param_find(param_name);
+
+	updateParams();
+}
+
+void AgpSource::updateParams()
+{
+	if (_param_handles.ctrl == PARAM_INVALID) {
+		return;
+	}
+
+	param_get(_param_handles.ctrl, &_params.ctrl);
+	param_get(_param_handles.mode, &_params.mode);
+	param_get(_param_handles.delay, &_params.delay);
+	param_get(_param_handles.noise, &_params.noise);
+	param_get(_param_handles.gate, &_params.gate);
+}
+
+void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed)
+{
+	const int64_t time_us = msg.timestamp_sample
+				- static_cast<int64_t>(_params.delay * 1000);
+
+	AuxGlobalPositionSample sample{};
+	sample.time_us = time_us;
+	sample.id = msg.id;
+	sample.latitude = msg.lat;
+	sample.longitude = msg.lon;
+	sample.altitude_amsl = msg.alt;
+	sample.eph = msg.eph;
+	sample.epv = msg.epv;
+	sample.lat_lon_reset_counter = msg.lat_lon_reset_counter;
+
+	_buffer.push(sample);
+	_time_last_buffer_push = imu_delayed.time_us;
+}
+
+bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
+{
+	AuxGlobalPositionSample sample;
+
+	if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
+
+		if (!(_params.ctrl & static_cast<int32_t>(Ctrl::kHPos))) {
+			return true;
+		}
+
+		const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
+		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
+
+		float pos_noise = math::max(sample.eph, _params.noise, 0.01f);
+		const float pos_var = sq(pos_noise);
+		const Vector2f pos_obs_var(pos_var, pos_var);
+
+		ekf.updateAidSourceStatus(_aid_src,
+					  sample.time_us,                                      // sample timestamp
+					  matrix::Vector2d(sample.latitude, sample.longitude), // observation
+					  pos_obs_var,                                         // observation variance
+					  innovation,                                          // innovation
+					  Vector2f(ekf.getPositionVariance()) + pos_obs_var,   // innovation variance
+					  math::max(_params.gate, 1.f));                              // innovation gate
+
+		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
+						 && ekf.control_status_flags().yaw_align;
+		const bool continuing_conditions = starting_conditions
+						   && ekf.global_origin_valid();
+
+		switch (_state) {
+		case State::kStopped:
+
+		/* FALLTHROUGH */
+		case State::kStarting:
+			if (starting_conditions) {
+				_state = State::kStarting;
+
+				if (ekf.global_origin_valid()) {
+					const bool fused = ekf.fuseHorizontalPosition(_aid_src);
+					bool reset = false;
+
+					if (!fused && isResetAllowed(ekf)) {
+						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(_aid_src.observation_variance));
+						ekf.resetAidSourceStatusZeroInnovation(_aid_src);
+						reset = true;
+					}
+
+					if (fused || reset) {
+						ekf.enableControlStatusAuxGpos();
+						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+						_state = State::kActive;
+					}
+
+				} else {
+					// Try to initialize using measurement
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
+								      sq(sample.epv))) {
+						ekf.resetAidSourceStatusZeroInnovation(_aid_src);
+						ekf.enableControlStatusAuxGpos();
+						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+						_state = State::kActive;
+					}
+				}
+			}
+
+			break;
+
+		case State::kActive:
+			if (continuing_conditions) {
+				ekf.fuseHorizontalPosition(_aid_src);
+
+				if (isTimedOut(_aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
+				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
+					if (isResetAllowed(ekf)) {
+
+						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(_aid_src.observation_variance));
+
+						ekf.resetAidSourceStatusZeroInnovation(_aid_src);
+
+						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+
+					} else {
+						_state = State::kStopped;
+
+						if (!_manager->anySourceFusing()) {
+							ekf.disableControlStatusAuxGpos();
+						}
+					}
+				}
+
+			} else {
+				_state = State::kStopped;
+
+				if (!_manager->anySourceFusing()) {
+					ekf.disableControlStatusAuxGpos();
+				}
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+		_aid_src.timestamp = hrt_absolute_time();
+		_aid_src_pub.publish(_aid_src);
+
+		_test_ratio_filtered = math::max(fabsf(_aid_src.test_ratio_filtered[0]), fabsf(_aid_src.test_ratio_filtered[1]));
+
+		return true;
+
+	} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
+		_state = State::kStopped;
+
+		if (!_manager->anySourceFusing()) {
+			ekf.disableControlStatusAuxGpos();
+		}
+
+		ECL_INFO("Aux global position data stopped for slot %d", _slot);
+	}
+
+	return false;
+}
+
+bool AgpSource::isResetAllowed(const Ekf &ekf) const
+{
+	return ((static_cast<Mode>(_params.mode) == Mode::kAuto)
+		&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
+	       || ((static_cast<Mode>(_params.mode) == Mode::kDeadReckoning)
+		   && !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
+}
+
+bool AgpSource::isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const
+{
+	return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us);
+}
+
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.hpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef EKF_AUX_GLOBAL_POSITION_CONTROL_HPP
+#define EKF_AUX_GLOBAL_POSITION_CONTROL_HPP
+
+#include "../../common.h"
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+
+#include <lib/parameters/param.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/estimator_aid_source2d.h>
+#include <uORB/topics/aux_global_position.h>
+
+class Ekf;
+class AuxGlobalPosition;
+
+class AgpSource
+{
+public:
+	AgpSource(int slot, AuxGlobalPosition *manager);
+	~AgpSource() = default;
+
+	void bufferData(const aux_global_position_s &msg, const estimator::imuSample &imu_delayed);
+	bool update(Ekf &ekf, const estimator::imuSample &imu_delayed);
+	void advertise() { _aid_src_pub.advertise(); }
+	void initParams();
+	void updateParams();
+
+	float getTestRatioFiltered() const { return _test_ratio_filtered; }
+	bool isFusing() const { return _state == State::kActive; }
+
+private:
+	struct AuxGlobalPositionSample {
+		uint64_t time_us{};
+		uint8_t id{};
+		double latitude{};
+		double longitude{};
+		float altitude_amsl{};
+		float eph{};
+		float epv{};
+		uint8_t lat_lon_reset_counter{};
+	};
+
+	enum class Ctrl : uint8_t {
+		kHPos  = (1 << 0),
+		kVPos  = (1 << 1)
+	};
+
+	enum class Mode : uint8_t {
+		kAuto           = 0,   	///< Reset on fusion timeout if no other source of position is available
+		kDeadReckoning = 1   	///< Reset on fusion timeout if no source of velocity is available
+	};
+
+	enum class State {
+		kStopped,
+		kStarting,
+		kActive,
+	};
+
+	struct reset_counters_s {
+		uint8_t lat_lon{};
+	};
+
+	uORB::PublicationMulti<estimator_aid_source2d_s> _aid_src_pub{ORB_ID(estimator_aid_src_aux_global_position)};
+
+	TimestampedRingBuffer<AuxGlobalPositionSample> _buffer{20};
+	State _state{State::kStopped};
+	estimator_aid_source2d_s _aid_src{};
+	float _test_ratio_filtered{0.f};
+	uint64_t _time_last_buffer_push{0};
+	reset_counters_s _reset_counters{};
+
+	AuxGlobalPosition *_manager;
+	int _slot;
+
+	struct ParamHandles {
+		param_t ctrl{PARAM_INVALID};
+		param_t mode{PARAM_INVALID};
+		param_t delay{PARAM_INVALID};
+		param_t noise{PARAM_INVALID};
+		param_t gate{PARAM_INVALID};
+	} _param_handles;
+
+	struct Params {
+		int32_t ctrl{0};
+		int32_t mode{0};
+		float delay{0.f};
+		float noise{10.f};
+		float gate{3.f};
+	} _params;
+
+	bool isResetAllowed(const Ekf &ekf) const;
+	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const;
+};
+
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME
+
+#endif // !EKF_AUX_GLOBAL_POSITION_CONTROL_HPP

src/modules/ekf2/EKF/ekf.cpp

@@ -419,7 +419,8 @@ void Ekf::updateParameters()
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
-	_aux_global_position.updateParameters();
+
+	_aux_global_position.paramsUpdated();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
 }
 

src/modules/ekf2/EKF/ekf.h

@@ -435,6 +435,7 @@ public:
 	void updateParameters();
 
 	friend class AuxGlobalPosition;
+	friend class AgpSource;
 
 private:
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -541,7 +541,7 @@ float Ekf::getHorizontalPositionInnovationTestRatio() const
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 
 	if (_control_status.flags.aux_gpos) {
-		test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));
+		test_ratio = math::max(test_ratio, fabsf(_aux_global_position.testRatioFiltered()));
 	}
 
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

src/modules/ekf2/params_aux_global_position.yaml

@@ -1,10 +1,22 @@
+__max_num_agp_instances: &max_num_agp_instances 4
+
 module_name: ekf2
 parameters:
 - group: EKF2
   definitions:
-    EKF2_AGP_CTRL:
+    EKF2_AGP${i}_ID:
       description:
-        short: Aux global position (AGP) sensor aiding
+        short: Auxiliary global position sensor ${i} ID
+        long: 'Sensor ID for slot ${i}. Set to 0 to disable this slot.'
+      type: int32
+      default: 0
+      min: 0
+      max: 255
+      num_instances: *max_num_agp_instances
+      instance_start: 0
+    EKF2_AGP${i}_CTRL:
+      description:
+        short: Auxiliary global position sensor ${i} aiding
         long: 'Set bits in the following positions to enable: 0 : Horizontal position
           fusion 1 : Vertical position fusion'
       type: bitmask
@@ -14,9 +26,11 @@ parameters:
       default: 0
       min: 0
       max: 3
-    EKF2_AGP_MODE:
+      num_instances: *max_num_agp_instances
+      instance_start: 0
+    EKF2_AGP${i}_MODE:
       description:
-        short: Fusion reset mode
+        short: Fusion reset mode for sensor ${i}
         long: 'Automatic: reset on fusion timeout if no other source of position is available
           Dead-reckoning: reset on fusion timeout if no source of velocity is available'
       type: enum
@@ -24,28 +38,34 @@ parameters:
         0: Automatic
         1: Dead-reckoning
       default: 0
-    EKF2_AGP_DELAY:
+      num_instances: *max_num_agp_instances
+      instance_start: 0
+    EKF2_AGP${i}_DELAY:
       description:
-        short: Aux global position estimator delay relative to IMU measurements
+        short: Auxiliary global position sensor ${i} delay (to IMU)
       type: float
       default: 0
       min: 0
-      max: 300
+      max: 1000
       unit: ms
       reboot_required: true
       decimal: 1
-    EKF2_AGP_NOISE:
+      num_instances: *max_num_agp_instances
+      instance_start: 0
+    EKF2_AGP${i}_NOISE:
       description:
-        short: Measurement noise for aux global position measurements
+        short: Measurement noise for auxiliary global position sensor ${i}
         long:  Used to lower bound or replace the uncertainty included in the message
       type: float
-      default: 0.9
+      default: 1.0
       min: 0.01
       unit: m
       decimal: 2
-    EKF2_AGP_GATE:
+      num_instances: *max_num_agp_instances
+      instance_start: 0
+    EKF2_AGP${i}_GATE:
       description:
-        short: Gate size for aux global position fusion
+        short: Gate size for auxiliary global position sensor ${i} fusion
         long: Sets the number of standard deviations used by the innovation consistency
           test.
       type: float
@@ -53,3 +73,5 @@ parameters:
       min: 1.0
       unit: SD
       decimal: 1
+      num_instances: *max_num_agp_instances
+      instance_start: 0

src/modules/logger/logged_topics.cpp

@@ -210,7 +210,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("vehicle_imu_status", 1000, 4);
 	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
 	add_topic("vehicle_optical_flow", 500);
-	add_topic("aux_global_position", 500);
+	add_topic_multi("aux_global_position", 500);
 	add_optional_topic("pps_capture");
 
 	// additional control allocation logging
@@ -319,7 +319,7 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("vehicle_magnetometer");
 	add_topic("vehicle_status");
 	add_topic("vehicle_visual_odometry");
-	add_topic("aux_global_position");
+	add_topic_multi("aux_global_position");
 	add_topic_multi("distance_sensor");
 }
 

src/modules/mavlink/streams/GLOBAL_POSITION.hpp

@@ -36,7 +36,7 @@
 
 #include <stdint.h>
 
-#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/aux_global_position.h>
 
 class MavlinkStreamGLobalPosition : public MavlinkStream
 {

src/modules/replay/ReplayEkf2.cpp

@@ -53,6 +53,7 @@
 #include <uORB/topics/vehicle_optical_flow.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/aux_global_position.h>
 
 #include "ReplayEkf2.hpp"
 

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

@@ -112,7 +112,7 @@ void SensorAgpSim::Run()
 		_time_last_update = now;
 
 		if (!(_param_sim_agp_fail.get() & static_cast<int32_t>(FailureMode::Stuck))) {
-			_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);
+			_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt);
 		}
 
 		if (_param_sim_agp_fail.get() & static_cast<int32_t>(FailureMode::Drift)) {
@@ -129,17 +129,16 @@ void SensorAgpSim::Run()
 					 CONSTANTS_RADIUS_OF_EARTH);
 		const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f));
 
-		vehicle_global_position_s sample{};
+		aux_global_position_s sample{};
 
+		sample.id = 33;
 		sample.timestamp_sample = gpos.timestamp_sample;
 		sample.lat = latitude;
 		sample.lon = longitude;
 		sample.alt = altitude;
-		sample.lat_lon_valid = true;
-		sample.alt_ellipsoid = altitude;
-		sample.alt_valid = true;
 		sample.eph = 20.f;
 		sample.epv = 5.f;
+		sample.lat_lon_reset_counter = 0;
 
 		sample.timestamp = hrt_absolute_time();
 		_aux_global_position_pub.publish(sample);

src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp

@@ -44,6 +44,7 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/aux_global_position.h>
 
 using namespace time_literals;
 
@@ -83,7 +84,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
 
-	uORB::PublicationMulti<vehicle_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
+	uORB::PublicationMulti<aux_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -52,7 +52,7 @@ publications:
 
   - topic: /fmu/out/transponder_report
     type: px4_msgs::msg::TransponderReport
- 
+
   # - topic: /fmu/out/vehicle_angular_velocity
   #   type: px4_msgs::msg::VehicleAngularVelocity
 
@@ -191,7 +191,7 @@ subscriptions:
     type: px4_msgs::msg::ActuatorServos
 
   - topic: /fmu/in/aux_global_position
-    type: px4_msgs::msg::VehicleGlobalPosition
+    type: px4_msgs::msg::AuxGlobalPosition
 
   - topic: /fmu/in/fixed_wing_longitudinal_setpoint
     type: px4_msgs::msg::FixedWingLongitudinalSetpoint