diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index 31cfc321f0..33b5f36d6c 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 cd88b442e4..1c49c631c6 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.h +++ b/libraries/AP_HAL_ESP32/RCOutput.h @@ -55,6 +55,8 @@ public: 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 @@ private: 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 @@ private: 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[];