diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index f563d9dbc1..6f4a6f1727 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -138,9 +138,10 @@ void Util::set_imu_temp(float current) uint32_t now = AP_HAL::millis(); if (now - heater.last_update_ms < 1000) { #if defined(HAL_HEATER_GPIO_PIN) - // output as duty cycle to local pin - hal.gpio->write(HAL_HEATER_GPIO_PIN, heater.duty_counter < heater.output); - heater.duty_counter = (heater.duty_counter+1) % 100; + // output as duty cycle to local pin. Use a random sequence to + // prevent a periodic change to magnetic field + bool heater_on = (get_random16() < uint32_t(heater.output) * 0xFFFFU / 100U); + hal.gpio->write(HAL_HEATER_GPIO_PIN, heater_on); #endif return; } diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index b86ba417c7..4fc1cadcdd 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -86,7 +86,6 @@ private: uint16_t count; float sum; uint32_t last_update_ms; - uint8_t duty_counter; float output; } heater; #endif