SITL: Add angular acceleration to simulator states

The calculation used is an approximation to work around the lack of angular acceleration in the interface with the flight dynamics model.

libraries/SITL/SIM_Aircraft.cpp

@@ -42,6 +42,8 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
     frame_height(0),
     dcm(),
     gyro(),
+    gyro_prev(),
+    ang_accel(),
     velocity_ef(),
     mass(0),
     accel_body(0, 0, -GRAVITY_MSS),
@@ -331,6 +333,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
     fdm.rollRate  = degrees(gyro.x);
     fdm.pitchRate = degrees(gyro.y);
     fdm.yawRate   = degrees(gyro.z);
+    fdm.angAccel.x = degrees(ang_accel.x);
+    fdm.angAccel.y = degrees(ang_accel.y);
+    fdm.angAccel.z = degrees(ang_accel.z);
     float r, p, y;
     dcm.to_euler(&r, &p, &y);
     fdm.rollDeg  = degrees(r);
@@ -407,6 +412,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     gyro.x = constrain_float(gyro.x, -radians(2000), radians(2000));
     gyro.y = constrain_float(gyro.y, -radians(2000), radians(2000));
     gyro.z = constrain_float(gyro.z, -radians(2000), radians(2000));
+
+    // estimate angular acceleration using a first order difference calculation
+    // TODO the simulator interface should provide the angular acceleration
+    ang_accel = (gyro - gyro_prev) / delta_time;
+    gyro_prev = gyro;
     
     // update attitude
     dcm.rotate(gyro * delta_time);

libraries/SITL/SIM_Aircraft.h

@@ -114,6 +114,8 @@ protected:
     float frame_height;
     Matrix3f dcm;  // rotation matrix, APM conventions, from body to earth
     Vector3f gyro; // rad/s
+    Vector3f gyro_prev; // rad/s
+    Vector3f ang_accel; // rad/s/s
     Vector3f velocity_ef; // m/s, earth frame
     Vector3f wind_ef; // m/s, earth frame
     Vector3f velocity_air_ef; // velocity relative to airmass, earth frame

libraries/SITL/SITL.h

@@ -24,6 +24,7 @@ struct sitl_fdm {
     uint8_t rcin_chan_count;
     float  rcin[8];         // RC input 0..1
     Vector3f bodyMagField;  // Truth XYZ magnetic field vector in body-frame. Includes motor interference. Units are milli-Gauss.
+    Vector3f angAccel; // Angular acceleration in degrees/s/s about the XYZ body axes
 };
 
 // number of rc output channels