From b4051b62a31c015655de7f437ef745b2c9ad5cca Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 29 Jan 2021 15:36:27 +0100 Subject: [PATCH] gps blend: add failover logic and unit test also remove dependency to hrt library --- .../VehicleGPSPosition.cpp | 2 +- .../vehicle_gps_position/gps_blending.hpp | 45 +++-- .../gps_blending_test.cpp | 178 ++++++++++++++---- 3 files changed, 176 insertions(+), 49 deletions(-) diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 6a98df244e..67cd48da5f 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -142,7 +142,7 @@ void VehicleGPSPosition::Run() } if (any_gps_updated) { - _gps_blending.update(); + _gps_blending.update(hrt_absolute_time()); if (_gps_blending.isNewOutputDataAvailable()) { Publish(_gps_blending.getOutputGpsData(), _gps_blending.getSelectedGps()); diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index 607957f0c0..eca8ad877b 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -72,10 +72,12 @@ public: void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; } void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; } void setBlendingTimeConstant(float tau) { _blending_time_constant = tau; } + void setPrimaryInstance(int primary) { _primary_instance = primary; } - void update(); + void update(uint64_t hrt_now_us); bool isNewOutputDataAvailable() const { return _is_new_output_data_available; } + int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; } const sensor_gps_s &getOutputGpsData() const { if (_selected_gps < GPS_MAX_RECEIVERS) { @@ -94,7 +96,7 @@ private: * have significant position differences, variation in receiver estimated accuracy will cause undesirable * variation in the position solution. */ - bool blend_gps_data(); + bool blend_gps_data(uint64_t hrt_now_us); /* * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated @@ -118,6 +120,9 @@ private: sensor_gps_s _gps_blended_state {}; bool _gps_updated[GPS_MAX_RECEIVERS] {}; int _selected_gps{0}; + int _np_gps_suitable_for_blending{0}; + int _primary_instance{-1}; + bool _fallback_allowed{false}; bool _is_new_output_data_available{false}; @@ -138,12 +143,12 @@ private: }; template -void GpsBlending::update() +void GpsBlending::update(uint64_t hrt_now_us) { _is_new_output_data_available = false; // blend multiple receivers if available - if (!blend_gps_data()) { + if (!blend_gps_data(hrt_now_us)) { // Only use selected receiver data if it has been updated uint8_t gps_select_index = 0; @@ -173,14 +178,24 @@ void GpsBlending::update() _hgt_offset_mm[i] = 0.0f; } - // re-publish current best GPS + // Only use a secondary instance if the fallback is allowed + if ((_primary_instance > -1) + && (gps_select_index != _primary_instance) + && !_fallback_allowed) { + gps_select_index = _primary_instance; + } + _selected_gps = gps_select_index; _is_new_output_data_available = _gps_updated[gps_select_index]; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + _gps_updated[gps_select_index] = false; + } } } template -bool GpsBlending::blend_gps_data() +bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) { /* * If both receivers have the same update rate, use the oldest non-zero time. @@ -193,23 +208,29 @@ bool GpsBlending::blend_gps_data() // Find the largest and smallest time step. float dt_max = 0.0f; float dt_min = GPS_TIMEOUT_S; - uint8_t gps_count = 0; // Count of receivers which have an active >=2D fix - const hrt_abstime hrt_now = hrt_absolute_time(); + _np_gps_suitable_for_blending = 0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { const float raw_dt = 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]); - const float present_dt = 1e-6f * (float)(hrt_now - _gps_state[i].timestamp); + const float present_dt = 1e-6f * (float)(hrt_now_us - _gps_state[i].timestamp); if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; - } else if (present_dt >= GPS_TIMEOUT_S) { + } 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; _gps_state[i].fix_type = 0; _gps_state[i].satellites_used = 0; _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; + } + continue; } @@ -227,7 +248,7 @@ bool GpsBlending::blend_gps_data() dt_min = _gps_dt[i]; } - gps_count++; + _np_gps_suitable_for_blending++; } // Find the receiver that is last be updated @@ -246,7 +267,7 @@ bool GpsBlending::blend_gps_data() } } - if (gps_count < 2) { + if (_np_gps_suitable_for_blending < 2) { // Less than 2 receivers left, so fall out of blending return 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 31bc6ef191..a144deb3ba 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -49,14 +49,16 @@ class GpsBlendingTest : public ::testing::Test { public: sensor_gps_s getDefaultGpsData(); + void runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data, int instance); + void runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data0, sensor_gps_s &gps_data1); - GpsBlending<2> _gps_blending; + uint64_t _time_now_us{1000000}; }; sensor_gps_s GpsBlendingTest::getDefaultGpsData() { sensor_gps_s gps_data{}; - gps_data.timestamp = hrt_absolute_time() - 10e3; // microseconds + gps_data.timestamp = _time_now_us - 10e3; gps_data.time_utc_usec = 0; gps_data.lat = 47e7; gps_data.lon = 9e7; @@ -85,76 +87,180 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData() return gps_data; } +void GpsBlendingTest::runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data, int instance) +{ + const float dt = 0.1; + const uint64_t dt_us = static_cast(dt * 1e6f); + + for (int k = 0; k < static_cast(duration_s / dt); k++) { + gps_blending.setGpsData(gps_data, instance); + gps_blending.update(_time_now_us); + + _time_now_us += dt_us; + gps_data.timestamp += dt_us; + } +} + +void GpsBlendingTest::runSeconds(float duration_s, GpsBlending<2> &gps_blending, sensor_gps_s &gps_data0, + sensor_gps_s &gps_data1) +{ + const float dt = 0.1; + const uint64_t dt_us = static_cast(dt * 1e6f); + + for (int k = 0; k < static_cast(duration_s / dt); k++) { + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + + gps_blending.update(_time_now_us); + + _time_now_us += dt_us; + gps_data0.timestamp += dt_us; + gps_data1.timestamp += dt_us; + } +} + TEST_F(GpsBlendingTest, noData) { - EXPECT_EQ(_gps_blending.getSelectedGps(), 0); - EXPECT_FALSE(_gps_blending.isNewOutputDataAvailable()); + GpsBlending<2> gps_blending; - _gps_blending.update(); + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); - EXPECT_EQ(_gps_blending.getSelectedGps(), 0); - EXPECT_FALSE(_gps_blending.isNewOutputDataAvailable()); + gps_blending.update(_time_now_us); + + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); } TEST_F(GpsBlendingTest, singleReceiver) { + GpsBlending<2> gps_blending; + sensor_gps_s gps_data = getDefaultGpsData(); - _gps_blending.setGpsData(gps_data, 1); - _gps_blending.update(); + gps_blending.setGpsData(gps_data, 1); + gps_blending.update(_time_now_us); - gps_data.timestamp = hrt_absolute_time() - 10e3; - _gps_blending.setGpsData(gps_data, 1); - _gps_blending.update(); + _time_now_us += 200e3; + gps_data.timestamp = _time_now_us - 10e3; + gps_blending.setGpsData(gps_data, 1); + gps_blending.update(_time_now_us); - EXPECT_EQ(_gps_blending.getSelectedGps(), 1); - EXPECT_EQ(_gps_blending.getNumberOfGpsSuitableForBlending(), 1); - EXPECT_TRUE(_gps_blending.isNewOutputDataAvailable()); + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // BUT IF: a second update is called without data + gps_blending.update(_time_now_us); + + // THEN: no new data should be available + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); } TEST_F(GpsBlendingTest, dualReceiverNoBlending) { + GpsBlending<2> gps_blending; + 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(); + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.update(_time_now_us); // 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()); + 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(); + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.update(_time_now_us); // 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()); + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); } TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) { + GpsBlending<2> gps_blending; + sensor_gps_s gps_data0 = getDefaultGpsData(); sensor_gps_s gps_data1 = getDefaultGpsData(); - _gps_blending.setBlendingUseHPosAccuracy(true); + 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(); + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.update(_time_now_us); // 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 + 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 +} + +TEST_F(GpsBlendingTest, dualReceiverFailover) +{ + GpsBlending<2> gps_blending; + + sensor_gps_s gps_data1 = getDefaultGpsData(); + + gps_blending.setPrimaryInstance(0); + gps_blending.setBlendingUseSpeedAccuracy(0); + gps_blending.setBlendingUseHPosAccuracy(0); + gps_blending.setBlendingUseVPosAccuracy(0); + + 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); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); + EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); + + // 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); + + // THEN: the primary instance is selected and the data + // is available + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + runSeconds(duration_s, gps_blending, gps_data0, gps_data1); + + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // BUT WHEN: the primary receiver isn't available anymore + runSeconds(duration_s, gps_blending, gps_data1, 1); + + // THEN: the data of the secondary receiver can be used + EXPECT_EQ(gps_blending.getSelectedGps(), 1); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); + + // AND IF: the primary receiver is available again and has + // better metrics than the secondary one + gps_data0.timestamp = gps_data1.timestamp; + gps_data0.satellites_used = gps_data1.satellites_used + 2; + + runSeconds(1.f, gps_blending, gps_data0, gps_data1); + + // THEN: the primary receiver should be used again + EXPECT_EQ(gps_blending.getSelectedGps(), 0); + EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); }