added function to control when to fuse beta

EKF/control.cpp

@@ -89,6 +89,7 @@ void Ekf::controlFusionModes()
 	controlBaroFusion();
 	controlRangeFinderFusion();
 	controlAirDataFusion();
+	controlBetaFusion();
 
 	// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
 	// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
@@ -771,6 +772,16 @@ void Ekf::controlAirDataFusion()
 	}
 }
 
+void Ekf::controlBetaFusion()
+{
+	bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_ms;
+ 	if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air){
+ 		fuseSideslip();
+ 	}
+ 	_control_status.flags.wind = true; // Look into this
+
+}
+
 void Ekf::controlMagFusion()
 {
 	// If we are using external vision data for heading then no magnetometer fusion is used

EKF/ekf.h

@@ -433,6 +433,9 @@ private:
 	// control fusion of air data observations
 	void controlAirDataFusion();
 
+	// control fusion of synthetic sideslip observations
+	void controlBetaFusion();
+
 	// control fusion of pressure altitude observations
 	void controlBaroFusion();