Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 14:17:20 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

ekf: run rng consistency check only when not horizontally moving

Browse Source 
The check assumes a non-moving terrain height
This commit is contained in:
bresch
2022-03-21 15:32:10 +01:00
committed by Mathieu Bresciani
parent d903613c9c
commit 079a5e92ba
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+4
View File
@@ -140,8 +140,12 @@ void Ekf::controlFusionModes()
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
// Run the kinematic consistency check when not moving horizontally
if ((sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu); _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu);
} }
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
} }