From b30e873c96b1586aeb006f1978dd0a518decf278 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 8 Dec 2024 13:55:59 -0600 Subject: [PATCH] AP_HAL_ESP32: RCOutput: rework to properly support output groups Each of the six available timers now handles two consecutive PWM output channels. This also implements support for changing the group PWM frequency in a similar manner to the ChibiOS HAL. --- libraries/AP_HAL_ESP32/RCOutput.cpp | 239 +++++++++++++++++----------- libraries/AP_HAL_ESP32/RCOutput.h | 37 +++-- 2 files changed, 168 insertions(+), 108 deletions(-) diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index 8654ebd652..31cfc321f0 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.cpp +++ b/libraries/AP_HAL_ESP32/RCOutput.cpp @@ -12,7 +12,8 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt and Andrey "ARg" Romanov + * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt, + * Andrey "ARg" Romanov, and Thomas "tpw_rules" Watson * */ @@ -29,9 +30,6 @@ #include "esp_log.h" -#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1us per tick -#define SERVO_TIMEBASE_PERIOD 20000 // 20000 ticks, 20ms - #define TAG "RCOut" extern const AP_HAL::HAL& hal; @@ -49,16 +47,56 @@ gpio_num_t outputs_pins[] = {}; #endif +/* + * The MCPWM (motor control PWM) peripheral is used to generate PWM signals for + * RC output. It is divided up into the following blocks: + * * The chip has SOC_MCPWM_GROUPS groups + * * Each group has SOC_MCPWM_TIMERS_PER_GROUP timers and operators + * * Each operator has SOC_MCPWM_COMPARATORS_PER_OPERATOR comparators and + * generators + * * Each generator can drive one GPIO pin + * Though there is more possible, we use the following capabilities: + * * Groups have an 8 bit integer prescaler from a 160MHz peripheral clock + * (the prescaler value defaults to 2) + * * Each timer has an 8 bit integer prescaler from the group clock, a 16 bit + * period, and is connected to exactly one operator + * * Each comparator in an operator acts on the corresponding timer's value and + * is connected to exactly one generator which drives exactly one GPIO pin + * + * Each MCPWM group (on ESP32/ESP32S3) gives us 3 independent "PWM groups" + * (in the STM32 sense) which contain 2 GPIO pins. The pins are assigned + * consecutively from the HAL_ESP32_RCOUT list. The frequency of each group can + * be controlled independently by changing that timer's period. + * * Running the timer at 1MHz allows 16-1000Hz with at least 1000 ticks per + * cycle and makes assigning the compare value easy + * + * MCPWM is only capable of PWM; DMA-based modes will require using the RMT + * peripheral. + */ + +// each of our PWM groups has its own timer +#define MAX_GROUPS (SOC_MCPWM_GROUPS*SOC_MCPWM_TIMERS_PER_GROUP) +// we connect one timer to one operator +static_assert(SOC_MCPWM_OPERATORS_PER_GROUP >= SOC_MCPWM_TIMERS_PER_GROUP); +// and one generator to one comparator +static_assert(SOC_MCPWM_GENERATORS_PER_OPERATOR >= SOC_MCPWM_COMPARATORS_PER_OPERATOR); + +#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1us per tick, 2x80 prescaler + +#define SERVO_DEFAULT_FREQ_HZ 50 // the rest of ArduPilot assumes this! + #define MAX_CHANNELS ARRAY_SIZE(outputs_pins) static_assert(MAX_CHANNELS < 12, "overrunning _pending and safe_pwm"); // max for current chips static_assert(MAX_CHANNELS < 32, "overrunning bitfields"); -struct RCOutput::pwm_out RCOutput::pwm_group_list[MAX_CHANNELS]; +#define NEEDED_GROUPS ((MAX_CHANNELS+SOC_MCPWM_COMPARATORS_PER_OPERATOR-1)/SOC_MCPWM_COMPARATORS_PER_OPERATOR) +static_assert(NEEDED_GROUPS <= MAX_GROUPS, "not enough hardware PWM groups"); + +RCOutput::pwm_group RCOutput::pwm_group_list[NEEDED_GROUPS]; +RCOutput::pwm_chan RCOutput::pwm_chan_list[MAX_CHANNELS]; void RCOutput::init() { - _max_channels = MAX_CHANNELS; - #ifdef CONFIG_IDF_TARGET_ESP32 // only on plain esp32 // 32 and 33 are special as they dont default to gpio, but can be if u disable their rtc setup: @@ -70,97 +108,100 @@ void RCOutput::init() printf("RCOutput::init() - channels available: %d \n",(int)MAX_CHANNELS); printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n"); - const int MCPWM_CNT = SOC_MCPWM_OPERATORS_PER_GROUP*SOC_MCPWM_GENERATORS_PER_OPERATOR; + _initialized = true; // assume we are initialized, any error will call abort() - for (int i = 0; i < MAX_CHANNELS; ++i) { + RCOutput::pwm_group *curr_group = &pwm_group_list[0]; + RCOutput::pwm_chan *curr_ch = &pwm_chan_list[0]; + int chan = 0; - mcpwm_timer_handle_t h_timer; - mcpwm_oper_handle_t h_oper; + // loop through all the hardware blocks and set them up. returns when we run + // out of GPIO pins (each of which is assigned in order to a PWM channel) + for (int mcpwm_group_id=0; mcpwm_group_id SERVO_DEFAULT_FREQ_HZ) { + fast_channel_mask |= group.ch_mask; + } } } } @@ -170,17 +211,25 @@ void RCOutput::set_default_rate(uint16_t freq_hz) if (!_initialized) { return; } - set_freq(0xFFFFFFFF, freq_hz); + + for (auto &group : pwm_group_list) { + // only set frequency of groups without fast channels + if (!(group.ch_mask & fast_channel_mask) && group.ch_mask) { + set_freq(group.ch_mask, freq_hz); + // setting a high default frequency mustn't make channels fast + fast_channel_mask &= ~group.ch_mask; + } + } } uint16_t RCOutput::get_freq(uint8_t chan) { if (!_initialized || chan >= MAX_CHANNELS) { - return 50; + return SERVO_DEFAULT_FREQ_HZ; } - pwm_out &out = pwm_group_list[chan]; - return out.freq; + pwm_group &group = *pwm_chan_list[chan].group; + return group.rc_frequency; } void RCOutput::enable_ch(uint8_t chan) @@ -189,9 +238,9 @@ void RCOutput::enable_ch(uint8_t chan) return; } - pwm_out &out = pwm_group_list[chan]; + pwm_chan &ch = pwm_chan_list[chan]; // set output to high when timer == 0 like normal - ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(out.h_gen, + ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(ch.h_gen, MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH))); } @@ -202,10 +251,10 @@ void RCOutput::disable_ch(uint8_t chan) } write(chan, 0); - pwm_out &out = pwm_group_list[chan]; + pwm_chan &ch = pwm_chan_list[chan]; // 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, + ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(ch.h_gen, MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_LOW))); } @@ -230,13 +279,13 @@ uint16_t RCOutput::read(uint8_t chan) return 0; } - pwm_out &out = pwm_group_list[chan]; - return out.value; + pwm_chan &ch = pwm_chan_list[chan]; + return ch.value; } void RCOutput::read(uint16_t *period_us, uint8_t len) { - for (int i = 0; i < MIN(len, _max_channels); i++) { + for (int i = 0; i < MIN(len, MAX_CHANNELS); i++) { period_us[i] = read(i); } } @@ -287,9 +336,13 @@ void RCOutput::write_int(uint8_t chan, uint16_t period_us) period_us = safe_pwm[chan]; } - pwm_out &out = pwm_group_list[chan]; - ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(out.h_cmpr, period_us)); - out.value = period_us; + pwm_chan &ch = pwm_chan_list[chan]; + const uint16_t max_period_us = SERVO_TIMEBASE_RESOLUTION_HZ/SERVO_DEFAULT_FREQ_HZ; + if (period_us > max_period_us) { + period_us = max_period_us; + } + ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(ch.h_cmpr, period_us)); + ch.value = period_us; } /* diff --git a/libraries/AP_HAL_ESP32/RCOutput.h b/libraries/AP_HAL_ESP32/RCOutput.h index e2d5610701..cd88b442e4 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.h +++ b/libraries/AP_HAL_ESP32/RCOutput.h @@ -12,7 +12,8 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . * - * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt and Andrey "ARg" Romanov + * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt, + * Andrey "ARg" Romanov, and Thomas "tpw_rules" Watson' * */ @@ -99,32 +100,38 @@ public: private: - struct pwm_out { + struct pwm_group { + // SDK objects for the group + uint8_t mcpwm_group_id; + mcpwm_timer_handle_t h_timer; + mcpwm_oper_handle_t h_oper; - uint8_t chan; - uint8_t gpio_num; - uint8_t group_id; - int freq; - int value; - - mcpwm_timer_handle_t h_timer; - mcpwm_oper_handle_t h_oper; - mcpwm_cmpr_handle_t h_cmpr; - mcpwm_gen_handle_t h_gen; + uint32_t rc_frequency; // frequency in Hz + uint32_t ch_mask; // mask of channels in this group }; + struct pwm_chan { + // SDK objects for the channel + mcpwm_cmpr_handle_t h_cmpr; + mcpwm_gen_handle_t h_gen; + pwm_group *group; // associated group + + uint8_t gpio_num; // associated GPIO number (always defined) + int value; // output value in microseconds + }; + + uint32_t fast_channel_mask; void write_int(uint8_t chan, uint16_t period_us); - static pwm_out pwm_group_list[]; + static pwm_group pwm_group_list[]; + static pwm_chan pwm_chan_list[]; bool _corked; uint32_t _pending_mask; uint16_t _pending[12]; uint16_t safe_pwm[12]; // pwm to use when safety is on - uint16_t _max_channels; - // safety switch state AP_HAL::Util::safety_state safety_state; uint32_t safety_update_ms;