diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp index 441c9758062..fddf54de207 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -187,16 +187,6 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss) runOnGroundGnssChecks(gnss); - // force horizontal speed failure if above the limit - if (gnss.vel.xy().longerThan(_params.ekf2_vel_lim)) { - setFail(estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR, true); - } - - // force vertical speed failure if above the limit - if (fabsf(gnss.vel(2)) > _params.ekf2_vel_lim) { - setFail(estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR, true); - } - return enabledChecksPass(UINT16_MAX); } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp index 87a3702ba3a..91819f77382 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp @@ -45,9 +45,9 @@ class GnssChecks final { public: GnssChecks(int32_t &check_mask, int32_t &ekf2_req_nsats, float &ekf2_req_pdop, float &ekf2_req_eph, float &ekf2_req_epv, - float &ekf2_req_sacc, float &ekf2_req_hdrift, float &ekf2_req_vdrift, int32_t &ekf2_req_fix, float &ekf2_vel_lim, + float &ekf2_req_sacc, float &ekf2_req_hdrift, float &ekf2_req_vdrift, int32_t &ekf2_req_fix, uint32_t &min_health_time_us, filter_control_status_u &control_status): - _params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_req_fix, ekf2_vel_lim, min_health_time_us}, + _params{check_mask, ekf2_req_nsats, ekf2_req_pdop, ekf2_req_eph, ekf2_req_epv, ekf2_req_sacc, ekf2_req_hdrift, ekf2_req_vdrift, ekf2_req_fix, min_health_time_us}, _control_status(control_status) {}; @@ -179,7 +179,6 @@ private: const float &ekf2_req_hdrift; const float &ekf2_req_vdrift; const int32_t &ekf2_req_fix; - const float &ekf2_vel_lim; const uint32_t &min_health_time_us; }; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 6eb84587ccb..7040d439b32 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -74,25 +74,37 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) const gnssSample &gnss_sample = _gps_sample_delayed; const bool initial_checks_passed_prev = _gnss_checks.initialChecksPassed(); + const bool checks_passed = _gnss_checks.run(gnss_sample, _time_delayed_us); - if (_gnss_checks.run(gnss_sample, _time_delayed_us)) { + if (checks_passed && _gnss_checks.initialChecksPassed() && !initial_checks_passed_prev) { + // First time checks are passing, latching. + _information_events.flags.gps_checks_passed = true; + } + + // Each axis of the velocity state is constrained to EKF2_VEL_LIM, so a sample beyond it cannot be fused + const bool vel_within_limit = gnss_sample.vel.isAllFinite() + && (gnss_sample.vel.abs().max() <= _params.ekf2_vel_lim); + + if (checks_passed && vel_within_limit) { _time_last_gnss_checks_pass_us = _time_delayed_us; - if (_gnss_checks.initialChecksPassed() && !initial_checks_passed_prev) { - // First time checks are passing, latching. - _information_events.flags.gps_checks_passed = true; - } - } else { // Skip this sample _gps_data_ready = false; - const bool using_gnss = _control_status.flags.gnss_vel || _control_status.flags.gnss_pos; + const bool using_gnss = _control_status.flags.gnss_vel || _control_status.flags.gnss_pos + || _control_status.flags.gps_hgt; const bool gnss_checks_pass_timeout = isTimedOut(_time_last_gnss_checks_pass_us, _params.reset_timeout_max); if (using_gnss && gnss_checks_pass_timeout) { stopGnssFusion(); - ECL_WARN("GNSS quality poor - stopping use"); + + if (checks_passed) { + ECL_WARN("GNSS velocity above limit - stopping use"); + + } else { + ECL_WARN("GNSS quality poor - stopping use"); + } } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 06bbd637e4a..357078c93d1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -615,7 +615,7 @@ private: // height sensor status bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent - uint64_t _time_last_gnss_checks_pass_us{0}; ///< last delayed-horizon time a GNSS sample passed the checks (us) + uint64_t _time_last_gnss_checks_pass_us{0}; ///< last delayed-horizon time a GNSS sample passed the checks and the velocity limit (us) uint64_t _time_last_gnss_fusion_stop_us{0}; ///< when GNSS velocity and position fusion were last both stopped HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5ce93bf775d..0758321aa32 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -421,7 +421,6 @@ protected: _params.ekf2_req_hdrift, _params.ekf2_req_vdrift, _params.ekf2_req_fix, - _params.ekf2_vel_lim, _min_gps_health_time_us, _control_status}; diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 03b06e54ada..c3d1180b30a 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -180,6 +180,7 @@ parameters: EKF2_VEL_LIM: description: short: Velocity limit + long: Each axis of the velocity state is constrained to this magnitude. GNSS and external vision velocity samples beyond it are not fused. type: float default: 100 max: 299792458 diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 06f32e647e3..756f7f66649 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -259,6 +259,28 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback) EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); } +TEST_F(EkfGpsTest, gnssHeightOnlyStopsWhenChecksFail) +{ + // GIVEN: GNSS height fusion is active in flight while position and velocity fusion are disabled + _ekf_wrapper.enableGpsHeightFusion(); + _sensor_simulator.runSeconds(1); + ASSERT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + + _ekf_wrapper.disableGpsFusion(); + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _sensor_simulator.runSeconds(2); + ASSERT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); + ASSERT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + + // WHEN: the receiver fails the in-flight checks for longer than the fusion timeout + _sensor_simulator._gps.setFixType(2); + _sensor_simulator.runSeconds(8); + + // THEN: height fusion stops instead of staying latched on a receiver whose samples are skipped + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); +} + TEST_F(EkfGpsTest, altitudeDrift) { // GIVEN: a drifting GNSS altitude @@ -361,3 +383,93 @@ TEST_F(EkfGpsTest, gnssIntermittentSaccFailureDisablesFusion) // and reset_timeout_max was exceeded since the last real pass. EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); } + +TEST_F(EkfGpsTest, velocityAboveLimitIsNotFused) +{ + // GIVEN: an airborne EKF that fuses GPS with the optional quality checks disabled + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->getParamHandle()->ekf2_gps_check = 0; + + // WHEN: the receiver reports a velocity above EKF2_VEL_LIM (100 m/s by default) + _sensor_simulator._gps.setVelocity(Vector3f(150.f, 0.f, 0.f)); + _sensor_simulator.runSeconds(1); + + // THEN: the samples are skipped, nothing is fused while the fusion is still intended + const uint64_t time_last_vel_fuse = _ekf->aid_src_gnss_vel().time_last_fuse; + const uint64_t time_last_pos_fuse = _ekf->aid_src_gnss_pos().time_last_fuse; + _sensor_simulator.runSeconds(1); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + EXPECT_EQ(_ekf->aid_src_gnss_vel().time_last_fuse, time_last_vel_fuse); + EXPECT_EQ(_ekf->aid_src_gnss_pos().time_last_fuse, time_last_pos_fuse); + + // AND: the GNSS fusion stops once samples have been skipped for longer than the timeout + _sensor_simulator.runSeconds(6); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); + + // AND: valid samples restart the fusion + _sensor_simulator._gps.setVelocity(Vector3f{}); + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); +} + +TEST_F(EkfGpsTest, invalidVelocityIsSkipped) +{ + // GIVEN: an airborne EKF that fuses GPS with the optional quality checks disabled + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->getParamHandle()->ekf2_gps_check = 0; + const float velocity_limit = _ekf->getParamHandle()->ekf2_vel_lim; + const Vector3f invalid_velocities[] { + {velocity_limit + 1.f, 0.f, 0.f}, + {0.f, -velocity_limit - 1.f, 0.f}, + {0.f, 0.f, velocity_limit + 1.f}, + {0.f, 0.f, -velocity_limit - 1.f}, + {NAN, 0.f, 0.f}, + {0.f, INFINITY, 0.f}, + {0.f, 0.f, -INFINITY}, + }; + + for (const Vector3f &velocity : invalid_velocities) { + ASSERT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + // WHEN: the receiver reports an over-limit or non-finite velocity for longer than the fusion timeout + _sensor_simulator._gps.setVelocity(velocity); + _sensor_simulator.runSeconds(1); // the valid samples still in the buffer get fused + const uint64_t time_last_vel_fuse = _ekf->aid_src_gnss_vel().time_last_fuse; + const uint64_t time_last_pos_fuse = _ekf->aid_src_gnss_pos().time_last_fuse; + _sensor_simulator.runSeconds(7); + + // THEN: nothing was fused and the fusion stopped instead of resetting to the sample + EXPECT_EQ(_ekf->aid_src_gnss_vel().time_last_fuse, time_last_vel_fuse); + EXPECT_EQ(_ekf->aid_src_gnss_pos().time_last_fuse, time_last_pos_fuse); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); + + // AND: valid samples restart it + _sensor_simulator._gps.setVelocity(Vector3f{}); + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + } +} + +TEST_F(EkfGpsTest, velocityAtLimitIsNotSkipped) +{ + // GIVEN: an airborne EKF that fuses GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + const float velocity_limit = _ekf->getParamHandle()->ekf2_vel_lim; + + // WHEN: every component of the reported velocity sits exactly at EKF2_VEL_LIM for longer than the fusion timeout + _sensor_simulator._gps.setVelocity(Vector3f(velocity_limit, -velocity_limit, velocity_limit)); + _sensor_simulator.runSeconds(1); + const uint64_t time_last_vel_fuse = _ekf->aid_src_gnss_vel().time_last_fuse; + _sensor_simulator.runSeconds(7); + + // THEN: the velocity state can hold it, so the samples reach the fusion: the innovation failure resets to the + // sample instead of the skip timeout stopping the fusion + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + EXPECT_GT(_ekf->aid_src_gnss_vel().time_last_fuse, time_last_vel_fuse); +}