diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 4a0dc6aa1f..493c207184 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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; diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index 6bad3d2695..22ac4ec764 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -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"); }