diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 25f06a806f..8ce7bb2a7e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1769,11 +1769,13 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan #if HAL_INS_ENABLED const AP_InertialSensor &ins = AP::ins(); const Compass &compass = AP::compass(); + int16_t _temperature = 0; bool have_data = false; Vector3f accel{}; if (ins.get_accel_count() > instance) { accel = ins.get_accel(instance); + _temperature = ins.get_temperature(instance)*100; have_data = true; } Vector3f gyro{}; @@ -1801,7 +1803,7 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan mag.x, mag.y, mag.z, - int16_t(ins.get_temperature(instance)*100)); + _temperature); #endif }