Browse Source

ekf: get rid of intermediate variable "fuse_height"

master
bresch 3 years ago committed by Mathieu Bresciani
parent
commit
c947562183
  1. 58
      src/modules/ekf2/EKF/control.cpp

58
src/modules/ekf2/EKF/control.cpp

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

Loading…
Cancel
Save