From 19147a05f6a89de7f19df80e20335bf2ee3df3cf Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 2 May 2015 20:29:16 +1000
Subject: [PATCH] SITL: support functions for new simulators

---
 libraries/SITL/SITL.cpp | 25 +++++++++++++++++++++++++
 libraries/SITL/SITL.h   |  4 ++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index d81e33349a..a8c6cc03f0 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -156,3 +156,28 @@ void SITL::convert_body_frame(double rollDeg, double pitchDeg,
 	*r = cosf(phi)*psiDot*cosf(theta) - sinf(phi)*thetaDot;    
 }
 
+
+/*
+  convert angular velocities from body frame to
+  earth frame.
+  
+  all inputs and outputs are in radians/s
+*/
+Vector3f SITL::convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro)
+{
+    float p = gyro.x;
+    float q = gyro.y;
+    float r = gyro.z;
+
+    float phi, theta, psi;
+    dcm.to_euler(&phi, &theta, &psi);
+
+    float phiDot = p + tanf(theta)*(q*sinf(phi) + r*cosf(phi));
+    float thetaDot = q*cosf(phi) - r*sinf(phi);
+    if (fabsf(cosf(theta)) < 1.0e-20f) {
+        theta += 1.0e-10f;
+    }
+    float psiDot = (q*sinf(phi) + r*cosf(phi))/cosf(theta);
+    return Vector3f(phiDot, thetaDot, psiDot);
+}
+
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 0047127970..6892a8b7f5 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -13,6 +13,7 @@ struct PACKED sitl_fdm {
 	// this is the packet sent by the simulator
 	// to the APM executable to update the simulator state
 	// All values are little-endian
+    uint64_t timestamp_us;
 	double latitude, longitude; // degrees
 	double altitude;  // MSL
 	double heading;   // degrees
@@ -106,6 +107,9 @@ public:
 	static void convert_body_frame(double rollDeg, double pitchDeg,
 				       double rollRate, double pitchRate, double yawRate,
 				       double *p, double *q, double *r);
+
+	// convert a set of roll rates from body frame to earth frame
+    static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
 };
 
 #endif // __SITL_H__