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

ekf2: don't reset height when in conditional range aiding

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent 20cd599bb7
commit 8dd5d0d1a3
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/range_height_control.cpp
+6 -1
View File
@@ -137,7 +137,12 @@ bool Ekf::isConditionalRangeAidSuitable()
void Ekf::startRngHgtFusion()
{
if (!_control_status.flags.rng_hgt) {
if (_params.height_sensor_ref == HeightSensor::RANGE) {
if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl == RngCtrl::CONDITIONAL)) {
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
_rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom());
_height_sensor_ref = HeightSensor::RANGE;
} else if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl != RngCtrl::CONDITIONAL)) {
// Range finder is the primary height source, the ground is now the datum used
// to compute the local vertical position
_rng_hgt_b_est.reset();