From 5205a8198bf0e891a654809178213071496d6554 Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Tue, 12 Jul 2016 19:54:14 +0200
Subject: [PATCH] added function to control when to fuse beta

---
 EKF/control.cpp | 11 +++++++++++
 EKF/ekf.h       |  3 +++
 2 files changed, 14 insertions(+)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index c915035608..cbeb4e654b 100644
--- a/EKF/control.cpp
+++ b/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
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 457d24a999..e3017b63c3 100644
--- a/EKF/ekf.h
+++ b/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();