Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

ekf2: fix timeout after gps failure (#23346)

Browse Source
This commit is contained in:
Marco Hauswirth
2024-07-02 16:38:49 +02:00
committed by GitHub
parent 0742d356f5
commit 3880073716
5 changed files with 712 additions and 711 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
+2 -1
View File
@@ -63,7 +63,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
if (_gps_data_ready) { if (_gps_data_ready) {
const gnssSample &gnss_sample = _gps_sample_delayed; const gnssSample &gnss_sample = _gps_sample_delayed;
if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) { if (runGnssChecks(gnss_sample)
&& isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
// First time checks are passing, latching. // First time checks are passing, latching.
_gps_checks_passed = true; _gps_checks_passed = true;
src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv
+374 -374
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf2/test/change_indication/iris_gps.csv
+333 -333
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf2/test/test_EKF_gps.cpp
+1 -1
View File
@@ -136,7 +136,7 @@ TEST_F(EkfGpsTest, resetToGpsVelocity)
_ekf->set_in_air_status(true); _ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false); _ekf->set_vehicle_at_rest(false);
_sensor_simulator.runSeconds(5.2); // required to pass the checks _sensor_simulator.runSeconds(1.2); // required to pass the checks
_sensor_simulator.runMicroseconds(dt_us); _sensor_simulator.runMicroseconds(dt_us);
// THEN: a reset to GPS velocity should be done // THEN: a reset to GPS velocity should be done
src/modules/ekf2/test/test_EKF_mag.cpp
+2 -2
View File
@@ -68,6 +68,7 @@ public:
TEST_F(EkfMagTest, fusionStartWithReset) TEST_F(EkfMagTest, fusionStartWithReset)
{ {
_ekf->set_min_required_gps_health_time(5e6);
// GIVEN: some meaningful mag data // GIVEN: some meaningful mag data
const float mag_heading = M_PI_F / 3.f; const float mag_heading = M_PI_F / 3.f;
const float incl = 63.1f; const float incl = 63.1f;
@@ -84,11 +85,10 @@ TEST_F(EkfMagTest, fusionStartWithReset)
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
// AND WHEN: GNSS fusion starts // AND WHEN: GNSS fusion starts
_ekf_wrapper.enableGpsFusion(); _ekf_wrapper.enableGpsFusion();
_sensor_simulator.startGps(); _sensor_simulator.startGps();
_sensor_simulator.runSeconds(11); _sensor_simulator.runSeconds(6);
// THEN: the earth mag field is reset to the WMM // THEN: the earth mag field is reset to the WMM
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);