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

EKF: Add missing reset for output observer vertical position derivative

Browse Source 
Required to prevent large transients in value returned by get_pos_d_deriv accessor following a vertical state reset event.
This commit is contained in:
Paul Riseborough
2018-01-31 11:30:46 +11:00
committed by Lorenz Meier
parent d078d68f64
commit db5264e45c
1 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+12
View File
@@ -300,12 +300,24 @@ void Ekf::resetHeight()
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
if (vert_pos_reset) {
_output_buffer[i].pos(2) += _state_reset_status.posD_change;
_output_vert_buffer[i].vel_d_integ += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
_output_buffer[i].vel(2) += _state_reset_status.velD_change;
_output_vert_buffer[i].vel_d += _state_reset_status.velD_change;
}
}
// add the reset amount to the output observer vertical position state
if (vert_pos_reset) {
_output_vert_delayed.vel_d_integ = _state.pos(2);
_output_vert_new.vel_d_integ = _state.pos(2);
}
if (vert_vel_reset) {
_output_vert_delayed.vel_d = _state.vel(2);
_output_vert_new.vel_d = _state.vel(2);
}
}
// align output filter states to match EKF states at the fusion time horizon