gps blend: add parameter to select the primary instance

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -97,6 +97,7 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
 		_gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC);
 		_gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC);
 		_gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get());
+		_gps_blending.setPrimaryInstance(_param_sens_gps_prime.get());
 	}
 }
 

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -94,7 +94,8 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
-		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
+		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,
+		(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime
 	)
 };
 }; // namespace sensors

src/modules/sensors/vehicle_gps_position/gps_blending.hpp

@@ -121,7 +121,7 @@ private:
 	bool _gps_updated[GPS_MAX_RECEIVERS] {};
 	int _selected_gps{0};
 	int _np_gps_suitable_for_blending{0};
-	int _primary_instance{-1};
+	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 _is_new_output_data_available{false};

src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp

@@ -136,6 +136,7 @@ TEST_F(GpsBlendingTest, singleReceiver)
 {
 	GpsBlending<2> gps_blending;
 
+	gps_blending.setPrimaryInstance(-1);
 	sensor_gps_s gps_data = getDefaultGpsData();
 
 	gps_blending.setGpsData(gps_data, 1);
@@ -163,6 +164,8 @@ TEST_F(GpsBlendingTest, dualReceiverNoBlending)
 {
 	GpsBlending<2> gps_blending;
 
+	// GIVEN: two receivers with the same prioity
+	gps_blending.setPrimaryInstance(-1);
 	sensor_gps_s gps_data0 = getDefaultGpsData();
 	sensor_gps_s gps_data1 = getDefaultGpsData();
 
@@ -214,12 +217,15 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)
 {
 	GpsBlending<2> gps_blending;
 
-	sensor_gps_s gps_data1 = getDefaultGpsData();
-
+	// GIVEN: a dual GPS setup with the first instance (0)
+	// set as primary
 	gps_blending.setPrimaryInstance(0);
-	gps_blending.setBlendingUseSpeedAccuracy(0);
-	gps_blending.setBlendingUseHPosAccuracy(0);
-	gps_blending.setBlendingUseVPosAccuracy(0);
+	gps_blending.setBlendingUseSpeedAccuracy(false);
+	gps_blending.setBlendingUseHPosAccuracy(false);
+	gps_blending.setBlendingUseVPosAccuracy(false);
+
+	// WHEN: only the secondary receiver is available
+	sensor_gps_s gps_data1 = getDefaultGpsData();
 
 	const float duration_s = 10.f;
 	runSeconds(duration_s, gps_blending, gps_data1, 1);

src/modules/sensors/vehicle_gps_position/params.c

@@ -61,3 +61,22 @@ PARAM_DEFINE_INT32(SENS_GPS_MASK, 0);
  * @decimal 1
  */
 PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
+
+/**
+ * Multi GPS primary instance
+ *
+ * When no blending is active, this defines the preferred GPS receiver instance.
+ * The GPS selection logic waits until the primary receiver is available to
+ * send data to the EKF even if a secondary instance is already available.
+ * The secondary instance is then only used if the primary one times out.
+ *
+ * To have an equal priority of all the instances, set this parameter to -1 and
+ * the best receiver will be used.
+ *
+ * This parameter has no effect if blending is active.
+ *
+ * @group Sensors
+ * @min -1
+ * @max 1
+ */
+PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);