dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gps_blending: fallback to secondary if primary has no fix

This commit is contained in:
bresch 2023-12-20 11:06:30 +01:00 committed by Daniel Agar
parent 094048ed04
commit b8c81f6281
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
View File

@ -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;
}

16
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
View File

@ -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)