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

reset the newest output sample for all possible resets

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-10-11 17:05:05 +02:00 committed by Lorenz Meier
parent 238d947239
commit f80b3481a4
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/control.cpp
View File

@ -153,6 +153,10 @@ void Ekf::controlExternalVisionFusion()
_output_buffer.push_to_index(i,output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;

14
EKF/ekf_helper.cpp
View File

@ -293,6 +293,16 @@ void Ekf::resetHeight()
}
// apply the change in height / height rate to our newest height / height rate estimate
// which have already been taken out from the output buffer
if (vert_pos_reset) {
_output_new.pos(2) += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
_output_new.vel(2) += _state_reset_status.velD_change;
}
}
// align output filter states to match EKF states at the fusion time horizon
@ -469,6 +479,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
_output_buffer.push_to_index(i,output_states);
}
// apply the change in attitude quaternion to our newest quaternion estimate
// which was already taken out from the output buffer
_output_new.quat_nominal *= _state_reset_status.quat_change;
// capture the reset event
_state_reset_status.quat_counter++;