diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 7516c86214..b1d16f871a 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/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()); } } diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 6585ae6c32..3ab5a152fe 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -94,7 +94,8 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_sens_gps_mask, - (ParamFloat) _param_sens_gps_tau + (ParamFloat) _param_sens_gps_tau, + (ParamInt) _param_sens_gps_prime ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index eca8ad877b..f91179bfac 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/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}; diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index a144deb3ba..1dc7a2c535 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/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); diff --git a/src/modules/sensors/vehicle_gps_position/params.c b/src/modules/sensors/vehicle_gps_position/params.c index 9ea7513277..0136ad52d8 100644 --- a/src/modules/sensors/vehicle_gps_position/params.c +++ b/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);