ekf-agp: do not reset to AGP if GNSS fusion is active

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

@@ -104,9 +104,20 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 				_state = State::starting;
 
 				if (ekf.global_origin_valid()) {
-					ekf.enableControlStatusAuxGpos();
-					_reset_counters.lat_lon = sample.lat_lon_reset_counter;
-					_state = State::active;
+					const bool fused = ekf.fuseHorizontalPosition(aid_src);
+					bool reset = false;
+
+					if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) {
+						ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv));
+						ekf.resetAidSourceStatusZeroInnovation(aid_src);
+						reset = true;
+					}
+
+					if (fused || reset) {
+						ekf.enableControlStatusAuxGpos();
+						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+						_state = State::active;
+					}
 
 				} else {
 					// Try to initialize using measurement
@@ -128,12 +139,18 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
 				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
+					if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) {
 
-					ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
+						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
 
-					ekf.resetAidSourceStatusZeroInnovation(aid_src);
+						ekf.resetAidSourceStatusZeroInnovation(aid_src);
 
-					_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
+
+					} else {
+						ekf.disableControlStatusAuxGpos();
+						_state = State::stopped;
+					}
 				}
 
 			} else {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -600,12 +600,32 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f
 }
 
 int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const
+{
+	return getNumberOfActiveHorizontalPositionAidingSources() + getNumberOfActiveHorizontalVelocityAidingSources();
+}
+
+bool EstimatorInterface::isOnlyActiveSourceOfHorizontalPositionAiding(const bool aiding_flag) const
+{
+	return aiding_flag && !isOtherSourceOfHorizontalPositionAidingThan(aiding_flag);
+}
+
+bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool aiding_flag) const
+{
+	const int nb_sources = getNumberOfActiveHorizontalPositionAidingSources();
+	return aiding_flag ? nb_sources > 1 : nb_sources > 0;
+}
+
+int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const
 {
 	return int(_control_status.flags.gps)
-	       + int(_control_status.flags.opt_flow)
 	       + int(_control_status.flags.ev_pos)
+	       + int(_control_status.flags.aux_gpos);
+}
+
+int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const
+{
+	return int(_control_status.flags.opt_flow)
 	       + int(_control_status.flags.ev_vel)
-	       + int(_control_status.flags.aux_gpos)
 	       // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning
 	       // and so is treated as a single aiding source.
 	       + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta);

src/modules/ekf2/EKF/estimator_interface.h

@@ -207,8 +207,8 @@ public:
 	// set air density used by the multi-rotor specific drag force fusion
 	void set_air_density(float air_density) { _air_density = air_density; }
 
-	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
+	bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const;
 
 	/*
 	 * Check if there are any other active source of horizontal aiding
@@ -221,6 +221,7 @@ public:
 	 * @return true if an other source than aiding_flag is active
 	 */
 	bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const;
+	bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const;
 
 	// Return true if at least one source of horizontal aiding is active
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
@@ -228,6 +229,8 @@ public:
 	bool isVerticalAidingActive() const;
 
 	int getNumberOfActiveHorizontalAidingSources() const;
+	int getNumberOfActiveHorizontalPositionAidingSources() const;
+	int getNumberOfActiveHorizontalVelocityAidingSources() const;
 
 	bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const;
 	bool isVerticalPositionAidingActive() const;