diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 61e42c29f3..79eed58f74 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -407,6 +407,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp GOBJECT(servo_channels, "OUT", SRV_Channels), + // @Param: ESC_RATE + // @DisplayName: ESC Update Rate + // @Description: Rate in Hz that ESC PWM outputs (function is MotorN) will update at + // @Units: Hz + // @Range: 50 400 + // @Increment: 1 + // @User: Advanced + GSCALAR(esc_rate, "ESC_RATE", 400), // effective Copter and QuadPlane default after clamping + // @Param: ESC_PWM_TYPE // @DisplayName: Output PWM type // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index d68e38f7b4..03730a5ff4 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -95,6 +95,7 @@ public: k_param_rangefinder_port1, k_param_options, k_param_rpm_msg_rate, + k_param_esc_rate, }; AP_Int16 format_version; @@ -174,6 +175,7 @@ public: #endif #ifdef HAL_PERIPH_ENABLE_RC_OUT + AP_Int16 esc_rate; AP_Int8 esc_pwm_type; AP_Int16 esc_command_timeout_ms; #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 6f8459db37..4b9f5e97d6 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -67,6 +67,9 @@ void AP_Periph_FW::rcout_init() hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type()); #endif + // run PWM ESCs at configured rate + hal.rcout->set_freq(esc_mask, g.esc_rate.get()); + // setup ESCs with the desired PWM type, allowing for DShot SRV_Channels::init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get());