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

ekf: get rid of intermediate variable "fuse_height"

Browse Source
This commit is contained in:
bresch 2021-12-06 16:38:14 +01:00
parent 97764aa23a
commit 6db9ed099d
1 changed files with 26 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/modules/ekf2/EKF/control.cpp
View File

@ -906,50 +906,42 @@ void Ekf::controlHeightFusion()
break;
}
bool fuse_height = false;
if (_control_status.flags.gps_hgt && _gps_data_ready) {
fuse_height = true;
} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
fuse_height = true;
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
fuse_height = true;
} else if (_control_status.flags.ev_hgt && _ev_data_ready) {
fuse_height = true;
}
updateBaroHgtBias();
updateBaroHgtOffset();
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 (_control_status.flags.baro_hgt) {
// 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
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setDataReadiness(true);
_range_sensor.setValidity(true); // bypass the checks
fuse_height = true;
}
if (fuse_height) {
if (_control_status.flags.baro_hgt) {
if (_baro_data_ready && !_baro_hgt_faulty) {
fuseBaroHgt();
}
} else if (_control_status.flags.gps_hgt) {
} else if (_control_status.flags.gps_hgt) {
if (_gps_data_ready) {
fuseGpsHgt();
}
} else if (_control_status.flags.rng_hgt) {
} else if (_control_status.flags.rng_hgt) {
if (_range_sensor.isDataHealthy()) {
fuseRngHgt();
} else if (_control_status.flags.ev_hgt) {
} else if (!_control_status.flags.in_air
&& _range_sensor.isRegularlySendingData()
&& isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
// 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
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setDataReadiness(true);
_range_sensor.setValidity(true); // bypass the checks
fuseRngHgt();
}
} else if (_control_status.flags.ev_hgt) {
if (_control_status.flags.ev_hgt && _ev_data_ready) {
fuseEvHgt();
}
}