From 270451e17b0299250277cfec58fca0bec25f79ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:20:24 +1100 Subject: [PATCH] EKF: Update height reset to support range finder height use --- EKF/ekf_helper.cpp | 34 +++++++++++++++------------------- 1 file changed, 15 insertions(+), 19 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f126098563..2a7a5e85b3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -80,32 +80,28 @@ void Ekf::resetPosition() } } -// Reset height state using the last baro measurement +// Reset height state using the last height measurement void Ekf::resetHeight() { - // if we have a valid GPS measurement use it to initialise the vertical velocity state - gpsSample gps_newest = _gps_buffer.get_newest(); + if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + rangeSample range_newest = _range_buffer.get_newest(); - if (_time_last_imu - gps_newest.time_us < 400000) { - _state.vel(2) = gps_newest.vel(2); + if (_time_last_imu - range_newest.time_us < 200000) { + _state.pos(2) = _hgt_at_alignment - range_newest.rng; + } else { + // TODO: reset to last known range based estimate + } } else { - _state.vel(2) = 0.0f; - } + // initialize vertical position with newest baro measurement + baroSample baro_newest = _baro_buffer.get_newest(); - // if we have a valid height measurement, use it to initialise the vertical position state - baroSample baro_newest = _baro_buffer.get_newest(); + if (_time_last_imu - baro_newest.time_us < 200000) { + _state.pos(2) = _hgt_at_alignment - baro_newest.hgt; - if (_time_last_imu - baro_newest.time_us < 200000) { - // use baro as the default - _state.pos(2) = _baro_at_alignment - baro_newest.hgt; - - } else if (_time_last_imu - gps_newest.time_us < 400000) { - // use GPS as a backup - _state.pos(2) = _gps_alt_ref - gps_newest.hgt; - - } else { - // Do not modify the state as there are no measurements to use + } else { + // TODO: reset to last known baro based estimate + } } }