diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 7fda979a44..55ef068a0c 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -430,6 +430,27 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan) mag.x, mag.y, mag.z); + + if (ins.get_gyro_count() <= 1 && + ins.get_accel_count() <= 1 && + compass.get_count() <= 1) { + return; + } + const Vector3f &accel2 = ins.get_accel(1); + const Vector3f &gyro2 = ins.get_gyro(1); + const Vector3f &mag2 = compass.get_field(1); + mavlink_msg_scaled_imu2_send( + chan, + millis(), + accel2.x * 1000.0f / GRAVITY_MSS, + accel2.y * 1000.0f / GRAVITY_MSS, + accel2.z * 1000.0f / GRAVITY_MSS, + gyro2.x * 1000.0f, + gyro2.y * 1000.0f, + gyro2.z * 1000.0f, + mag2.x, + mag2.y, + mag2.z); } static void NOINLINE send_raw_imu2(mavlink_channel_t chan)