fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-rng-kin: allow invalidating at any vertical speed

Browse Source 
The issue is that when the HAGL is low, the drone will usually
decelerate and then the check would run anymore. If the low HAGL
estimate is due to bad sensor readings (e.g.: reflections), it will be
stuck in that state.
This commit is contained in:
bresch 2024-11-01 10:39:33 +01:00 committed by Silvan Fuhrer
parent 87b52ae6ee
commit aa5fdd3bb3
1 changed files with 2 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View File

@ -74,11 +74,6 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
{
if (fabsf(vz) < _min_vz_for_valid_consistency) {
// We can only check consistency if there is vertical motion
return;
}
if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
_is_kinematically_consistent = false;
@ -86,7 +81,8 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
}
} else {
if ((_test_ratio < 1.f)
if ((fabsf(vz) > _min_vz_for_valid_consistency)
&& (_test_ratio < 1.f)
&& ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
) {
_is_kinematically_consistent = true;