diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index ade1106b..da6661ca 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -14,7 +14,7 @@ * 6 PWM independent signals per unit * unit(0/1) > timer(0-2) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(A/B) * -* -------------------------------------- Table View ----------------------------- +* -------------------------------------- Table View ----------------------------- * * group | timer | operator | comparator | generator | pwm * -------------------------------------------------------------------------------- @@ -28,31 +28,31 @@ * ------------------------------------- Example 3PWM ------------------------------ * ┌─ comparator 0 - generator 0 -> pwm A * ┌─ operator 0 -| -* | └─ comparator 1 - generator 1 -> pmw B +* | └─ comparator 1 - generator 1 -> pmw B * unit - timer 0-2 -| * 0-1 └─ operator 1 - comparator 0 - generator 0 - pwm C -* +* * ------------------------------------- Example 2PWM ------------------------------ * ┌─ comparator 0 - generator 0 -> pwm A * unit - timer 0-2 - operator 0 -| -* 0-1 └─ comparator 1 - generator 1 -> pmw B +* 0-1 └─ comparator 1 - generator 1 -> pmw B * -* -------------------------------------- Example 4PWM ----------------------------- +* -------------------------------------- Example 4PWM ----------------------------- * ┌─ comparator 0 - generator 0 -> pwm A * ┌─ operator 0 -| -* | └─ comparator 1 - generator 1 -> pmw B -* unit - timer 0-2 -| +* | └─ comparator 1 - generator 1 -> pmw B +* unit - timer 0-2 -| * 0-1 | ┌─ comparator 0 - generator 0 -> pwm C * └─ operator 1 -| -* └─ comparator 0 - generator 0 -> pwm D +* └─ comparator 0 - generator 0 -> pwm D * Complementary mode * ------------------ * - : 3 pairs of complementary PWM signals per unit * unit(0/1) > timer(0) > operator(0-2) > comparator(0-1) > generator(0-1) > pwm(high/low pair) -* -* -------------------------------------- Table View ----------------------------- +* +* -------------------------------------- Table View ----------------------------- * * group | timer | operator | comparator | generator | pwm * ------------------------------------------------------------------------ @@ -63,12 +63,12 @@ * 0-1 | 0 | 2 | 0 | 0 | A * 0-1 | 0 | 2 | 1 | 1 | B * -* -------------------------------------- Example 6PWM ----------------------------- -* +* -------------------------------------- Example 6PWM ----------------------------- +* * ┌─ comparator 0 - generator 0 -> pwm A_h -* ┌─ operator 0 -| +* ┌─ operator 0 -| * | └─ comparator 1 - generator 1 -> pmw A_l -* | +* | * unit | ┌─ comparator 0 - generator 0 -> pwm B_h * (group) - timer 0 -|- operator 1 -| * 0-1 | └─ comparator 1 - generator 1 -> pmw B_l @@ -76,13 +76,13 @@ * | ┌─ comparator 0 - generator 0 -> pwm C_h * └─ operator 2 -| * └─ comparator 1 - generator 1 -> pmw C_l -* +* * More info * ---------- * - timers can be associated with any operator, and multiple operators can be associated with the same timer -* - comparators can be associated with any operator +* - comparators can be associated with any operator * - two comparators per operator for independent mode * - one comparator per operator for complementary mode * - generators can be associated with any comparator @@ -96,7 +96,7 @@ * and here: // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/migration-guides/release-5.x/5.0/peripherals.html */ -#include "../../hardware_api.h" +#include "../../hardware_api.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) @@ -114,7 +114,7 @@ // MCPWM driver hardware timer pointers mcpwm_timer_handle_t timers[2][3] = {NULL}; -// MCPWM timer periods configured (directly related to the pwm frequency) +// MCPWM timer periods configured (directly related to the pwm frequency) uint32_t pwm_periods[2][3]; // how many pins from the groups 6 pins is used uint8_t group_pins_used[2] = {0}; @@ -143,7 +143,7 @@ uint8_t _findLastTimer(int group){ // return the last index return i; } -// returns the index of the next timer to instantiate +// returns the index of the next timer to instantiate // -1 if no timers available uint8_t _findNextTimer(int group){ int i = 0; @@ -157,7 +157,7 @@ uint8_t _findNextTimer(int group){ /* * find the best group for the pins - * if 6pwm + * if 6pwm * - Only option is an an empty group * if 3pwm * - Best is an empty group (we can set a pwm frequency) @@ -180,11 +180,11 @@ uint8_t _findNextTimer(int group){ * For example if the group has already used 3pwms, there is one generator that has one pwm channel left. * If we use this channel we have to use the same timer it has been used with before, so we cannot change the pwm frequency. * Current implementation does use the remaining channel only if there isn't other options that would allow changing the pwm frequency. - * In this example where we have 3pwms already configured, if we try to configure 2pws after, we will skip the remaining channel + * In this example where we have 3pwms already configured, if we try to configure 2pws after, we will skip the remaining channel * and use a new timer and operator to allow changing the pwm frequency. In such cases we loose (cannot be used) the remaining channel. * TODO: use the pwm_frequency to avoid skipping pwm channels ! * - * returns + * returns * - 1 if solution found in one group * - 2 if solution requires using both groups * - 0 if no solution possible @@ -199,13 +199,13 @@ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ } } - // if 3 or 1pwm + // if 3 or 1pwm // check if there is available space in one of the groups // otherwise fail if(no_pins == 3 || no_pins==1){ - // second best option is if there is a group with - // pair number of pwms available as we can then - // set the pwm frequency + // second best option is if there is a group with + // pair number of pwms available as we can then + // set the pwm frequency for(int i=0; imcpwm_period = pwm_periods[mcpwm_group][timer_no]; uint8_t no_operators = 3; // use 3 comparators one per pair of pwms - SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " operators."); mcpwm_operator_config_t operator_config = { .group_id = mcpwm_group }; operator_config.intr_priority = 0; operator_config.flags.update_gen_action_on_tep = true; @@ -313,9 +313,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, } #if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) - + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with hardware dead-time"); - + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; @@ -324,7 +324,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_comparator(params->oper[i], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - + #else // software dead-time (software 6pwm) // software dead-time (software 6pwm) SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); @@ -339,7 +339,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_comparator(params->oper[oper_index], &comparator_config, ¶ms->comparator[i]),"Could not create comparator: " + String(i)); CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } -#endif +#endif int no_generators = 6; // one per pwm SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); @@ -355,9 +355,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, #if SIMPLEFOC_ESP32_HW_DEADTIME == true // hardware dead-time (hardware 6pwm) for (int i = 0; i < no_operators; i++) { - CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[i]), "Failed to configure high-side center align pwm: " + String(2*i)); - CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[i]), "Failed to configure low-side center align pwm: " + String(2*i+1)); - + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[i]), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[i]), "Failed to configure low-side center align pwm: " + String(2*i+1)); + } // only available for 6pwm SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); @@ -369,15 +369,15 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, mcpwm_dead_time_config_t dt_config_low; dt_config_low.posedge_delay_ticks = 0; dt_config_low.negedge_delay_ticks = dead_time; - dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; + dt_config_low.flags.invert_output = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i], &dt_config_high),"Could not set dead time for generator: " + String(i)); CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i+1], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); } #else // software dead-time (software 6pwm) for (int i = 0; i < 3; i++) { - CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH), "Failed to configure high-side center align pwm: " + String(2*i)); - CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH) , "Failed to configure low-side center align pwm: " + String(2*i+1)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH), "Failed to configure high-side center align pwm: " + String(2*i)); + CHECK_ERR(_configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH) , "Failed to configure low-side center align pwm: " + String(2*i+1)); } #endif @@ -412,7 +412,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int .pwm_frequency = pwm_frequency, .group_id = mcpwm_group }; - + bool shared_timer = false; // check if timer is configured if (timers[mcpwm_group][timer_no] == NULL){ @@ -420,8 +420,8 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int pwm_config.group_id = mcpwm_group; pwm_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; pwm_config.resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ; - pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; - pwm_config.intr_priority = 0; + pwm_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN; + pwm_config.intr_priority = 0; pwm_config.period_ticks = _calcPWMPeriod(pwm_frequency); #ifdef ESP_IDF_VERSION_ABOVE_5_4_0 pwm_config.flags.allow_pd = 0; @@ -430,10 +430,10 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); // save variables for later pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; - params->timers[0] = timers[mcpwm_group][timer_no]; + params->timers[0] = timers[mcpwm_group][timer_no]; // if the numer of used channels it not pair skip one channel // the skipped channel cannot be used with the new timer - // TODO avoid loosing channels like this + // TODO avoid loosing channels like this if(group_pins_used[mcpwm_group] %2) group_pins_used[mcpwm_group]++; }else{ // we will use an already instantiated timer @@ -446,7 +446,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int return SIMPLEFOC_DRIVER_INIT_FAILED; } CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); - + shared_timer = true; } @@ -463,7 +463,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); CHECK_ERR(mcpwm_operator_connect_timer(params->oper[i], params->timers[0]),"Could not connect timer to operator: " + String(i)); - } + } // save the last operator in this group last_operator[mcpwm_group] = params->oper[no_operators - 1]; @@ -485,7 +485,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i) +String(" on pin: ")+String(pins[i])); } - + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < no_pins; i++) { @@ -507,7 +507,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } // function setting the duty cycle to the MCPWM pin -void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ +void IRAM_ATTR _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle){ float duty = _constrain(duty_cycle, 0.0, 1.0); mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); } @@ -515,7 +515,7 @@ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_c // function setting the duty cycle to the MCPWM pin void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state){ // phase state is forced in hardware pwm mode - // esp-idf docs: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // esp-idf docs: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions // github issue: https://github.com/espressif/esp-idf/issues/12237 mcpwm_generator_set_force_level(generator_high, (phase_state == PHASE_ON || phase_state == PHASE_HI) ? -1 : 0, true); mcpwm_generator_set_force_level(generator_low, (phase_state == PHASE_ON || phase_state == PHASE_LO) ? -1 : 1, true); diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index d7aff79c..bf89fe6a 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -9,7 +9,7 @@ #include "driver/ledc.h" #include "esp_idf_version.h" - + // version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x #if ESP_IDF_VERSION < ESP_IDF_VERSION_VAL(5, 0, 0) #error SimpleFOC: ESP-IDF version 4 or lower detected. Please update to ESP-IDF 5.x and Arduino-esp32 3.0 (or higher) @@ -41,7 +41,7 @@ // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -// channels from 0-7 are in group 0 and 8-15 in group 1 +// channels from 0-7 are in group 0 and 8-15 in group 1 // - only esp32 as of mid 2024 has the second group, all the s versions don't int group_channels_used[2] = {0}; @@ -72,7 +72,7 @@ int esp32_gpio_nr(int pin) { - inverted - output inverted - group - ledc group - This function is a workaround for the ledcAttachPin function that is not available in the ESP32 Arduino core, in which the + This function is a workaround for the ledcAttachPin function that is not available in the ESP32 Arduino core, in which the PWM signals are synchronized in pairs, while the simplefoc requires a bit more flexible configuration. This function sets also allows configuring a channel as inverted, which is not possible with the ledcAttachPin function. @@ -105,7 +105,7 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t ledc_channel_config_t ledc_channel; ledc_channel.speed_mode = group; ledc_channel.channel = channel; - ledc_channel.timer_sel = LEDC_TIMER_0; + ledc_channel.timer_sel = LEDC_TIMER_0; ledc_channel.intr_type = LEDC_INTR_DISABLE; ledc_channel.gpio_num = esp32_gpio_nr(pin); ledc_channel.duty = duty; @@ -129,9 +129,9 @@ int _availableGroupChannels(int group){ // returns the number of the group that has enough channels available // returns -1 if no group has enough channels -// +// // NOT IMPLEMENTED BUT COULD BE USEFUL -// returns 2 if no group has enough channels but combined they do +// returns 2 if no group has enough channels but combined they do int _findGroupWithChannelsAvailable(int no_channels){ if(no_channels <= _availableGroupChannels(0)) return 0; if(no_channels <= _availableGroupChannels(1)) return 1; @@ -158,8 +158,8 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pinA); return SIMPLEFOC_DRIVER_INIT_FAILED; } - - + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { .channels = { static_cast(group_channels_used[group]) }, .groups = { (ledc_mode_t)group }, @@ -203,7 +203,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } - + params->channels[i] = static_cast(group_channels_used[group]); params->groups[i] = (ledc_mode_t)group; } @@ -239,7 +239,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); return SIMPLEFOC_DRIVER_INIT_FAILED; } - + params->channels[i] = static_cast(group_channels_used[group]); params->groups[i] = (ledc_mode_t)group; group_channels_used[group]++; @@ -301,25 +301,25 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in } -void _writeDutyCycle(float dc, void* params, int index){ +void IRAM_ATTR _writeDutyCycle(float dc, void* params, int index){ ledc_set_duty_with_hpoint(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index], _PWM_RES*dc, _PWM_RES/2.0*(1.0-dc)); ledc_update_duty(((ESP32LEDCDriverParams*)params)->groups[index],((ESP32LEDCDriverParams*)params)->channels[index]); } -void _writeDutyCycle1PWM(float dc_a, void* params){ +void IRAM_ATTR _writeDutyCycle1PWM(float dc_a, void* params){ _writeDutyCycle(dc_a, params, 0); } -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ +void IRAM_ATTR _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ _writeDutyCycle(dc_a, params, 0); _writeDutyCycle(dc_b, params, 1); } -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ +void IRAM_ATTR _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ _writeDutyCycle(dc_a, params, 0); _writeDutyCycle(dc_b, params, 1); _writeDutyCycle(dc_c, params, 2); @@ -327,7 +327,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ +void IRAM_ATTR _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ _writeDutyCycle(dc_1a, params, 0); _writeDutyCycle(dc_1b, params, 1); _writeDutyCycle(dc_2a, params, 2); @@ -370,7 +370,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); return SIMPLEFOC_DRIVER_INIT_FAILED; } - + params->channels[2*i] = static_cast(group_channels_used[group]); params->groups[2*i] = (ledc_mode_t)group; @@ -379,18 +379,18 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pin_pairs[i][0]); return SIMPLEFOC_DRIVER_INIT_FAILED; } - + params->channels[2*i+1] = static_cast(group_channels_used[group]); params->groups[2*i+1] = (ledc_mode_t)group; } - + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup successful in group: ", (group)); return params; } -void _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float dead_time, PhaseState ps){ - float pwm_h = _constrain(val - dead_time/2.0, 0, 1.0); - float pwm_l = _constrain(val + dead_time/2.0, 0, 1.0); +void IRAM_ATTR _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float dead_time, PhaseState ps){ + float pwm_h = _constrain(val - (dead_time * 0.5), 0, 1.0); + float pwm_l = _constrain(val + (dead_time * 0.5), 0, 1.0); // determine the phase state and set the pwm accordingly // deactivate phases if needed @@ -406,7 +406,7 @@ void _setPwmPairDutyCycle( void* params, int ind_h, int ind_l, float val, float } } -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ +void IRAM_ATTR _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ _setPwmPairDutyCycle(params, 0, 1, dc_a, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[0]); _setPwmPairDutyCycle(params, 2, 3, dc_b, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[1]); _setPwmPairDutyCycle(params, 4, 5, dc_c, ((ESP32LEDCDriverParams*)params)->dead_zone, phase_state[2]); diff --git a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 2b18417f..e251ab84 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -1,7 +1,7 @@ #include "esp32_driver_mcpwm.h" #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - + #pragma message("") #pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") #pragma message("") @@ -34,7 +34,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); } - + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware specific @@ -178,7 +178,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ +void IRAM_ATTR _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_c); @@ -187,14 +187,14 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ // function setting the pwm duty cycle to the hardware // - DCMotor -1PWM setting // - hardware specific -void _writeDutyCycle1PWM(float dc_a, void* params){ +void IRAM_ATTR _writeDutyCycle1PWM(float dc_a, void* params){ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware specific -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ +void IRAM_ATTR _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); } @@ -204,7 +204,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware specific -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ +void IRAM_ATTR _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // se the PWM on the slot timers _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1a); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_1b); @@ -212,7 +212,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2b); } -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ +void IRAM_ATTR _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ #if SIMPLEFOC_ESP32_HW_DEADTIME == true _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); @@ -224,7 +224,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[4], ((ESP32MCPWMDriverParams*)params)->generator[5], phase_state[2]); #else uint32_t period = ((ESP32MCPWMDriverParams*)params)->mcpwm_period; - float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone /2.0f; + const float dead_zone = (float)((ESP32MCPWMDriverParams*)params)->dead_zone * 0.5f; _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? dc_a-dead_zone : 0.0f); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], period, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? dc_a+dead_zone : 1.0f); _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? dc_b-dead_zone : 0.0f);