From 406de11fe98b6ddabe0dac9e352bbcd2845fc725 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 27 Apr 2022 16:33:12 +1000 Subject: [PATCH] AP_Periph: mark ESC DShot channels as digital this allows them to get the right default MIN and MAX values on the channels --- Tools/AP_Periph/rc_out.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index d50cee638b..d1e43f1ee4 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -59,7 +59,13 @@ void AP_Periph_FW::rcout_init() } // setup ESCs with the desired PWM type, allowing for DShot - hal.rcout->set_output_mode(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get()); + const auto esc_type = (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get(); + hal.rcout->set_output_mode(esc_mask, esc_type); + + if (esc_type >= AP_HAL::RCOutput::MODE_PWM_DSHOT150 && + esc_type <= AP_HAL::RCOutput::MODE_PWM_DSHOT1200) { + SRV_Channels::set_digital_outputs(esc_mask, 0); + } // run this once and at 1Hz to configure aux and esc ranges rcout_init_1Hz();