ekf2: implement multi-instance AGP fusion

- Refactor aux_global_position into manager + per-source AgpSource class
- Support up to 4 independent AGP sources simultaneously
- Dynamic parameter lookup without boilerplate switch statements
- Per-source state tracking and fusion logic
- Update simulator to use new AuxGlobalPosition message

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,195 @@
  ****************************************************************************/
 
 #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 i = 0; i < MAX_AGP_IDS; i++) {
+		_sources[i] = new AgpSource(i, this);
+	}
+
+	touchActiveAgpParams();
+}
+
+AuxGlobalPosition::~AuxGlobalPosition()
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		delete _sources[i];
+		_sources[i] = nullptr;
+	}
+}
+
+void AuxGlobalPosition::touchActiveAgpParams()
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (getIdParam(i) != 0) {
+			getCtrlParam(i);
+			getModeParam(i);
+			getDelayParam(i);
+			getNoiseParam(i);
+			getGateParam(i);
+			_sources[i]->advertise();
+
+		} else {
+			break;
+		}
+	}
+}
+
 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 (_params_updated) {
+		touchActiveAgpParams();
+		_params_updated = false;
 	}
 
-#endif // MODULE_NAME
-
-	AuxGlobalPositionSample sample;
-
-	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;
-					}
-				}
-			}
-
-			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;
-			}
-
-			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 i = 0; i < MAX_AGP_IDS; i++) {
+		_sources[i]->checkAndBufferData(imu_delayed);
+		_sources[i]->update(ekf, imu_delayed);
 	}
 }
 
-bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
+float AuxGlobalPosition::test_ratio_filtered() const
 {
-	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));
+	float max_ratio = 0.f;
+
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_sources[i]->isFusing()) {
+			max_ratio = math::max(max_ratio, _sources[i]->getTestRatioFiltered());
+		}
+	}
+
+	return max_ratio;
 }
 
-#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+bool AuxGlobalPosition::anySourceFusing() const
+{
+	for (int i = 0; i < MAX_AGP_IDS; i++) {
+		if (_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;
+}
+
+float AuxGlobalPosition::getAgpParamFloat(const char *param_suffix, int instance) const
+{
+	char param_name[20] {};
+	snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
+
+	float value = NAN;
+
+	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::getCtrlParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].ctrl = getAgpParamInt32("CTRL", instance);
+	}
+
+	return _param_values[instance].ctrl;
+}
+
+int32_t AuxGlobalPosition::getModeParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].mode = getAgpParamInt32("MODE", instance);
+	}
+
+	return _param_values[instance].mode;
+}
+
+float AuxGlobalPosition::getDelayParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].delay = getAgpParamFloat("DELAY", instance);
+	}
+
+	return _param_values[instance].delay;
+}
+
+float AuxGlobalPosition::getNoiseParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].noise = getAgpParamFloat("NOISE", instance);
+	}
+
+	return _param_values[instance].noise;
+}
+
+float AuxGlobalPosition::getGateParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].gate = getAgpParamFloat("GATE", instance);
+	}
+
+	return _param_values[instance].gate;
+}
+
+int32_t AuxGlobalPosition::getIdParam(int instance)
+{
+	if (_params_updated) {
+		_param_values[instance].id = getAgpParamInt32("ID", instance);
+	}
+
+	return _param_values[instance].id;
+}
+
+void AuxGlobalPosition::setIdParam(int instance, int32_t sensor_id)
+{
+	setAgpParamInt32("ID", instance, sensor_id);
+	_param_values[instance].id = sensor_id;
+	_params_updated = true;
+}
+
+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 MAX_AGP_IDS;
+}
+
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION && MODULE_NAME

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

@@ -34,38 +34,24 @@
 #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 <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 +60,42 @@ public:
 		updateParams();
 	}
 
-	float test_ratio_filtered() const { return _test_ratio_filtered; }
+	float test_ratio_filtered() const;
+	bool anySourceFusing() const;
+	int32_t getCtrlParam(int instance);
+	int32_t getModeParam(int instance);
+	float getDelayParam(int instance);
+	float getNoiseParam(int instance);
+	float getGateParam(int instance);
+	int32_t getIdParam(int instance);
+	void setIdParam(int instance, int32_t sensor_id);
+	int mapSensorIdToSlot(int32_t sensor_id);
+	void params_updated() { _params_updated = true; }
 
 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);
-	}
+	friend class AgpSource;
 
