Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 15:08:16 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

ekf2: rng only reset vz as a last resort

Browse Source
This commit is contained in:
Daniel Agar
2024-07-10 13:25:14 -04:00
parent 80ee622f77
commit 8e5f28f834
2 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+4 -2
View File
@@ -180,8 +180,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_information_events.flags.reset_hgt_to_rng = true; _information_events.flags.reset_hgt_to_rng = true;
resetVerticalPositionTo(-(aid_src.observation - _state.terrain)); resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
// reset vertical velocity // reset vertical velocity if no valid sources available
resetVerticalVelocityToZero(); if (!isVerticalVelocityAidingActive()) {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = imu_sample.time_us; aid_src.time_last_fuse = imu_sample.time_us;
src/modules/ekf2/test/test_EKF_terrain.cpp
+2 -2
View File
@@ -207,9 +207,9 @@ TEST_F(EkfTerrainTest, testHeightReset)
const float new_baro_height = _sensor_simulator._baro.getData() + 50.f; const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
_sensor_simulator._baro.setData(new_baro_height); _sensor_simulator._baro.setData(new_baro_height);
_sensor_simulator.stopGps(); // prevent from switching to GNSS height _sensor_simulator.stopGps(); // prevent from switching to GNSS height
_sensor_simulator.runSeconds(6); _sensor_simulator.runSeconds(10);
// THEN: a height reset occured and the estimated distance to the ground remains constant // THEN: a height reset occurred and the estimated distance to the ground remains constant
reset_logging_checker.capturePostResetState(); reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f); EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f);