From 9a7da08f4e10d9fbfa79adba2bbf833e065d917e Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 8 Dec 2024 15:13:20 -0600 Subject: [PATCH 1/4] 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 2c6a7ac530743..836a4b1d129c7 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) From da145e0909e78b763b1973a928a4bd37c0391b59 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 8 Dec 2024 14:16:00 -0600 Subject: [PATCH 2/4] AP_HAL_ESP32: RCOutput: appropriately assert PWM array size Avoid over-running the pending PWM and safe value PWM arrays in case future chip revisions come out with more channels. --- libraries/AP_HAL_ESP32/RCOutput.cpp | 4 +++- libraries/AP_HAL_ESP32/RCOutput.h | 5 ++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index 836a4b1d129c7..8654ebd6529b7 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.cpp +++ b/libraries/AP_HAL_ESP32/RCOutput.cpp @@ -50,6 +50,8 @@ gpio_num_t outputs_pins[] = {}; #endif #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]; @@ -324,7 +326,7 @@ void RCOutput::force_safety_off(void) */ void RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us) { - for (uint8_t i=0; i<16; i++) { + for (uint8_t i=0; i Date: Sun, 8 Dec 2024 13:55:59 -0600 Subject: [PATCH 3/4] 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 | 35 ++-- 2 files changed, 167 insertions(+), 107 deletions(-) diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index 8654ebd6529b7..31cfc321f09d3 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 e2d5610701e1d..cd88b442e451d 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 @@ class RCOutput : public AP_HAL::RCOutput 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; + 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 - mcpwm_timer_handle_t h_timer; - mcpwm_oper_handle_t h_oper; - mcpwm_cmpr_handle_t h_cmpr; - mcpwm_gen_handle_t h_gen; + 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; From 903d2e6bbd9bc726d5bb9a893060fe64bb615ebb Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 8 Dec 2024 16:31:40 -0600 Subject: [PATCH 4/4] AP_HAL_ESP32: RCOutput: add support for brushed PWM mode Also adds some infrastructure for changing PWM group mode, though this is likely the complete set that can be supported with the ESP32 PWM peripheral. --- libraries/AP_HAL_ESP32/RCOutput.cpp | 222 ++++++++++++++++++++++++---- libraries/AP_HAL_ESP32/RCOutput.h | 7 + 2 files changed, 204 insertions(+), 25 deletions(-) diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index 31cfc321f09d3..33b5f36d6c272 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.cpp +++ b/libraries/AP_HAL_ESP32/RCOutput.cpp @@ -67,8 +67,10 @@ gpio_num_t outputs_pins[] = {}; * (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 + * * For "normal" PWM output, running the timer at 1MHz allows 16-1000Hz with + * at least 1000 ticks per cycle and makes assigning the compare value easy + * * For brushed PWM output, running the timer at 40MHz allows 650-32000Hz with + * at least 1000 ticks per cycle and makes an easy divider setting * * MCPWM is only capable of PWM; DMA-based modes will require using the RMT * peripheral. @@ -81,7 +83,10 @@ 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 +// default settings for PWM ("normal") and brushed mode. carefully understand +// the prescaler update logic in set_group_mode before changing resolution! +#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000 // 1MHz, 1000ns per tick, 2x80 prescaler +#define BRUSH_TIMEBASE_RESOLUTION_HZ 40000000 // 40MHz, 25ns per tick, 2x2 prescaler #define SERVO_DEFAULT_FREQ_HZ 50 // the rest of ArduPilot assumes this! @@ -120,31 +125,23 @@ void RCOutput::init() for (int timer_num=0; timer_num SERVO_DEFAULT_FREQ_HZ) { + if (group.rc_frequency > SERVO_DEFAULT_FREQ_HZ || group.current_mode > MODE_PWM_NORMAL) { fast_channel_mask |= group.ch_mask; } } @@ -222,6 +236,138 @@ void RCOutput::set_default_rate(uint16_t freq_hz) } } +/* + setup output mode for a group, using group.current_mode. + */ +void RCOutput::set_group_mode(pwm_group &group) +{ + if (!_initialized) { + return; + } + + // calculate group timer resolution + uint32_t resolution_hz; + switch (group.current_mode) { + case MODE_PWM_BRUSHED: + resolution_hz = BRUSH_TIMEBASE_RESOLUTION_HZ; + break; + + default: + group.current_mode = MODE_PWM_NONE; // treat as 0 output normal + // fallthrough + case MODE_PWM_NONE: + case MODE_PWM_NORMAL: + resolution_hz = SERVO_TIMEBASE_RESOLUTION_HZ; + break; + } + + if (group.current_mode > MODE_PWM_NORMAL) { + fast_channel_mask |= group.ch_mask; + } + + // the timer prescaler is different in normal vs. brushed mode. the only way + // to change it with the SDK is to completely destroy the timer, then + // create a "new" one with the right "resolution" (which is converted + // internally to the prescaler). fortunately we can keep the + // operator/comparators/generators around. also fortunately the SDK + // defaults the MCPWM group prescaler to 2, so we have wiggle room to set + // each timer's prescaler independently without affecting the others. + + // the code to do this was written after experimenting and studying the SDK + // code and chip manual. we hope it's applicable to future versions and + // doesn't create enough output glitches to be a serious problem... + + // if allocated, disable and delete the timer (which, mostly due to hardware + // limitations, doesn't stop it or disconnect it from the operator) + if (group.h_timer) { + ESP_ERROR_CHECK(mcpwm_timer_disable(group.h_timer)); + ESP_ERROR_CHECK(mcpwm_del_timer(group.h_timer)); + } + + // build new timer config with the correct resolution and period + mcpwm_timer_config_t timer_config { + .group_id = group.mcpwm_group_id, + .clk_src = MCPWM_TIMER_CLK_SRC_PLL160M, + .resolution_hz = resolution_hz, + .count_mode = MCPWM_TIMER_COUNT_MODE_UP, + .period_ticks = constrain_freq(group), + }; + + // create a "new" timer with the correct settings for this mode (if already + // allocated this need not reuse the same hardware unit, but likely will) + ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config, &group.h_timer)); + ESP_ERROR_CHECK(mcpwm_timer_enable(group.h_timer)); + + // convincing the hardware unit, if reused, to use the new prescaler value + // is yet another challenge on top of convincing the software to write it + // to the register. quoth the technical reference manual (ESP32S3, Version + // 1.6, section 36.3.2.3), "The moment of updating the clock prescaler’s + // active register is at the time when the timer starts operating.". this + // statement is backed up and enhanced here: + // https://www.esp32.com/viewtopic.php?t=14210#p57277 + + // stop the timer when its value equals 0 (though we don't need to start it) + ESP_ERROR_CHECK(mcpwm_timer_start_stop(group.h_timer, MCPWM_TIMER_STOP_EMPTY)); + + // now use the sync mechanism to force the value to 0 so we don't have to + // wait for it to roll around + + // create a software-activated sync source + mcpwm_sync_handle_t h_sync; + mcpwm_soft_sync_config_t sync_config {}; + ESP_ERROR_CHECK(mcpwm_new_soft_sync_src(&sync_config, &h_sync)); + + // tell the timer to set its value to 0 on sync + mcpwm_timer_sync_phase_config_t timer_sync_config { + .sync_src = h_sync, + .count_value = 0, + .direction = MCPWM_TIMER_DIRECTION_UP, + }; + ESP_ERROR_CHECK(mcpwm_timer_set_phase_on_sync(group.h_timer, &timer_sync_config)); + + // activate the sync and so request the set + ESP_ERROR_CHECK(mcpwm_soft_sync_activate(h_sync)); + + // disconnect the sync source and delete it; that's all we needed it for + timer_sync_config.sync_src = nullptr; // set timer to no source + ESP_ERROR_CHECK(mcpwm_timer_set_phase_on_sync(group.h_timer, &timer_sync_config)); + ESP_ERROR_CHECK(mcpwm_del_sync_src(h_sync)); + + // wait a few timer ticks (at the slowest prescale we use) for the set to + // happen, the timer to stop, and the prescaler to update (these might each + // take a full tick) + hal.scheduler->delay_microseconds(10); + + // now, finally!, start it free-running with the right prescale and period + ESP_ERROR_CHECK(mcpwm_timer_start_stop(group.h_timer, MCPWM_TIMER_START_NO_STOP)); + + // oh, and connect the operator, in case we are using a new timer (it can + // only connect to one timer so this will clear out any old connection) + ESP_ERROR_CHECK(mcpwm_operator_connect_timer(group.h_oper, group.h_timer)); +} + + +void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode) +{ + while (mask) { + uint8_t chan = __builtin_ffs(mask)-1; + if (!_initialized || chan >= MAX_CHANNELS) { + return; + } + + pwm_group &group = *pwm_chan_list[chan].group; + group.current_mode = mode; + set_group_mode(group); + + // acknowledge the setting of any channels sharing this group + for (chan=0; chan= MAX_CHANNELS) { @@ -341,8 +487,34 @@ void RCOutput::write_int(uint8_t chan, uint16_t period_us) 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; + + uint16_t compare_value; + switch(ch.group->current_mode) { + case MODE_PWM_BRUSHED: { + float duty = 0; + if (period_us <= _esc_pwm_min) { + duty = 0; + } else if (period_us >= _esc_pwm_max) { + duty = 1; + } else { + duty = ((float)(period_us - _esc_pwm_min))/(_esc_pwm_max - _esc_pwm_min); + } + compare_value = duty*BRUSH_TIMEBASE_RESOLUTION_HZ/ch.group->rc_frequency; + break; + } + + case MODE_PWM_NORMAL: + compare_value = period_us; + break; + + case MODE_PWM_NONE: + default: + compare_value = 0; + break; + } + + ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(ch.h_cmpr, compare_value)); } /* diff --git a/libraries/AP_HAL_ESP32/RCOutput.h b/libraries/AP_HAL_ESP32/RCOutput.h index cd88b442e451d..1c49c631c69dd 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.h +++ b/libraries/AP_HAL_ESP32/RCOutput.h @@ -55,6 +55,8 @@ class RCOutput : public AP_HAL::RCOutput uint16_t read(uint8_t ch) override; void read(uint16_t* period_us, uint8_t len) override; + void set_output_mode(uint32_t mask, const enum output_mode mode) override; + void cork() override; void push() override; @@ -108,6 +110,7 @@ class RCOutput : public AP_HAL::RCOutput uint32_t rc_frequency; // frequency in Hz uint32_t ch_mask; // mask of channels in this group + enum output_mode current_mode; // output mode (none, normal, brushed) }; struct pwm_chan { @@ -122,6 +125,10 @@ class RCOutput : public AP_HAL::RCOutput uint32_t fast_channel_mask; + uint32_t constrain_freq(pwm_group &group); + + void set_group_mode(pwm_group &group); + void write_int(uint8_t chan, uint16_t period_us); static pwm_group pwm_group_list[];