diff --git a/Tools/AP_Periph/airspeed.cpp b/Tools/AP_Periph/airspeed.cpp index 17a4da1607..579b1f04bc 100644 --- a/Tools/AP_Periph/airspeed.cpp +++ b/Tools/AP_Periph/airspeed.cpp @@ -21,7 +21,7 @@ void AP_Periph_FW::can_airspeed_update(void) return; } #if AP_PERIPH_PROBE_CONTINUOUS - if (!airspeed.healthy()) { + if (option_is_set(PeriphOptions::PROBE_CONTINUOUS) && !hal.util->get_soft_armed() && !airspeed.healthy()) { uint32_t now = AP_HAL::millis(); static uint32_t last_probe_ms; if (now - last_probe_ms >= 1000) { diff --git a/Tools/AP_Periph/compass.cpp b/Tools/AP_Periph/compass.cpp index da5d230776..2c70ae1cc0 100644 --- a/Tools/AP_Periph/compass.cpp +++ b/Tools/AP_Periph/compass.cpp @@ -40,7 +40,7 @@ void AP_Periph_FW::can_mag_update(void) compass.read(); #if AP_PERIPH_PROBE_CONTINUOUS - if (compass.get_count() == 0) { + if (option_is_set(PeriphOptions::PROBE_CONTINUOUS) && !hal.util->get_soft_armed() && (compass.get_count() == 0)) { static uint32_t last_probe_ms; uint32_t now = AP_HAL::millis(); if (now - last_probe_ms >= 1000) {