diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index f6d681ad02..e7f82d2294 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/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); diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index f311723f99..2ce2d1ac76 100644 --- a/libraries/SITL/SITL.cpp +++ b/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) { diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 74b0d925a9..3fb9c8b491 100644 --- a/libraries/SITL/SITL.h +++ b/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