From 88c52aba5ed8bf1335601865fad3b50028b532c0 Mon Sep 17 00:00:00 2001
From: kritz <kritz@ethz.ch>
Date: Mon, 27 Jul 2020 11:42:52 +0200
Subject: [PATCH] Refactor ev fusion start into helper functions (#872)

---
 EKF/control.cpp    | 27 +++------------------------
 EKF/ekf.h          |  7 ++++---
 EKF/ekf_helper.cpp | 33 +++++++++++++++++++++++++++++++++
 3 files changed, 40 insertions(+), 27 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index c727bbb963..01a996499c 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -200,16 +200,12 @@ void Ekf::controlExternalVisionFusion()
 			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
 				// turn on use of external vision measurements for position
 				if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
-					_control_status.flags.ev_pos = true;
-					resetHorizontalPosition();
-					ECL_INFO_TIMESTAMPED("starting vision pos fusion");
+					startEvPosFusion();
 				}
 
 				// turn on use of external vision measurements for velocity
 				if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
-					_control_status.flags.ev_vel = true;
-					resetVelocity();
-					ECL_INFO_TIMESTAMPED("starting vision vel fusion");
+					startEvVelFusion();
 				}
 			}
 		}
@@ -218,24 +214,7 @@ void Ekf::controlExternalVisionFusion()
 		if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
 			// don't start using EV data unless data is arriving frequently
 			if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
-				// reset the yaw angle to the value from the vision quaternion
-				const Eulerf euler_obs(_ev_sample_delayed.quat);
-				const float yaw = euler_obs(2);
-				const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
-
-				resetQuatStateYaw(yaw, yaw_variance, true);
-
-				// flag the yaw as aligned
-				_control_status.flags.yaw_align = true;
-
-				// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
-				_control_status.flags.ev_yaw = true;
-				_control_status.flags.mag_dec = false;
-
-				stopMagHdgFusion();
-				stopMag3DFusion();
-
-				ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
+				startEvYawFusion();
 			}
 		}
 
diff --git a/EKF/ekf.h b/EKF/ekf.h
index ab38e52afe..66be18b855 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -854,12 +854,13 @@ private:
 	void startGpsYawFusion();
 	void stopGpsYawFusion();
 
-	void stopEvFusion();
+	void startEvPosFusion();
+	void startEvVelFusion();
+	void startEvYawFusion();
 
+	void stopEvFusion();
 	void stopEvPosFusion();
-
 	void stopEvVelFusion();
-
 	void stopEvYawFusion();
 
 	void stopAuxVelFusion();
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index b4c2f65905..68beaa2df2 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -1575,6 +1575,39 @@ void Ekf::stopGpsYawFusion()
 	_control_status.flags.gps_yaw = false;
 }
 
+void Ekf::startEvPosFusion() {
+	_control_status.flags.ev_pos = true;
+	resetHorizontalPosition();
+	ECL_INFO_TIMESTAMPED("starting vision pos fusion");
+}
+
+void Ekf::startEvVelFusion() {
+	_control_status.flags.ev_vel = true;
+	resetVelocity();
+	ECL_INFO_TIMESTAMPED("starting vision vel fusion");
+}
+
+void Ekf::startEvYawFusion() {
+	// reset the yaw angle to the value from the vision quaternion
+	const Eulerf euler_obs(_ev_sample_delayed.quat);
+	const float yaw = euler_obs(2);
+	const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
+
+	resetQuatStateYaw(yaw, yaw_variance, true);
+
+	// flag the yaw as aligned
+	_control_status.flags.yaw_align = true;
+
+	// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
+	_control_status.flags.ev_yaw = true;
+	_control_status.flags.mag_dec = false;
+
+	stopMagHdgFusion();
+	stopMag3DFusion();
+
+	ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
+}
+
 void Ekf::stopEvFusion()
 {
 	stopEvPosFusion();