Add stop*Fusion function

EKF/control.cpp

@@ -392,9 +392,7 @@ void Ekf::controlExternalVisionFusion()
 		   && ((_time_last_imu - _time_last_ext_vision) > (uint64_t)_params.reset_timeout_max)) {
 
 		// Turn off EV fusion mode if no data has been received
-		_control_status.flags.ev_pos = false;
-		_control_status.flags.ev_vel = false;
-		_control_status.flags.ev_yaw = false;
+		stopEvFusion();
 		ECL_INFO_TIMESTAMPED("EKF External Vision Data Stopped");
 
 	}
@@ -464,12 +462,12 @@ void Ekf::controlOpticalFlowFusion()
 		// Handle cases where we are using optical flow but are no longer able to because data is old
 		// or its use has been inhibited.
 		if (_control_status.flags.opt_flow) {
-		       if (_inhibit_flow_use) {
-			       _control_status.flags.opt_flow = false;
-			       _time_last_of_fuse = 0;
+			if (_inhibit_flow_use) {
+				stopFlowFusion();
+				_time_last_of_fuse = 0;
 
 			} else if ((_time_last_imu - _time_last_of_fuse) > (uint64_t)_params.reset_timeout_max) {
-				_control_status.flags.opt_flow = false;
+				stopFlowFusion();
 
 			}
 		}
@@ -642,7 +640,7 @@ void Ekf::controlGpsFusion()
 
 		// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
 		if (_control_status.flags.gps  && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
-			_control_status.flags.gps = false;
+			stopGpsFusion();
 			// Reset position state to external vision if we are going to use absolute values
 			if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
 				resetPosition();
@@ -740,12 +738,12 @@ void Ekf::controlGpsFusion()
 		}
 
 	} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
-		_control_status.flags.gps = false;
+		stopGpsFusion();
 		ECL_WARN_TIMESTAMPED("EKF GPS data stopped");
 	}  else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) {
 		// Handle the case where we are fusing another position source along GPS,
 		// stop waiting for GPS after 1 s of lost signal
-		_control_status.flags.gps = false;
+		stopGpsFusion();
 		ECL_WARN_TIMESTAMPED("EKF GPS data stopped, using only EV or OF");
 	}
 }

EKF/ekf.h

@@ -789,4 +789,37 @@ private:
 	// sensor measurement
 	float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted);
 
+	// stop GPS fusion
+	void stopGpsFusion();
+
+	// stop GPS position fusion
+	void stopGpsPosFusion();
+
+	// stop GPS velocity fusion
+	void stopGpsVelFusion();
+
+	// stop GPS yaw fusion
+	void stopGpsYawFusion();
+
+	// stop external vision fusion
+	void stopEvFusion();
+
+	// stop external vision position fusion
+	void stopEvPosFusion();
+
+	// stop external vision velocity fusion
+	void stopEvVelFusion();
+
+	// stop external vision yaw fusion
+	void stopEvYawFusion();
+
+	// stop auxiliary horizontal velocity fusion
+	void stopAuxVelFusion();
+
+	// stop optical flow fusion
+	void stopFlowFusion();
+
+	// Helper function to set velPos fault status
+	void setVelPosFaultStatus(const int index, const bool status);
+
 };

EKF/ekf_helper.cpp

@@ -1870,3 +1870,73 @@ float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) c
 	accumulator = (t - sum_previous) - y;
 	return t;
 }
+
+
+void Ekf::stopGpsFusion()
+{
+	stopGpsPosFusion();
+	stopGpsVelFusion();
+	stopGpsYawFusion();
+}
+
+void Ekf::stopGpsPosFusion()
+{
+	_control_status.flags.gps = false;
+	_control_status.flags.gps_hgt = false;
+	memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3);
+	memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
+	memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
+}
+
+void Ekf::stopGpsVelFusion()
+{
+	memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3);
+	memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3);
+	memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2);
+}
+
+void Ekf::stopGpsYawFusion()
+{
+	_control_status.flags.gps_yaw = false;
+}
+
+void Ekf::stopEvFusion()
+{
+	stopEvPosFusion();
+	stopEvVelFusion();
+	stopEvYawFusion();
+}
+
+void Ekf::stopEvPosFusion()
+{
+	_control_status.flags.ev_pos = false;
+	memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3);
+	memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
+	memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
+}
+
+void Ekf::stopEvVelFusion()
+{
+	_control_status.flags.ev_vel = false;
+	memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3);
+	memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3);
+	memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);}
+
+void Ekf::stopEvYawFusion()
+{
+	_control_status.flags.ev_yaw = false;
+
+}
+
+void Ekf::stopAuxVelFusion()
+{
+	// TODO: Add proper handling of auxiliar velocity fusion
+}
+
+void Ekf::stopFlowFusion()
+{
+	_control_status.flags.opt_flow = false;
+	memset(_flow_innov,0.0f,sizeof(_flow_innov));
+	memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
+	memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
+}