Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

ekf_helper: use latest validated range sample to reset height

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2019-10-14 08:52:18 +02:00
committed by Paul Riseborough
parent 00f49e62c2
commit 6299d8199c
1 changed files with 1 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+1 -13
View File
@@ -224,15 +224,8 @@ void Ekf::resetHeight()
// reset the vertical position // reset the vertical position
if (_control_status.flags.rng_hgt) { if (_control_status.flags.rng_hgt) {
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) { float new_pos_down = _hgt_sensor_offset - _range_sample_delayed.rng * _R_rng_to_earth_2_2;
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
range_newest.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
// calculate the new vertical position using range sensor
float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2;
// update the state and associated variance // update the state and associated variance
_state.pos(2) = new_pos_down; _state.pos(2) = new_pos_down;
@@ -250,11 +243,6 @@ void Ekf::resetHeight()
const baroSample &baro_newest = _baro_buffer.get_newest(); const baroSample &baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt + _state.pos(2); _baro_hgt_offset = baro_newest.hgt + _state.pos(2);
} else {
// TODO: reset to last known range based estimate
}
} else if (_control_status.flags.baro_hgt) { } else if (_control_status.flags.baro_hgt) {
// initialize vertical position with newest baro measurement // initialize vertical position with newest baro measurement
const baroSample &baro_newest = _baro_buffer.get_newest(); const baroSample &baro_newest = _baro_buffer.get_newest();