From cc5db74d1b8ca5febbfcae0cb3ee479f636934d1 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 7 Aug 2017 09:19:48 +1000
Subject: [PATCH 1/2] EKF: Add true airspeed accessor

---
 EKF/airspeed_fusion.cpp   | 6 ++++++
 EKF/ekf.h                 | 3 +++
 EKF/estimator_interface.h | 2 ++
 3 files changed, 11 insertions(+)

diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp
index 3adae54bb8..0d720f7589 100644
--- a/EKF/airspeed_fusion.cpp
+++ b/EKF/airspeed_fusion.cpp
@@ -225,6 +225,12 @@ void Ekf::get_wind_velocity(float *wind)
 	wind[1] = _state.wind_vel(1);
 }
 
+void Ekf::get_true_airspeed(float *tas)
+{
+	float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
+	memcpy(tas, &tempvar, sizeof(float));
+}
+
 /*
  * Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
 */
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 91c7e191e0..38329d56b3 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -112,6 +112,9 @@ public:
 	// get the wind velocity in m/s
 	void get_wind_velocity(float *wind);
 
+	// get the true airspeed in m/s
+	void get_true_airspeed(float *tas);
+
 	// get the diagonal elements of the covariance matrix
 	void get_covariances(float *covariances);
 
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index cec722fb2a..9d652a4c14 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -93,6 +93,8 @@ public:
 
 	virtual void get_wind_velocity(float *wind) = 0;
 
+	virtual void get_true_airspeed(float *tas) = 0;
+
 	virtual void get_covariances(float *covariances) = 0;
 
 	// gets the variances for the NED velocity states

From 35ffd5548126a356fa7084ca1e964f3da9b2897f Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 7 Aug 2017 10:34:19 +1000
Subject: [PATCH 2/2] EKF: Fix incorrect use of double precision operation

---
 EKF/mag_fusion.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index 00780c4300..6c19d63950 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -537,12 +537,12 @@ void Ekf::fuseHeading()
 			// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
 			// Equations from Tbn_312.c produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
 			Dcmf R_to_earth;
-			float sy = sin(yaw);
-			float cy = cos(yaw);
-			float sp = sin(pitch);
-			float cp = cos(pitch);
-			float sr = sin(roll);
-			float cr = cos(roll);
+			float sy = sinf(yaw);
+			float cy = cosf(yaw);
+			float sp = sinf(pitch);
+			float cp = cosf(pitch);
+			float sr = sinf(roll);
+			float cr = cosf(roll);
 			R_to_earth(0,0) = cy*cp-sy*sp*sr;
 			R_to_earth(0,1) = -sy*cr;
 			R_to_earth(0,2) = cy*sp+sy*cp*sr;