From b8c81f6281fb5ca85faa0bc5881393c70bac7a43 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Dec 2023 11:06:30 +0100 Subject: [PATCH] gps_blending: fallback to secondary if primary has no fix --- .../vehicle_gps_position/gps_blending.cpp | 3 ++- .../vehicle_gps_position/gps_blending_test.cpp | 16 ++++++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index c774e26eac..73b5fe1cee 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -76,7 +76,8 @@ 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) - && _primary_instance_available) { + && _primary_instance_available + && (_gps_state[_primary_instance].fix_type >= 3)) { gps_select_index = _primary_instance; } 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 53efb29a7e..50352162c0 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -235,11 +235,10 @@ TEST_F(GpsBlendingTest, dualReceiverFailover) const float duration_s = 10.f; runSeconds(duration_s, gps_blending, gps_data1, 1); - // THEN: the primary instance should be selected even if - // not available. No data is then available - EXPECT_EQ(gps_blending.getSelectedGps(), 0); + // THEN: the secondary instance as the primary one is not available + EXPECT_EQ(gps_blending.getSelectedGps(), 1); EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); - EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); // BUT WHEN: the data of the primary receiver is avaialbe sensor_gps_s gps_data0 = getDefaultGpsData(); @@ -282,6 +281,15 @@ TEST_F(GpsBlendingTest, dualReceiverFailover) // THEN: the selector shouldn't switch again as the primary one is available EXPECT_EQ(gps_blending.getSelectedGps(), 0); EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // BUT IF: the primary receiver looses its fix + gps_data0.fix_type = 1; + + runSeconds(1.f, gps_blending, gps_data0, gps_data1); + + // THEN: the selector should switch as the primary one is unable to provide correct data + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); } TEST_F(GpsBlendingTest, dualReceiverUTCTime)