Skip to content

Commit

Permalink
ekf2: apply yaw reset to vision attitude error filter
Browse files Browse the repository at this point in the history
 - 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 <[email protected]>
  • Loading branch information
dagar and bresch authored Jul 24, 2023
1 parent 190371c commit 01bcc47
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 0 deletions.
8 changes: 8 additions & 0 deletions src/modules/ekf2/EKF/ekf_helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF/ev_control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}
Expand Down

0 comments on commit 01bcc47

Please sign in to comment.