-	bool isResetAllowed(const Ekf &ekf) const;
+	AgpSource *_sources[MAX_AGP_IDS];
+	bool _params_updated{true};
 
-	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 getAgpParamInt32(const char *param_suffix, int instance) const;
+	float getAgpParamFloat(const char *param_suffix, int instance) const;
+	bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value);
+
+	struct ParamValues {
+		int32_t ctrl{0};
+		int32_t mode{0};
+		float delay{0.f};
+		float noise{10.f};
+		float gate{3.f};
+		int32_t id{0};
 	};
 
-	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};
+	void touchActiveAgpParams();
+	ParamValues _param_values[MAX_AGP_IDS] {};
 
-	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,242 @@
+/****************************************************************************
+ *
+ *   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 instance_id, AuxGlobalPosition *manager)
+	: _agp_sub(ORB_ID(aux_global_position), instance_id)
+	, _manager(manager)
+	, _instance_id(instance_id) {}
+
+void AgpSource::checkAndBufferData(const estimator::imuSample &imu_delayed)
+{
+	if (_agp_sub.updated()) {
+
+		aux_global_position_s aux_global_position{};
+		_agp_sub.copy(&aux_global_position);
+
+		const uint8_t sensor_id = aux_global_position.id;
+		const int slot = _manager->mapSensorIdToSlot(sensor_id);
+
+		if (slot >= AuxGlobalPosition::MAX_AGP_IDS) {
+			// All parameter slots are full, cannot handle this sensor
+			return;
+		}
+
+		if (slot != _instance_id) {
+			// This sensor is mapped to a different instance
+			return;
+		}
+
+		const int64_t time_us = aux_global_position.timestamp_sample
+					- static_cast<int64_t>(getDelayParam() * 1000);
+
+		AuxGlobalPositionSample sample{};
+		sample.time_us = time_us;
+		sample.id = sensor_id;
+		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;
+
+		_buffer.push(sample);
+		_time_last_buffer_push = imu_delayed.time_us;
+	}
+}
+
+void AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
+{
+	AuxGlobalPositionSample sample;
+
+	if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
+
+		if (!(getCtrlParam() & static_cast<int32_t>(Ctrl::kHPos))) {
+			return;
+		}
+
+		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, getNoiseParam(), 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(getGateParam(), 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]));
+
+	} 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_WARN("Aux global position data stopped for instance %d", _instance_id);
+	}
+}
+
+bool AgpSource::isResetAllowed(const Ekf &ekf) const
+{
+	return ((static_cast<Mode>(getModeParam()) == Mode::kAuto)
+		&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
+	       || ((static_cast<Mode>(getModeParam()) == 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);
+}
+
+int32_t AgpSource::getCtrlParam() const
+{
+	return _manager->getCtrlParam(_instance_id);
+}
+
+int32_t AgpSource::getModeParam() const
+{
+	return _manager->getModeParam(_instance_id);
+}
+
+float AgpSource::getDelayParam() const
+{
+	return _manager->getDelayParam(_instance_id);
+}
+
+float AgpSource::getNoiseParam() const
+{
+	return _manager->getNoiseParam(_instance_id);
+}
+
+float AgpSource::getGateParam() const
+{
+	return _manager->getGateParam(_instance_id);
+}
+
+#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,120 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#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/ringbuffer/TimestampedRingBuffer.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_aid_source2d.h>
+#include <uORB/topics/aux_global_position.h>
+
+class Ekf;
+class AuxGlobalPosition;
+
+class AgpSource
+{
+public:
+	AgpSource(int instance_id, AuxGlobalPosition *manager);
+	~AgpSource() = default;
+
+	void checkAndBufferData(const estimator::imuSample &imu_delayed);
+	void update(Ekf &ekf, const estimator::imuSample &imu_delayed);
+	void advertise() { _aid_src_pub.advertise(); }
+
+	float getTestRatioFiltered() const { return _test_ratio_filtered; }
+	bool isFusing() const { return _state == State::kActive; }
+
+private:
+	struct AuxGlobalPositionSample {
+		uint64_t time_us{};     ///< timestamp of the measurement (uSec)
+		uint8_t id{};           ///< source identifier
+		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::Subscription _agp_sub;
+	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;
+	const int _instance_id;
+
+	bool isResetAllowed(const Ekf &ekf) const;
+	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const;
+
+	int32_t getCtrlParam() const;
+	int32_t getModeParam() const;
+	float getDelayParam() const;
+	float getNoiseParam() const;
+	float getGateParam() 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.params_updated();
 #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/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")};