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

make range sensor height offset computation more robust

Browse Source 
- when switching to range finder use the current terrain estimate as
height sensor offset, otherwise spikes in the range measurements could lead
to a wrong offset

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2017-06-15 09:10:43 +02:00
committed by ChristophTobler
parent 182ea43445
commit c5d464b821
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -2
View File
@@ -713,7 +713,11 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
}
} else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) {
@@ -745,7 +749,11 @@ void Ekf::controlHeightFusion()
// we have just switched to using range finder, calculate height sensor offset such that current
// measurment matches our current height estimate
if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
if (_terrain_initialised) {
_hgt_sensor_offset = _terrain_vpos;
} else {
_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
}
}
} else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {