From 933c32c921e1c46b3d0853e388fe5d9637d9521a Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 20 Aug 2019 15:16:19 +0200 Subject: [PATCH] Enable local frame alignment also without using it --- EKF/control.cpp | 11 +++++++++-- EKF/ekf_helper.cpp | 2 +- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 61a8d076fe..12e4f74329 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -167,14 +167,21 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } + // reset external vision rotation matrix independent if is actually used for debugging purposes + if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL) + && (_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)){ + // Reset transformation between EV and EKF navigation frames + resetExtVisRotMat(); + } + // external vision position aiding selection logic if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align - && _control_status.flags.yaw_align) { + && _control_status.flags.yaw_align) { // check for a external vision measurement that has fallen behind the fusion time horizon if ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL)) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a9fdc5ba09..f86ca38be3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -731,7 +731,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update _R_to_earth = quat_to_invrotmat(_state.quat_nominal); // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { + if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) { resetExtVisRotMat(); }