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

Fix GPS altitude not fused if GPS checks fail and GPS is primary height source (#813)

Browse Source 
* EKF: Fix failure to select a height sensor

* EKF: Remove unnecessary check and improve documentation
This commit is contained in:
Paul Riseborough
2020-05-14 19:26:16 +10:00
committed by GitHub
parent 24f2e60b7e
commit 19bbea75c0
1 changed files with 22 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+22 -11
View File
@@ -975,26 +975,37 @@ void Ekf::controlHeightFusion()
case VDIST_SENSOR_GPS: case VDIST_SENSOR_GPS:
// Determine if GPS should be used as the height source // NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
if (do_range_aid && _range_sensor.isDataHealthy()) { // to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
// Do switching between GPS and rangefinder if using range finder as a height source when close
// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
if (!_control_status_prev.flags.rng_hgt && do_range_aid && _range_sensor.isDataHealthy()) {
setControlRangeHeight(); setControlRangeHeight();
fuse_height = true;
// we have just switched to using range finder, calculate height sensor offset such that current // we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate // measurement matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { _hgt_sensor_offset = _terrain_vpos;
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status_prev.flags.rng_hgt && !do_range_aid) {
// must stop using range finder so find another sensor now
if (!_gps_hgt_intermittent && _gps_checks_passed) {
// GPS quality OK
startGpsHgtFusion();
} else if (!_baro_hgt_faulty) {
// Use baro as a fallback
startBaroHgtFusion();
} }
} else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
} else if (!do_range_aid && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) { // In baro fallback mode and GPS has recovered so start using it
fuse_height = true;
startGpsHgtFusion(); startGpsHgtFusion();
}
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) { } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
fuse_height = true; fuse_height = true;
} }
break; break;
case VDIST_SENSOR_EV: case VDIST_SENSOR_EV: