SITL: support functions for new simulators

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);
+}
+

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__