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

EKF: update vertical position and velocity reset capture

Browse Source 
Use reset event struct members instead of separate variables
This commit is contained in:
Paul Riseborough
2016-06-01 17:13:00 +10:00
parent 0605c88ecc
commit aca0336392
3 changed files with 9 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+8 -8
View File
@@ -289,18 +289,18 @@ void Ekf::resetHeight()
// store the reset amount and time to be published
if (vert_pos_reset) {
_vert_pos_reset_delta = _state.pos(2) - old_vert_pos;
_time_vert_pos_reset = _time_last_imu;
_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
_state_reset_status.posD_time_us = _imu_sample_delayed.time_us;
}
if (vert_vel_reset) {
_vert_vel_reset_delta = _state.vel(2) - old_vert_vel;
_time_vert_vel_reset = _time_last_imu;
_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
_state_reset_status.velD_time_us = _imu_sample_delayed.time_us;
}
// add the reset amount to the output observer states
_output_new.pos(2) += _vert_pos_reset_delta;
_output_new.vel(2) += _vert_vel_reset_delta;
_output_new.pos(2) += _state_reset_status.posD_change;
_output_new.vel(2) += _state_reset_status.velD_change;
// add the reset amount to the output observer buffered data
outputSample output_states;
@@ -309,11 +309,11 @@ void Ekf::resetHeight()
output_states = _output_buffer.get_from_index(i);
if (vert_pos_reset) {
output_states.pos(2) += _vert_pos_reset_delta;
output_states.pos(2) += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
output_states.vel(2) += _vert_vel_reset_delta;
output_states.vel(2) += _state_reset_status.velD_change;
}
_output_buffer.push_to_index(i,output_states);