fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 rng kin: allow check to become true during horizontal motion

Browse Source 
Even if there is some horizontal motion, a passing check should be
accepted as the terrain can be flat. However, the vehicle must not be
moving horizontally to invalidate the consistency as a change in terrain
can make the kinematic check temporarily fail.
This commit is contained in:
bresch
2023-09-22 09:02:35 -04:00
committed by Daniel Agar
parent e920bfb188
commit db97a38a9d
3 changed files with 22 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/range_finder_consistency_check.cpp
+16 -4
View File
@@ -37,8 +37,12 @@
#include "range_finder_consistency_check.hpp"
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
{
if (horizontal_motion) {
_time_last_horizontal_motion = time_us;
}
const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
if ((_time_last_update_us == 0)
@@ -68,12 +72,20 @@ 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) {
_is_kinematically_consistent = false;
_time_last_inconsistent_us = time_us;
if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
_is_kinematically_consistent = false;
_time_last_inconsistent_us = time_us;
}
} else {
if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
if (_test_ratio < 1.f
&& ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
_is_kinematically_consistent = true;
}
}