diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index d3b71c6fee..0fdfcb3296 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1334,6 +1334,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us) } #endif bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; + bool armed = hal.util->get_soft_armed(); memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH); @@ -1383,6 +1384,11 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us) value += 47; } + if (!armed) { + // when disarmed we always send a zero value + value = 0; + } + // according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc bool request_telemetry = telem_request_mask & chan_mask; uint16_t packet = create_dshot_packet(value, request_telemetry, group.bdshot.enabled);