fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 19:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

rng height: reset to baro using common logic

Browse Source 
The failsafe from rng height to baro height should occur after a reset
to baro triggered by the controlHeightSensorTimeouts function and not in
the fusion selector.

Until now, the EKF was fusing baro measurements between rng updates if
the range finder ODR was slower than the EKF update rate. This is not
the case anymore as the height reset occurs after 5 seconds.

The unit test has been extended to show this behavior.
This commit is contained in:
bresch
2021-01-06 11:53:52 +01:00
committed by Mathieu Bresciani
parent 03fed323ab
commit c212975abe
5 changed files with 33 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -9
View File
@@ -989,8 +989,8 @@ void Ekf::controlHeightFusion()
}
}
} else if (_baro_data_ready && !_baro_hgt_faulty) {
startBaroHgtFusion();
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// fuse baro data if there was a reset to baro
fuse_height = true;
}
@@ -1055,16 +1055,17 @@ void Ekf::controlHeightFusion()
updateBaroHgtOffset();
if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt
&& (!_range_sensor.isDataHealthy())) {
if (_control_status.flags.rng_hgt
&& isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)
&& !_range_sensor.isDataHealthy()
&& _range_sensor.isRegularlySendingData()
&& !_control_status.flags.in_air) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setDataReadiness(true);
_range_sensor.setValidity(true); // bypass the checks
}
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setDataReadiness(true);
_range_sensor.setValidity(true); // bypass the checks
fuse_height = true;
}