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 353e78e4c0..31bc6ef191 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -48,9 +48,43 @@ using matrix::Vector3f; class GpsBlendingTest : public ::testing::Test { public: + sensor_gps_s getDefaultGpsData(); + GpsBlending<2> _gps_blending; }; +sensor_gps_s GpsBlendingTest::getDefaultGpsData() +{ + sensor_gps_s gps_data{}; + gps_data.timestamp = hrt_absolute_time() - 10e3; // microseconds + gps_data.time_utc_usec = 0; + gps_data.lat = 47e7; + gps_data.lon = 9e7; + gps_data.alt = 800e3; + gps_data.alt_ellipsoid = 800e3; + gps_data.s_variance_m_s = 0.2f; + gps_data.c_variance_rad = 0.5f; + gps_data.eph = 0.7f; + gps_data.epv = 1.2f; + gps_data.hdop = 1.f; + gps_data.vdop = 1.f; + gps_data.noise_per_ms = 20; + gps_data.jamming_indicator = 40; + gps_data.vel_m_s = 1.f; + gps_data.vel_n_m_s = 1.f; + gps_data.vel_e_m_s = 1.f; + gps_data.vel_d_m_s = 1.f; + gps_data.cog_rad = 0.f; + gps_data.timestamp_time_relative = 0; + gps_data.heading = NAN; + gps_data.heading_offset = 0.f; + gps_data.fix_type = 4; + gps_data.vel_ned_valid = true; + gps_data.satellites_used = 8; + + return gps_data; +} + TEST_F(GpsBlendingTest, noData) { EXPECT_EQ(_gps_blending.getSelectedGps(), 0); @@ -61,3 +95,66 @@ TEST_F(GpsBlendingTest, noData) EXPECT_EQ(_gps_blending.getSelectedGps(), 0); EXPECT_FALSE(_gps_blending.isNewOutputDataAvailable()); } + +TEST_F(GpsBlendingTest, singleReceiver) +{ + sensor_gps_s gps_data = getDefaultGpsData(); + + _gps_blending.setGpsData(gps_data, 1); + _gps_blending.update(); + + gps_data.timestamp = hrt_absolute_time() - 10e3; + _gps_blending.setGpsData(gps_data, 1); + _gps_blending.update(); + + EXPECT_EQ(_gps_blending.getSelectedGps(), 1); + EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable()); +} + +TEST_F(GpsBlendingTest, dualReceiverNoBlending) +{ + sensor_gps_s gps_data0 = getDefaultGpsData(); + sensor_gps_s gps_data1 = getDefaultGpsData(); + + gps_data1.satellites_used = gps_data0.satellites_used + 2; // gps1 has more satellites than gps0 + _gps_blending.setGpsData(gps_data0, 0); + _gps_blending.setGpsData(gps_data1, 1); + _gps_blending.update(); + + // THEN: gps1 should be selected because it has more satellites + EXPECT_EQ(_gps_blending.getSelectedGps(), 1); + EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable()); + + gps_data1.satellites_used = gps_data0.satellites_used - 2; // gps1 has less satellites than gps0 + _gps_blending.setGpsData(gps_data0, 0); + _gps_blending.setGpsData(gps_data1, 1); + _gps_blending.update(); + + // THEN: gps0 should be selected because it has more satellites + EXPECT_EQ(_gps_blending.getSelectedGps(), 0); + EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable()); +} + +TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) +{ + sensor_gps_s gps_data0 = getDefaultGpsData(); + sensor_gps_s gps_data1 = getDefaultGpsData(); + + _gps_blending.setBlendingUseHPosAccuracy(true); + + gps_data1.eph = gps_data0.eph / 2.f; + _gps_blending.setGpsData(gps_data0, 0); + _gps_blending.setGpsData(gps_data1, 1); + _gps_blending.update(); + + // THEN: the blended instance should be selected (2) + // and the eph should be adjusted + EXPECT_EQ(_gps_blending.getSelectedGps(), 2); + EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable()); + EXPECT_LT(_gps_blending.getOutputGpsData().eph, gps_data0.eph); + EXPECT_FLOAT_EQ(_gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than +}