SITL: removed earth frame rates

libraries/SITL/SIM_Aircraft.cpp

@@ -224,10 +224,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) const
     fdm.xAccel    = accel_body.x;
     fdm.yAccel    = accel_body.y;
     fdm.zAccel    = accel_body.z;
-    Vector3f gyro_ef = SITL::convert_earth_frame(dcm, gyro);
-    fdm.rollRate  = degrees(gyro_ef.x);
-    fdm.pitchRate = degrees(gyro_ef.y);
-    fdm.yawRate   = degrees(gyro_ef.z);
+    fdm.rollRate  = degrees(gyro.x);
+    fdm.pitchRate = degrees(gyro.y);
+    fdm.yawRate   = degrees(gyro.z);
     float r, p, y;
     dcm.to_euler(&r, &p, &y);
     fdm.rollDeg  = degrees(r);

libraries/SITL/SITL.cpp

@@ -76,15 +76,8 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
 /* report SITL state via MAVLink */
 void SITL::simstate_send(mavlink_channel_t chan)
 {
-    double p, q, r;
     float yaw;
 
-    // we want the gyro values to be directly comparable to the
-    // raw_imu message, which is in body frame
-    convert_body_frame(state.rollDeg, state.pitchDeg,
-                       state.rollRate, state.pitchRate, state.yawRate,
-                       &p, &q, &r);
-
     // convert to same conventions as DCM
     yaw = state.yawDeg;
     if (yaw > 180) {
@@ -98,7 +91,9 @@ void SITL::simstate_send(mavlink_channel_t chan)
                               state.xAccel,
                               state.yAccel,
                               state.zAccel,
-                              p, q, r,
+                              radians(state.rollRate), 
+                              radians(state.pitchRate), 
+                              radians(state.yawRate), 
                               state.latitude*1.0e7,
                               state.longitude*1.0e7);
 }
@@ -106,15 +101,8 @@ void SITL::simstate_send(mavlink_channel_t chan)
 /* report SITL state to DataFlash */
 void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
 {
-    double p, q, r;
     float yaw;
 
-    // we want the gyro values to be directly comparable to the
-    // raw_imu message, which is in body frame
-    convert_body_frame(state.rollDeg, state.pitchDeg,
-                       state.rollRate, state.pitchRate, state.yawRate,
-                       &p, &q, &r);
-
     // convert to same conventions as DCM
     yaw = state.yawDeg;
     if (yaw > 180) {

libraries/SITL/SITL.h

@@ -19,7 +19,7 @@ struct PACKED sitl_fdm {
     double heading;   // degrees
     double speedN, speedE, speedD; // m/s
     double xAccel, yAccel, zAccel;       // m/s/s in body frame
-    double rollRate, pitchRate, yawRate; // degrees/s/s in earth frame
+    double rollRate, pitchRate, yawRate; // degrees/s/s in body frame
     double rollDeg, pitchDeg, yawDeg;    // euler angles, degrees
     double airspeed; // m/s
     uint32_t magic; // 0x4c56414f