Enable local frame alignment also without using it

PX4 · Aug 28, 2019 · 933c32c · 933c32c
1 parent ea352a6
commit 933c32c
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 3 deletions.
diff --git 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
@@ -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();
 	}