From 094048ed041624bef403dbfe05862a7d59b65bac Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Dec 2023 10:49:33 +0100 Subject: [PATCH] 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. --- .../vehicle_gps_position/gps_blending.cpp | 17 +++++++++++------ .../vehicle_gps_position/gps_blending.hpp | 2 +- .../vehicle_gps_position/gps_blending_test.cpp | 12 ++++++++++-- 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 6582c37a24..c774e26eac 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/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; } } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index 6d33a643cd..fa646578e6 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/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}; 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 3c2fe326f0..53efb29a7e 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/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)