diff --git a/ArduCopter/rate_thread.cpp b/ArduCopter/rate_thread.cpp index 5b64301fb7bec9..a467239c1e67bc 100644 --- a/ArduCopter/rate_thread.cpp +++ b/ArduCopter/rate_thread.cpp @@ -214,7 +214,11 @@ void Copter::rate_controller_thread() #endif // allow changing option at runtime - if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) { + if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test +#if HAL_INS_TEMPERATURE_CAL_ENABLE + || ins.temperature_cal_running() +#endif + ) { if (was_using_rate_thread) { disable_fast_rate_loop(rates); was_using_rate_thread = false;