From f6e01dbee9adf5c170acb58395ee3dfb872c7102 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 8 Dec 2024 15:13:20 -0600 Subject: [PATCH] AP_HAL_ESP32: RCOutput: fix channel enable/disable Enabling/disabling the timer would apply the setting to whole groups of channels. Fix to poke the comparator so that the setting only applies to the particular channel. Conveniently, though not necessarily intentionally, this avoids truncating the output pulse and causing unexpected reactions from servos. This also preserves the old behavior. --- libraries/AP_HAL_ESP32/RCOutput.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index 2c6a7ac530..836a4b1d12 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.cpp +++ b/libraries/AP_HAL_ESP32/RCOutput.cpp @@ -188,7 +188,9 @@ void RCOutput::enable_ch(uint8_t chan) } pwm_out &out = pwm_group_list[chan]; - ESP_ERROR_CHECK(mcpwm_timer_start_stop(out.h_timer, MCPWM_TIMER_START_NO_STOP)); + // set output to high when timer == 0 like normal + ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(out.h_gen, + MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH))); } void RCOutput::disable_ch(uint8_t chan) @@ -199,7 +201,10 @@ void RCOutput::disable_ch(uint8_t chan) write(chan, 0); pwm_out &out = pwm_group_list[chan]; - ESP_ERROR_CHECK(mcpwm_timer_start_stop(out.h_timer, MCPWM_TIMER_STOP_EMPTY)); + // set output to low when timer == 0, so the output is always low (after + // this cycle). conveniently avoids pulse truncation + ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(out.h_gen, + MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_LOW))); } void RCOutput::write(uint8_t chan, uint16_t period_us)