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:
Daniel Agar
2023-07-24 10:32:33 -04:00
committed by GitHub
co-authored by bresch
parent 190371c6ee
commit 01bcc47fb1
2 changed files with 10 additions and 0 deletions
+8
View File
@@ -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;
+2
View File
@@ -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");
}