gps_blending: fix selection rapid switching

Once a timeout of the primary instance is detected, a fallback is only
allowed until the primary receiver is regained.

src/modules/sensors/vehicle_gps_position/gps_blending.cpp

@@ -76,7 +76,7 @@ void GpsBlending::update(uint64_t hrt_now_us)
 		// Only use a secondary instance if the fallback is allowed
 		if ((_primary_instance > -1)
 		    && (gps_select_index != _primary_instance)
-		    && !_fallback_allowed) {
+		    && _primary_instance_available) {
 			gps_select_index = _primary_instance;
 		}
 
@@ -87,6 +87,10 @@ void GpsBlending::update(uint64_t hrt_now_us)
 			_gps_updated[gps_select_index] = false;
 		}
 	}
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
+		_time_prev_us[i] = _gps_state[i].timestamp;
+	}
 }
 
 bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
@@ -121,6 +125,10 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
 		if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) {
 			_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
 
+			if (i == _primary_instance) {
+				_primary_instance_available = true;
+			}
+
 		} else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp > 0)) {
 			// Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt
 			_gps_state[i].timestamp = 0;
@@ -129,9 +137,8 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
 			_gps_state[i].vel_ned_valid = 0;
 
 			if (i == _primary_instance) {
-				// Allow using a secondary instance when the primary
-				// receiver has timed out
-				_fallback_allowed = true;
+				// Allow using a secondary instance when the primary receiver has timed out
+				_primary_instance_available = false;
 			}
 
 			continue;
@@ -523,8 +530,6 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
 			// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
 			alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
 					     0.0f, 1.0f);
-
-			_time_prev_us[i] = _gps_state[i].timestamp;
 		}
 	}
 

src/modules/sensors/vehicle_gps_position/gps_blending.hpp

@@ -126,7 +126,7 @@ private:
 	int _selected_gps{0};
 	int _np_gps_suitable_for_blending{0};
 	int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id
-	bool _fallback_allowed{false};
+	bool _primary_instance_available{false};
 
 	bool _is_new_output_data_available{false};
 

src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp

@@ -243,8 +243,7 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)
 
 	// BUT WHEN: the data of the primary receiver is avaialbe
 	sensor_gps_s gps_data0 = getDefaultGpsData();
-	gps_blending.setGpsData(gps_data0, 0);
-	gps_blending.update(_time_now_us);
+	runSeconds(1.f, gps_blending, gps_data0, gps_data1);
 
 	// THEN: the primary instance is selected and the data
 	// is available
@@ -274,6 +273,15 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)
 	// THEN: the primary receiver should be used again
 	EXPECT_EQ(gps_blending.getSelectedGps(), 0);
 	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
+
+	// BUT IF: the secondary receiver has better metrics than the primary one
+	gps_data1.satellites_used = gps_data0.satellites_used + 2;
+
+	runSeconds(1.f, gps_blending, gps_data0, gps_data1);
+
+	// THEN: the selector shouldn't switch again as the primary one is available
+	EXPECT_EQ(gps_blending.getSelectedGps(), 0);
+	EXPECT_TRUE(gps_blending.isNewOutputDataAvailable());
 }
 
 TEST_F(GpsBlendingTest, dualReceiverUTCTime)