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

EKF: fix range height sensor offset (#787)

Browse Source 
The offset needs to be updated directly when switching to range height
This commit is contained in:
Mathieu Bresciani 2020-03-26 10:22:32 +01:00 committed by GitHub
parent 870ec380f1
commit 3bd09fdcd3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
EKF/control.cpp
View File

@ -984,18 +984,19 @@ void Ekf::controlHeightFusion()
setControlRangeHeight();
fuse_height = _range_data_ready;
} else if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
if (_control_status.flags.in_air && isTerrainEstimateValid()) {
_hgt_sensor_offset = _terrain_vpos;
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
} else if (_control_status.flags.in_air) {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
} else {
_hgt_sensor_offset = _params.rng_gnd_clearance;
}
}
} else if (_baro_data_ready && !_baro_hgt_faulty) {