dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Update height reset to support range finder height use

This commit is contained in:
Paul Riseborough 2016-03-07 20:20:24 +11:00
parent dca186c6e8
commit 270451e17b
1 changed files with 15 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
EKF/ekf_helper.cpp
View File

@ -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
}
}
}