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

EKF: Reset vertical velocity when performing a height reset

Browse Source
This commit is contained in:
Paul Riseborough
2016-04-11 19:04:43 +10:00
parent 61b919754d
commit f4f108d57d
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+10 -3
View File
@@ -87,6 +87,9 @@ bool Ekf::resetPosition()
// Reset height state using the last height measurement
void Ekf::resetHeight()
{
// Get the most recent GPS data
gpsSample gps_newest = _gps_buffer.get_newest();
if (_control_status.flags.rng_hgt) {
rangeSample range_newest = _range_buffer.get_newest();
@@ -114,11 +117,8 @@ void Ekf::resetHeight()
} else if (_control_status.flags.gps_hgt) {
// initialize vertical position and velocity with newest gps measurement
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
_state.vel(2) = gps_newest.vel(2);
} else {
// TODO: reset to last known gps based estimate
@@ -129,6 +129,13 @@ void Ekf::resetHeight()
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
}
// If we are using GPS, then use it to reset the vertical velocity
if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
_state.vel(2) = gps_newest.vel(2);
} else {
_state.vel(2) = 0.0f;
}
}
// Reset heading and magnetic field states