mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 16:18:52 +08:00
ekf2: apply yaw reset to vision attitude error filter
- set vision attitude error filter uninitialized if vision data stops - ev error filter is only compiled when ev config is selected Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
@@ -1137,6 +1137,14 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
|
||||
// add the reset amount to the output observer buffered data
|
||||
_output_predictor.resetQuaternion(q_error);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// update EV attitude error filter
|
||||
if (_ev_q_error_initialized) {
|
||||
const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
|
||||
_ev_q_error_filt.reset(ev_q_error_updated);
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
|
||||
_state_reset_status.quat_change = q_error;
|
||||
|
||||
@@ -81,6 +81,8 @@ void Ekf::controlExternalVisionFusion()
|
||||
stopEvYawFusion();
|
||||
stopEvHgtFusion();
|
||||
|
||||
_ev_q_error_initialized = false;
|
||||
|
||||
_warning_events.flags.vision_data_stopped = true;
|
||||
ECL_WARN("vision data stopped");
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user