From 5d4187a3eb18a7e8eb3586115dd6a3b1e3571136 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 16 Jun 2024 22:54:19 +0200 Subject: [PATCH] added the current sensing + phase state for driver --- src/current_sense/LowsideCurrentSense.cpp | 3 +- src/current_sense/hardware_api.h | 5 +- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32_adc_driver.h | 2 +- .../esp32/esp32_mcpwm_mcu.cpp | 189 ++++++++++++++++++ .../hardware_specific/esp32/esp32_mcu.cpp | 165 --------------- .../esp32/esp32s_adc_driver.cpp | 2 +- .../hardware_specific/generic_mcu.cpp | 4 +- .../hardware_specific/rp2040/rp2040_mcu.cpp | 2 +- .../hardware_specific/samd/samd21_mcu.cpp | 5 +- .../stm32/b_g431/b_g431_mcu.cpp | 6 +- .../stm32/stm32f1/stm32f1_mcu.cpp | 7 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 7 +- .../stm32/stm32f7/stm32f7_mcu.cpp | 7 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 7 +- .../stm32/stm32l4/stm32l4_mcu.cpp | 6 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 7 +- .../esp32/esp32_driver_mcpwm.cpp | 53 +++-- .../esp32/esp32_driver_mcpwm.h | 19 +- .../esp32/esp32_ledc_mcu.cpp | 2 +- .../{esp32_mcu.cpp => esp32_mcpwm_mcu.cpp} | 37 ++-- .../hardware_specific/esp32/mcpwm_private.h | 78 ++++++++ 22 files changed, 390 insertions(+), 225 deletions(-) create mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp delete mode 100644 src/current_sense/hardware_specific/esp32/esp32_mcu.cpp rename src/drivers/hardware_specific/esp32/{esp32_mcu.cpp => esp32_mcpwm_mcu.cpp} (84%) create mode 100644 src/drivers/hardware_specific/esp32/mcpwm_private.h diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index aeb8dea0..9b07d353 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -47,7 +47,8 @@ int LowsideCurrentSense::init(){ // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // sync the driver - _driverSyncLowSide(driver->params, params); + void* r = _driverSyncLowSide(driver->params, params); + if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets calibrateOffsets(); // set the initialized flag diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7862b708..d1f5f160 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -59,7 +59,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params); * function syncing the Driver with the ADC for the LowSide Sensing * @param driver_params - driver parameter structure - hardware specific * @param cs_params - current sense parameter structure - hardware specific + * + * @return void* - returns the pointer to the current sense parameter structure (unchanged) + * - returns SIMPLEFOC_CURRENT_SENSE_INIT_FAILED if the init fails */ -void _driverSyncLowSide(void* driver_params, void* cs_params); +void* _driverSyncLowSide(void* driver_params, void* cs_params); #endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 4a37ddb8..7f0cc310 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h index 357b35b0..f76c003e 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -5,7 +5,7 @@ #include "Arduino.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) /* * Get ADC value for pin * */ diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..6081f87e --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,189 @@ +#include "../../hardware_api.h" +#include "../../../drivers/hardware_api.h" +#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +// check the version of the ESP-IDF +#include "esp_idf_version.h" + +#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) +#endif + + +#include "esp32_adc_driver.h" + +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#include +#include + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG +#include "driver/gpio.h" +#endif + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + + + +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("CS", str);\ + +#define CHECK_CS_ERR(func_call, message) \ + if ((func_call) != ESP_OK) { \ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; \ + } + +typedef struct ESP32MCPWMCurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32MCPWMCurrentSenseParams; + + +/** + * Inline adc reading implementation +*/ +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA, const void* cs_params){ + uint32_t raw_adc = adcRead(pinA); + return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; +} + +// function reading an ADC value and returning the read voltage +void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + + return params; +} + + +/** + * Low side adc reading implementation +*/ + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + ESP32MCPWMCurrentSenseParams* p = (ESP32MCPWMCurrentSenseParams*)cs_params; + int no_channel = 0; + for(int i=0; i < 3; i++){ + if(!_isset(p->pins[i])) continue; + if(pin == p->pins[i]) // found in the buffer + return p->adc_buffer[no_channel] * p->adc_voltage_conv; + else no_channel++; + } + // not found + return 0; +} + + +// function configuring low-side current sensing +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + // check if driver timer is already running + // fail if it is + // the easiest way that I've found to check if timer is running + // is to start it and stop it + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams{}; + int no_adc_channels = 0; + if( _isset(pinA) ){ + pinMode(pinA, INPUT); + params->pins[no_adc_channels++] = pinA; + } + if( _isset(pinB) ){ + pinMode(pinB, INPUT); + params->pins[no_adc_channels++] = pinB; + } + if( _isset(pinC) ){ + pinMode(pinC, INPUT); + params->pins[no_adc_channels++] = pinC; + } + + t->user_data = params; + params->adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION); + params->no_adc_channels = no_adc_channels; + return params; +} + +void* _driverSyncLowSide(void* driver_params, void* cs_params){ +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + pinMode(19, OUTPUT); +#endif + ESP32MCPWMDriverParams *p = (ESP32MCPWMDriverParams*)driver_params; + mcpwm_timer_t* t = (mcpwm_timer_t*) p->timers[0]; + + // check if low side callback is already set + // if it is, return error + if(t->on_full != nullptr){ + SIMPLEFOC_ESP32_CS_DEBUG("Low side callback is already set. Cannot set it again for timer: "+String(t->timer_id)+", group: "+String(t->group->group_id)); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // set the callback for the low side current sensing + // mcpwm_timer_event_callbacks_t can be used to set the callback + // for three timer events + // - on_full - low-side + // - on_empty - high-side + // - on_sync - sync event (not used with simplefoc) + auto cbs = mcpwm_timer_event_callbacks_t{ + .on_full = [](mcpwm_timer_handle_t tim, const mcpwm_timer_event_data_t* edata, void* user_data){ + ESP32MCPWMCurrentSenseParams *p = (ESP32MCPWMCurrentSenseParams*)user_data; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + gpio_set_level(GPIO_NUM_19,1); //cca 250ns for on+off +#endif + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // sample the phase currents one at a time + // adc read takes around 10us which is very long + // so we are sampling one phase per call + p->adc_buffer[p->buffer_index] = adcRead(p->pins[p->buffer_index]); +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG + gpio_set_level(GPIO_NUM_19,0); //cca 250ns for on+off +#endif + return true; + }, + }; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupt callback."); + // set the timer state to init (so that we can call the `mcpwm_timer_register_event_callbacks` ) + // this is a hack, as this function is not supposed to be called when the timer is running + // the timer does not really go to the init state! + t->fsm = MCPWM_TIMER_FSM_INIT; + // set the callback + CHECK_CS_ERR(mcpwm_timer_register_event_callbacks(t, &cbs, cs_params), "Failed to set low side callback"); + // set the timer state to enabled again + t->fsm = MCPWM_TIMER_FSM_ENABLE; + SIMPLEFOC_ESP32_CS_DEBUG("Timer "+String(t->timer_id)+" enable interrupts."); + CHECK_CS_ERR(esp_intr_enable(t->intr), "Failed to enable low-side interrupts!"); + + return cs_params; +} + + +#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp deleted file mode 100644 index d334008e..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ /dev/null @@ -1,165 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" -#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 - -#include "esp32_adc_driver.h" - -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" - -#include -#include - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - - -typedef struct ESP32MCPWMCurrentSenseParams { - int pins[3]; - float adc_voltage_conv; - mcpwm_unit_t mcpwm_unit; - int buffer_index; -} ESP32MCPWMCurrentSenseParams; - - -/** - * Inline adc reading implementation -*/ -// function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA, const void* cs_params){ - uint32_t raw_adc = adcRead(pinA); - return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; -} - -// function reading an ADC value and returning the read voltage -void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){ - _UNUSED(driver_params); - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - - - -/** - * Low side adc reading implementation -*/ - -static void IRAM_ATTR mcpwm0_isr_handler(void*); -static void IRAM_ATTR mcpwm1_isr_handler(void*); -byte currentState = 1; -// two mcpwm units -// - max 2 motors per mcpwm unit (6 adc channels) -int adc_pins[2][6]={0}; -int adc_pin_count[2]={0}; -uint32_t adc_buffer[2][6]={0}; -int adc_read_index[2]={0}; - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin, const void* cs_params){ - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; - int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; - float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; - - for(int i=0; i < adc_pin_count[unit]; i++){ - if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_buffer[unit][buffer_index + i] * adc_voltage_conv; - } - // not found - return 0; -} - -// function configuring low-side current sensing -void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - - mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - int index_start = adc_pin_count[unit]; - if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA; - if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB; - if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC; - - if( _isset(pinA) ) pinMode(pinA, INPUT); - if( _isset(pinB) ) pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION), - .mcpwm_unit = unit, - .buffer_index = index_start - }; - - return params; -} - - -void _driverSyncLowSide(void* driver_params, void* cs_params){ - - mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev; - mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit; - - // low-side register enable interrupt - mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt - // high side registers enable interrupt - //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt - - // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL) - if(mcpwm_unit == MCPWM_UNIT_0) - mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler - else - mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler -} - -static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm0_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]); - adc_read_index[0]++; - if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0; - } - // low side - MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - -static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused)); - -// Read currents when interrupt is triggered -static void IRAM_ATTR mcpwm1_isr_handler(void*){ - // // high side - // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st; - - // low side - uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st; - if(mcpwm_intr_status){ - adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]); - adc_read_index[1]++; - if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0; - } - // low side - MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status; - // high side - // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0; -} - - -#endif diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp index 31cbb027..a212d57b 100644 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) && 0 +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) && !defined(SIMPLEFOC_ESP32_USELEDC) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dec7205d..58545b17 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -34,8 +34,8 @@ __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, con } // sync driver and the adc -__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){ +__attribute__((weak)) void* _driverSyncLowSide(void* driver_params, void* cs_params){ _UNUSED(driver_params); - _UNUSED(cs_params); + return cs_params; } __attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index d84c2fd5..d2ed881b 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -86,7 +86,7 @@ void* _configureADCInline(const void *driver_params, const int pinA, const int p // }; -// void _driverSyncLowSide(void* driver_params, void* cs_params) { +// void* _driverSyncLowSide(void* driver_params, void* cs_params) { // // nothing to do // }; diff --git a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp index da5ba85b..046f3db4 100644 --- a/src/current_sense/hardware_specific/samd/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd/samd21_mcu.cpp @@ -54,14 +54,15 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params) /** * function syncing the Driver with the ADC for the LowSide Sensing */ -void _driverSyncLowSide(void* driver_params, void* cs_params) +void* _driverSyncLowSide(void* driver_params, void* cs_params) { _UNUSED(driver_params); - _UNUSED(cs_params); SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); instance.startADCScan(); //TODO: hook with PWM interrupts + + return cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 8456759c..8cfda6d7 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -148,7 +148,7 @@ void DMA1_Channel2_IRQHandler(void) { } } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -169,6 +169,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 5f090c20..327c5305 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -48,7 +48,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -97,6 +97,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 6388e8e0..21ac7555 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -40,7 +40,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -86,6 +86,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp index 3040ea46..458ec638 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -34,7 +34,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -69,6 +69,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index fb32d36b..7f9fd371 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -42,7 +42,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -123,6 +123,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index 1fb0ab6b..e5c9470f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -41,7 +41,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int } -void _driverSyncLowSide(void* _driver_params, void* _cs_params){ +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; @@ -120,6 +120,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver _startTimers(driver_params->timers, 6); + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; } diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 70815d0f..3a542b53 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -197,7 +197,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p } // sync driver and the adc -void _driverSyncLowSide(void* driver_params, void* cs_params){ +void* _driverSyncLowSide(void* driver_params, void* cs_params){ Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; @@ -238,6 +238,11 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; #endif + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return cs_params; } diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index 87f78788..b76d17c5 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -245,7 +245,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; uint8_t no_operators = 3; // use 3 comparators one per pair of pwms - SIMPLEFOC_ESP32_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 }; for (int i = 0; i < no_operators; i++) { CHECK_ERR(mcpwm_new_operator(&operator_config, ¶ms->oper[i]),"Could not create operator "+String(i)); @@ -254,9 +254,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_DEBUG("Configuring 6PWM with hardware dead-time"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with hardware dead-time"); - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_operators) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_operators) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_operators; i++) { @@ -265,7 +265,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, } int no_generators = 6; // one per pwm - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_generators) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_generators) + " generators."); // Create and configure generators mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_generators; i++) { @@ -274,12 +274,12 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring Center-Aligned 6 pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring Center-Aligned 6 pwm."); for (int i = 0; i < 3; i++) { _configureCenterAlign(params->generator[2*i],params->comparator[i]); } // only available for 6pwm - SIMPLEFOC_ESP32_DEBUG("Configuring dead-time."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring dead-time."); uint32_t dead_time = (int)pwm_periods[mcpwm_group][timer_no] * dead_zone; mcpwm_dead_time_config_t dt_config_high; dt_config_high.posedge_delay_ticks = dead_time; @@ -294,9 +294,9 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_generator_set_dead_time(params->generator[2*i], params->generator[2*i+1], &dt_config_low),"Could not set dead time for generator: " + String(i+1)); } #else // software dead-time (software 6pwm) - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM with software dead-time"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM with software dead-time"); int no_pins = 6; - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_pins; i++) { @@ -305,7 +305,7 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); // Create and configure generators; mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_pins; i++) { @@ -314,19 +314,19 @@ void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, CHECK_ERR(mcpwm_new_generator(params->oper[oper_index], &generator_config, ¶ms->generator[i]), "Could not create generator " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < 3; i++) { _configureCenterAlign(params->generator[2*i],params->comparator[2*i], !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH); _configureCenterAlign(params->generator[2*i+1],params->comparator[2*i+1], SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); } #endif - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); - SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); params->dead_zone = dead_zone; // save the configuration variables for later group_pins_used[mcpwm_group] = 6; @@ -374,11 +374,11 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int }else{ // we will use an already instantiated timer params->timers[0] = timers[mcpwm_group][timer_no]; - SIMPLEFOC_ESP32_DEBUG("Using previously configured timer: " + String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Using previously configured timer: " + String(timer_no)); // but we cannot change its configuration without affecting the other drivers // so let's first verify that the configuration is the same if(_calcPWMPeriod(pwm_frequency)/2 != pwm_periods[mcpwm_group][timer_no]){ - SIMPLEFOC_ESP32_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); + SIMPLEFOC_ESP32_DRV_DEBUG("ERR: Timer: "+String(timer_no)+" is confgured for freq: "+String(_calcPWMFreq(pwm_periods[mcpwm_group][timer_no]))+", not for freq:" +String(pwm_frequency)); return SIMPLEFOC_DRIVER_INIT_FAILED; } CHECK_ERR(mcpwm_timer_start_stop( params->timers[0], MCPWM_TIMER_STOP_EMPTY), "Failed to stop the timer!"); @@ -387,7 +387,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } uint8_t no_operators = ceil(no_pins / 2.0); - SIMPLEFOC_ESP32_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 }; for (int i = 0; i < no_operators; i++) { if (shared_timer && i == 0) { // first operator already configured @@ -400,7 +400,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int // save the last operator in this group last_operator[mcpwm_group] = params->oper[no_operators - 1]; - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " comparators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " comparators."); // Create and configure comparators mcpwm_comparator_config_t comparator_config = {0}; for (int i = 0; i < no_pins; i++) { @@ -409,7 +409,7 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int CHECK_ERR(mcpwm_comparator_set_compare_value(params->comparator[i], (0)), "Could not set duty on comparator: " + String(i)); } - SIMPLEFOC_ESP32_DEBUG("Configuring " + String(no_pins) + " generators."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring " + String(no_pins) + " generators."); // Create and configure generators; mcpwm_generator_config_t generator_config = {}; for (int i = 0; i < no_pins; i++) { @@ -419,19 +419,19 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int } - SIMPLEFOC_ESP32_DEBUG("Configuring center-aligned pwm."); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring center-aligned pwm."); for (int i = 0; i < no_pins; i++) { _configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH); } - SIMPLEFOC_ESP32_DEBUG("Enabling the timer: "+String(timer_no)); + SIMPLEFOC_ESP32_DRV_DEBUG("Enabling timer: "+String(timer_no)); // Enable and start timer if not shared if (!shared_timer) CHECK_ERR(mcpwm_timer_enable(params->timers[0]), "Failed to enable timer!"); // start the timer CHECK_ERR(mcpwm_timer_start_stop(params->timers[0], MCPWM_TIMER_START_NO_STOP), "Failed to start the timer!"); _delay(1); - SIMPLEFOC_ESP32_DEBUG("MCPWM configured!"); + SIMPLEFOC_ESP32_DRV_DEBUG("MCPWM configured!"); // save the configuration variables for later params->mcpwm_period = pwm_periods[mcpwm_group][timer_no]; group_pins_used[mcpwm_group] += no_pins; @@ -440,8 +440,17 @@ 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){ - float duty = constrain(duty_cycle, 0.0, 1.0); - mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); + float duty = constrain(duty_cycle, 0.0, 1.0); + mcpwm_comparator_set_compare_value(cmpr, (uint32_t)(mcpwm_period*duty)); +} + +// 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 can be forced + // https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#generator-force-actions + // TODO verify with ACTIVE_HIGH/ACTIVE_LOW flags + 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); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h index d0b7933e..446d2f25 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -11,7 +11,7 @@ #include "esp_idf_version.h" // version check - this mcpwm driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 +#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) #endif @@ -32,14 +32,17 @@ typedef struct ESP32MCPWMDriverParams { } ESP32MCPWMDriverParams; -#define SIMPLEFOC_ESP32_DEBUG(str)\ - SimpleFOCDebug::println( "ESP32-DRV: " + String(str));\ +#define SIMPLEFOC_ESP32_DEBUG(tag, str)\ + SimpleFOCDebug::println( "ESP32-"+String(tag)+ ": "+ String(str)); + +#define SIMPLEFOC_ESP32_DRV_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("DRV", str);\ // macro for checking the error of the mcpwm functions // if the function returns an error the function will return SIMPLEFOC_DRIVER_INIT_FAILED #define CHECK_ERR(func_call, message) \ if ((func_call) != ESP_OK) { \ - SIMPLEFOC_ESP32_DEBUG("ERROR - " + String(message)); \ + SIMPLEFOC_ESP32_DRV_DEBUG("ERROR - " + String(message)); \ return SIMPLEFOC_DRIVER_INIT_FAILED; \ } @@ -143,5 +146,13 @@ void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int */ void _setDutyCycle(mcpwm_cmpr_handle_t cmpr, uint32_t mcpwm_period, float duty_cycle); +/** + * function setting the phase state + * @param generator_high - mcpwm generator handle for the high side + * @param generator_low - mcpwm generator handle for the low side + * @param phase_state - phase state + */ +void _forcePhaseState(mcpwm_gen_handle_t generator_high, mcpwm_gen_handle_t generator_low, PhaseState phase_state); + #endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index 3c413725..dc667ab3 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -11,7 +11,7 @@ // version check - this ledc driver is specific for ESP-IDF 5.x and arduino-esp32 3.x -#if ESP_IDF_VERSION_MAJOR < 5 +#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) #endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp similarity index 84% rename from src/drivers/hardware_specific/esp32/esp32_mcu.cpp rename to src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index 33f9db20..e2c621c5 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -15,10 +15,10 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { int group, timer; if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 1PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 1PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[1] = {pinA}; return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); @@ -35,17 +35,17 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { int group, timer; int ret = _findBestGroup(2, pwm_frequency, &group, &timer); if(!ret) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 2PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 2PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } if(ret == 1){ // configure the 2pwm on only one group - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[2] = {pinA, pinB}; return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); }else{ - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM as two 1PWM drivers"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); ESP32MCPWMDriverParams* params[2]; // the code is a bit huge for what it does @@ -53,10 +53,10 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { int pins[2][1] = {{pinA}, {pinB}}; for(int i =0; i<2; i++){ int timer = _findLastTimer(i); //find last created timer in group i - SIMPLEFOC_ESP32_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(i)+" on timer: "+String(timer)); void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 1, pins[i]); if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ - SIMPLEFOC_ESP32_DEBUG("Error configuring 1PWM"); + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 1PWM"); return SIMPLEFOC_DRIVER_INIT_FAILED; }else{ params[i] = (ESP32MCPWMDriverParams*)p; @@ -87,10 +87,10 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i int group, timer; if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 3PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 3PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[3] = {pinA, pinB, pinC}; return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); @@ -108,16 +108,16 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in int group, timer; int ret = _findBestGroup(4, pwm_frequency, &group, &timer); if(!ret) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 4PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 4PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } if(ret == 1){ - SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[4] = {pinA, pinB, pinC, pinD}; return _configurePinsMCPWM(pwm_frequency, group, timer, 4, pins); }else{ - SIMPLEFOC_ESP32_DEBUG("Configuring 4PWM as two 2PWM drivers"); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 4PWM as two 2PWM drivers"); ESP32MCPWMDriverParams* params[2]; // the code is a bit huge for what it does @@ -125,10 +125,10 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in int pins[2][2] = {{pinA, pinB},{pinC, pinD}}; for(int i =0; i<2; i++){ int timer = _findNextTimer(i); //find next available timer in group i - SIMPLEFOC_ESP32_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM in group: "+String(i)+" on timer: "+String(timer)); void* p = _configurePinsMCPWM(pwm_frequency, i, timer, 2, pins[i]); if(p == SIMPLEFOC_DRIVER_INIT_FAILED){ - SIMPLEFOC_ESP32_DEBUG("Error configuring 2PWM"); + SIMPLEFOC_ESP32_DRV_DEBUG("Error configuring 2PWM"); return SIMPLEFOC_DRIVER_INIT_FAILED; }else{ params[i] = (ESP32MCPWMDriverParams*)p; @@ -157,10 +157,10 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons int group, timer; if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { - SIMPLEFOC_ESP32_DEBUG("Not enough pins available for 6PWM!"); + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 6PWM!"); return SIMPLEFOC_DRIVER_INIT_FAILED; } - SIMPLEFOC_ESP32_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); + SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 6PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer int pins[6] = {pinA_h,pinA_l, pinB_h, pinB_l, pinC_h, pinC_l}; return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); @@ -207,6 +207,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_ _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); + + // set the phase state + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[0], ((ESP32MCPWMDriverParams*)params)->generator[1], phase_state[0]); + _forcePhaseState(((ESP32MCPWMDriverParams*)params)->generator[2], ((ESP32MCPWMDriverParams*)params)->generator[3], phase_state[1]); + _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; diff --git a/src/drivers/hardware_specific/esp32/mcpwm_private.h b/src/drivers/hardware_specific/esp32/mcpwm_private.h new file mode 100644 index 00000000..e133a710 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -0,0 +1,78 @@ +/* + * This is a private declaration of different MCPWM structures and types. + * It has been copied from: https://github.com/espressif/esp-idf/blob/v5.1.4/components/driver/mcpwm/mcpwm_private.h + * + * extracted by askuric 16.06.2024 + * + * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef MCPWM_PRIVATE_H +#define MCPWM_PRIVATE_H + + +#include "freertos/FreeRTOS.h" +#include "esp_intr_alloc.h" +#include "esp_heap_caps.h" +#include "esp_pm.h" +#include "soc/soc_caps.h" +#include "hal/mcpwm_hal.h" +#include "hal/mcpwm_types.h" +#include "driver/mcpwm_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct mcpwm_group_t mcpwm_group_t; +typedef struct mcpwm_timer_t mcpwm_timer_t; +typedef struct mcpwm_cap_timer_t mcpwm_cap_timer_t; +typedef struct mcpwm_oper_t mcpwm_oper_t; +typedef struct mcpwm_gpio_fault_t mcpwm_gpio_fault_t; +typedef struct mcpwm_gpio_sync_src_t mcpwm_gpio_sync_src_t; +typedef struct mcpwm_timer_sync_src_t mcpwm_timer_sync_src_t; + +struct mcpwm_group_t { + int group_id; // group ID, index from 0 + int intr_priority; // MCPWM interrupt priority + mcpwm_hal_context_t hal; // HAL instance is at group level + portMUX_TYPE spinlock; // group level spinlock + uint32_t prescale; // group prescale + uint32_t resolution_hz; // MCPWM group clock resolution: clock_src_hz / clock_prescale = resolution_hz + esp_pm_lock_handle_t pm_lock; // power management lock + soc_module_clk_t clk_src; // peripheral source clock + mcpwm_cap_timer_t *cap_timer; // mcpwm capture timers + mcpwm_timer_t *timers[SOC_MCPWM_TIMERS_PER_GROUP]; // mcpwm timer array + mcpwm_oper_t *operators[SOC_MCPWM_OPERATORS_PER_GROUP]; // mcpwm operator array + mcpwm_gpio_fault_t *gpio_faults[SOC_MCPWM_GPIO_FAULTS_PER_GROUP]; // mcpwm fault detectors array + mcpwm_gpio_sync_src_t *gpio_sync_srcs[SOC_MCPWM_GPIO_SYNCHROS_PER_GROUP]; // mcpwm gpio sync array +}; + +typedef enum { + MCPWM_TIMER_FSM_INIT, + MCPWM_TIMER_FSM_ENABLE, +} mcpwm_timer_fsm_t; + +struct mcpwm_timer_t { + int timer_id; // timer ID, index from 0 + mcpwm_group_t *group; // which group the timer belongs to + mcpwm_timer_fsm_t fsm; // driver FSM + portMUX_TYPE spinlock; // spin lock + intr_handle_t intr; // interrupt handle + uint32_t resolution_hz; // resolution of the timer + uint32_t peak_ticks; // peak ticks that the timer could reach to + mcpwm_timer_sync_src_t *sync_src; // timer sync_src + mcpwm_timer_count_mode_t count_mode; // count mode + mcpwm_timer_event_cb_t on_full; // callback function when MCPWM timer counts to peak value + mcpwm_timer_event_cb_t on_empty; // callback function when MCPWM timer counts to zero + mcpwm_timer_event_cb_t on_stop; // callback function when MCPWM timer stops + void *user_data; // user data which would be passed to the timer callbacks +}; + +#ifdef __cplusplus +} +#endif + +#endif /* MCPWM_PRIVATE_H */ \ No newline at end of file