diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml index e0899bf1..db551b41 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -19,6 +19,7 @@ jobs: - esp32:esp32:esp32doit-devkit-v1 # esp32 - esp32:esp32:esp32s2 # esp32s2 - esp32:esp32:esp32s3 # esp32s3 + - esp32:esp32:esp32c3 # esp32c3 include: @@ -30,6 +31,10 @@ jobs: platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32c3 # esp32c3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index 3bb62bce..4d50b87c 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -38,6 +38,7 @@ void SimpleFOCDebug::println(const __FlashStringHelper* str) { } } + void SimpleFOCDebug::println(const char* str, float val) { if (_debugPrint != NULL) { _debugPrint->print(str); @@ -86,6 +87,20 @@ void SimpleFOCDebug::print(const __FlashStringHelper* str) { } } +void SimpleFOCDebug::print(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->print(str.c_str()); + } +} + + +void SimpleFOCDebug::println(const StringSumHelper str) { + if (_debugPrint != NULL) { + _debugPrint->println(str.c_str()); + } +} + + void SimpleFOCDebug::print(int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 4fcfd538..668e08af 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -33,13 +33,14 @@ **/ -#ifndef SIMPLEFOC_DISABLE_DEBUG +#ifndef SIMPLEFOC_DISABLE_DEBUG class SimpleFOCDebug { public: static void enable(Print* debugPrint = &Serial); static void println(const __FlashStringHelper* msg); + static void println(const StringSumHelper msg); static void println(const char* msg); static void println(const __FlashStringHelper* msg, float val); static void println(const char* msg, float val); @@ -52,6 +53,7 @@ class SimpleFOCDebug { static void print(const char* msg); static void print(const __FlashStringHelper* msg); + static void print(const StringSumHelper msg); static void print(int val); static void print(float val); 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 57ffdfb5..caf29c19 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,89 +1,22 @@ -#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) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} +#include "esp32_mcu.h" +#include "esp32_adc_driver.h" -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#define SIMPLEFOC_ADC_ATTEN ADC_11db +#define SIMPLEFOC_ADC_RES 12 -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits +#include "soc/sens_reg.h" +// configure the ADCs in RTC mode +// saves about 3us per call +// going from 12us to 9us +void __configFastADCs(){ + + // configure both ADCs in RTC mode SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); @@ -100,59 +33,18 @@ void IRAM_ATTR __analogInit(){ SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } -} - -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) - | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) - | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; } -bool IRAM_ATTR __adcStart(uint8_t pin){ +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } + // start teh ADC conversion if(channel > 9){ CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); @@ -162,97 +54,123 @@ bool IRAM_ATTR __adcStart(uint8_t pin){ SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } + if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); + // read the value value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); } - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); + // return value + return value; } -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} +#elif CONFIG_IDF_TARGET_ESP32S2 || CONFIG_IDF_TARGET_ESP32S3 // if esp32 s2 or s3 variants + +#include "soc/sens_reg.h" + + +// configure the ADCs in RTC mode +// no real gain - see if we do something with it later +// void __configFastADCs(){ + +// SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); +// SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); + +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW +// SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + +// CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + +// CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); +// SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); +// SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); +// while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== +// } uint16_t IRAM_ATTR adcRead(uint8_t pin) { int8_t channel = digitalPinToAnalogChannel(pin); if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); return false;//not adc pin } - __analogInit(); - + // start the ADC conversion if(channel > 9){ - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); } uint16_t value = 0; - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + if(channel > 9){ + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); + // read the value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + //wait for conversion + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); + // read teh value + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); } - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); + return value; } +#else // if others just use analogRead + +#pragma message("SimpleFOC: Using analogRead for ADC reading, no fast ADC configuration available!") + +uint16_t IRAM_ATTR adcRead(uint8_t pin){ + return analogRead(pin); +} + +#endif + + +// configure the ADC for the pin +bool IRAM_ATTR adcInit(uint8_t pin){ + static bool initialized = false; + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Not ADC pin: "+String(pin)); + return false;//not adc pin + } + + if(! initialized){ + analogSetAttenuation(SIMPLEFOC_ADC_ATTEN); + analogReadResolution(SIMPLEFOC_ADC_RES); + } + pinMode(pin, ANALOG); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); + analogRead(pin); + +#if CONFIG_IDF_TARGET_ESP32 // if esp32 variant + __configFastADCs(); +#endif + + initialized = true; + return true; +} #endif 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 869c4dde..cad441fc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h @@ -1,88 +1,24 @@ - - #ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ #define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ -#include "Arduino.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) -/* +/** * Get ADC value for pin + * @param pin - pin number + * @return ADC value (0 - 4095) * */ uint16_t adcRead(uint8_t pin); -/* - * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). - * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. - * Range is 1 - 16 - * - * Note: compatibility with Arduino SAM +/** + * Initialize ADC pin + * @param pin - pin number + * + * @return true if success + * false if pin is not an ADC pin */ -void __analogReadResolution(uint8_t bits); - -/* - * Sets the sample bits and read resolution - * Default is 12bit (0 - 4095) - * Range is 9 - 12 - * */ -void __analogSetWidth(uint8_t bits); - -/* - * Set number of cycles per sample - * Default is 8 and seems to do well - * Range is 1 - 255 - * */ -void __analogSetCycles(uint8_t cycles); +bool adcInit(uint8_t pin); -/* - * Set number of samples in the range. - * Default is 1 - * Range is 1 - 255 - * This setting splits the range into - * "samples" pieces, which could look - * like the sensitivity has been multiplied - * that many times - * */ -void __analogSetSamples(uint8_t samples); - -/* - * Set the divider for the ADC clock. - * Default is 1 - * Range is 1 - 255 - * */ -void __analogSetClockDiv(uint8_t clockDiv); - -/* - * Set the attenuation for all channels - * Default is 11db - * */ -void __analogSetAttenuation(uint8_t attenuation); - -/* - * Set the attenuation for particular pin - * Default is 11db - * */ -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); - -/* - * Attach pin to ADC (will also clear any other analog mode that could be on) - * */ -bool __adcAttachPin(uint8_t pin); - -/* - * Start ADC conversion on attached pin's bus - * */ -bool __adcStart(uint8_t pin); - -/* - * Check if conversion on the pin's ADC bus is currently running - * */ -bool __adcBusy(uint8_t pin); - -/* - * Get the result of the conversion (will wait if it have not finished) - * */ -uint16_t __adcEnd(uint8_t pin); #endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ #endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp deleted file mode 100644 index f2d65f8e..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "../../hardware_api.h" -#include "../../../drivers/hardware_api.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED) - -#include "esp32_adc_driver.h" - -#define _ADC_VOLTAGE 3.3f -#define _ADC_RESOLUTION 4095.0f - -// 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); - - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - if( _isset(pinC) ) pinMode(pinC, INPUT); - - GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) - }; - - return params; -} - -#endif 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..33d547db --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,158 @@ +#include "esp32_mcu.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 "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h" +#include "../../../drivers/hardware_specific/esp32/mcpwm_private.h" + +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + + + +// adding a debug toggle pin to measure the time of the interrupt with oscilloscope + +// #define SIMPLEFOC_ESP32_INTERRUPT_DEBUG + +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG +#include "driver/gpio.h" + +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#define DEBUGPIN 16 +#define GPIO_NUM GPIO_NUM_16 +#else +#define DEBUGPIN 19 +#define GPIO_NUM GPIO_NUM_19 +#endif + +#endif + + + +/** + * Low side adc reading implementation +*/ + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + ESP32CurrentSenseParams* p = (ESP32CurrentSenseParams*)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++; + } + SIMPLEFOC_DEBUG("ERROR: ADC pin not found in the buffer!"); + // 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("ERROR: 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; + } + + + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams{}; + int no_adc_channels = 0; + + // initialize the ADC pins + // fail if the pin is not an ADC pin + int adc_pins[3] = {pinA, pinB, pinC}; + for (int i = 0; i < 3; i++){ + if(_isset(adc_pins[i])){ + if(!adcInit(adc_pins[i])){ + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(adc_pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + params->pins[no_adc_channels++] = adc_pins[i]; + } + } + + 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(DEBUGPIN, 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("ERROR: 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){ + ESP32CurrentSenseParams *p = (ESP32CurrentSenseParams*)user_data; +#ifdef SIMPLEFOC_ESP32_INTERRUPT_DEBUG // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,1); //cca 250ns for on+off +#endif + + // sample the phase currents one at a time + // ESP's adc read takes around 10us which is very long + // increment buffer index + p->buffer_index = (p->buffer_index + 1) % p->no_adc_channels; + // 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 // debugging toggle pin to measure the time of the interrupt with oscilloscope + gpio_set_level(GPIO_NUM,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 index 2057463c..e5ed3fbf 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -1,29 +1,6 @@ -#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) - -#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; +#include "esp32_mcu.h" +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) /** * Inline adc reading implementation @@ -31,135 +8,31 @@ typedef struct ESP32MCPWMCurrentSenseParams { // 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; + return raw_adc * ((ESP32CurrentSenseParams*)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 { + ESP32CurrentSenseParams* params = new ESP32CurrentSenseParams { .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; + // initialize the ADC pins + // fail if the pin is not an ADC pin + for (int i = 0; i < 3; i++){ + if(_isset(params->pins[i])){ + pinMode(params->pins[i], ANALOG); + if(!adcInit(params->pins[i])) { + SIMPLEFOC_ESP32_CS_DEBUG("ERROR: Failed to initialise ADC pin: "+String(params->pins[i]) + String(", maybe not an ADC pin?")); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } } - // 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/esp32_mcu.h b/src/current_sense/hardware_specific/esp32/esp32_mcu.h new file mode 100644 index 00000000..9207fb6a --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.h @@ -0,0 +1,37 @@ +#ifndef ESP32_MCU_CURRENT_SENSING_H +#define ESP32_MCU_CURRENT_SENSING_H + +#include "../../hardware_api.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) + + +#include "../../../drivers/hardware_api.h" +#include "esp32_adc_driver.h" + + +// esp32 current sense parameters +typedef struct ESP32CurrentSenseParams { + int pins[3]; + float adc_voltage_conv; + int adc_buffer[3] = {}; + int buffer_index = 0; + int no_adc_channels = 0; +} ESP32CurrentSenseParams; + +// macros for debugging wuing the simplefoc debug system +#define SIMPLEFOC_ESP32_CS_DEBUG(str)\ + SimpleFOCDebug::println( "ESP32-CS: "+ String(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; \ + } + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4095.0f + +#endif // ESP_H && ARDUINO_ARCH_ESP32 +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp deleted file mode 100644 index a2f58ac3..00000000 --- a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp +++ /dev/null @@ -1,258 +0,0 @@ -#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)) - -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "rom/ets_sys.h" -#include "esp_attr.h" -//#include "esp_intr.h" // deprecated -#include "esp_intr_alloc.h" -#include "soc/rtc_io_reg.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/sens_reg.h" - -static uint8_t __analogAttenuation = 3;//11db -static uint8_t __analogWidth = 3;//12 bits -static uint8_t __analogCycles = 8; -static uint8_t __analogSamples = 0;//1 sample -static uint8_t __analogClockDiv = 1; - -// Width of returned answer () -static uint8_t __analogReturnedWidth = 12; - -void __analogSetWidth(uint8_t bits){ - if(bits < 9){ - bits = 9; - } else if(bits > 12){ - bits = 12; - } - __analogReturnedWidth = bits; - __analogWidth = bits - 9; - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); - - // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); -} - -void __analogSetCycles(uint8_t cycles){ - __analogCycles = cycles; - // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); - // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); -} - -void __analogSetSamples(uint8_t samples){ - if(!samples){ - return; - } - __analogSamples = samples - 1; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); -} - -void __analogSetClockDiv(uint8_t clockDiv){ - if(!clockDiv){ - return; - } - __analogClockDiv = clockDiv; - SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); - SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); -} - -void __analogSetAttenuation(uint8_t attenuation) -{ - __analogAttenuation = attenuation & 3; - uint32_t att_data = 0; - int i = 10; - while(i--){ - att_data |= __analogAttenuation << (i * 2); - } - WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels - WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); -} - -void IRAM_ATTR __analogInit(){ - static bool initialized = false; - if(initialized){ - return; - } - - __analogSetAttenuation(__analogAttenuation); - __analogSetCycles(__analogCycles); - __analogSetSamples(__analogSamples + 1);//in samples - __analogSetClockDiv(__analogClockDiv); - __analogSetWidth(__analogWidth + 9);//in bits - - SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); - SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); - - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW - - CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 - - CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); - SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); - SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); - while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== - - initialized = true; -} - -void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0 || attenuation > 3){ - return ; - } - __analogInit(); - if(channel > 7){ - SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); - } else { - SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); - } -} - -bool IRAM_ATTR __adcAttachPin(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - int8_t pad = digitalPinToTouchChannel(pin); - if(pad >= 0){ - uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_CONF_REG); - if(touch & (1 << pad)){ - touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S))); - WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_REG, touch); - } - } else if(pin == 25){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 - } else if(pin == 26){ - CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 - } - - pinMode(pin, ANALOG); - - __analogInit(); - return true; -} - -bool IRAM_ATTR __adcStart(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 9){ - channel -= 10; - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - return true; -} - -bool IRAM_ATTR __adcBusy(uint8_t pin){ - - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - if(channel > 7){ - return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); - } - return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); -} - -uint16_t IRAM_ATTR __adcEnd(uint8_t pin) -{ - - uint16_t value = 0; - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return 0;//not adc pin - } - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - -void __analogReadResolution(uint8_t bits) -{ - if(!bits || bits > 16){ - return; - } - __analogSetWidth(bits); // hadware from 9 to 12 - __analogReturnedWidth = bits; // software from 1 to 16 -} - -uint16_t IRAM_ATTR adcRead(uint8_t pin) -{ - int8_t channel = digitalPinToAnalogChannel(pin); - if(channel < 0){ - return false;//not adc pin - } - - __analogInit(); - - if(channel > 9){ - channel -= 10; - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); - } else { - CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); - SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); - } - - uint16_t value = 0; - - if(channel > 7){ - while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); - } else { - while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion - value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); - } - - // Shift result if necessary - uint8_t from = __analogWidth + 9; - if (from == __analogReturnedWidth) { - return value; - } - if (from > __analogReturnedWidth) { - return value >> (from - __analogReturnedWidth); - } - return value << (__analogReturnedWidth - from); -} - - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dec7205d..ff8c467c 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,4 +1,5 @@ #include "../hardware_api.h" +#include "../../communication/SimpleFOCDebug.h" // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){ @@ -24,18 +25,23 @@ __attribute__((weak)) void* _configureADCInline(const void* driver_params, cons // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){ - return _readADCVoltageInline(pinA, cs_params); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return 0.0; } // Configure low side for generic mcu // cannot do much but __attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - return _configureADCInline(driver_params, pinA, pinB, pinC); + SIMPLEFOC_DEBUG("ERR: Low-side cs not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } // 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; } + +// function starting the ADC conversion for the high side current sensing +// only necessary for certain types of MCUs __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..46cb20be 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 @@ -27,7 +27,7 @@ volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the resul // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result -float _readADCVoltageInline(const int pin, const void* cs_params){ +float _readADCVoltageLowSide(const int pin, const void* cs_params){ uint32_t raw_adc = 0; if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[1]; @@ -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..49f2f3d5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -48,12 +48,12 @@ 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; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -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..6b597d4e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -40,12 +40,12 @@ 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; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -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..f5ca70f3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -34,12 +34,12 @@ 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; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -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..9c73f6d7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -42,12 +42,12 @@ 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; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -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..5de6432a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -41,12 +41,12 @@ 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; // if compatible timer has not been found - if (cs_params->timer_handle == NULL) return; + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver _stopTimers(driver_params->timers, 6); @@ -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_api.h b/src/drivers/hardware_api.h index 8b477458..7809233d 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -25,9 +25,6 @@ #define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true #endif - - - // flag returned if driver init fails #define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp new file mode 100644 index 00000000..ad1ff0b2 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -0,0 +1,505 @@ + +/* +* MCPWM in espressif v5.x has +* - 2x groups (units) +* each one has +* - 3 timers +* - 3 operators (that can be associated with any timer) +* which control a 2xPWM signals +* - 1x comparator + 1x generator per PWM signal + + +* Independent mode: +* ------------------ +* 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 ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* -------------------------------------------------------------------------------- +* 0-1 | 0-2 | 0 | 0 | 0 | A +* 0-1 | 0-2 | 0 | 1 | 1 | B +* 0-1 | 0-2 | 1 | 0 | 0 | A +* 0-1 | 0-2 | 1 | 1 | 1 | B +* 0-1 | 0-2 | 2 | 0 | 0 | A +* 0-1 | 0-2 | 2 | 1 | 1 | B +* +* ------------------------------------- Example 3PWM ------------------------------ +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ 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 +* +* -------------------------------------- Example 4PWM ----------------------------- +* ┌─ comparator 0 - generator 0 -> pwm A +* ┌─ operator 0 -| +* | └─ 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 + + +* 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 ----------------------------- +* +* group | timer | operator | comparator | generator | pwm +* ------------------------------------------------------------------------ +* 0-1 | 0 | 0 | 0 | 0 | A +* 0-1 | 0 | 0 | 1 | 1 | B +* 0-1 | 0 | 1 | 0 | 0 | A +* 0-1 | 0 | 1 | 1 | 1 | B +* 0-1 | 0 | 2 | 0 | 0 | A +* 0-1 | 0 | 2 | 1 | 1 | B +* +* -------------------------------------- Example 6PWM ----------------------------- +* +* ┌─ comparator 0 - generator 0 -> pwm A_h +* ┌─ 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 +* | +* | ┌─ 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 +* - two comparators per operator for independent mode +* - one comparator per operator for complementary mode +* - generators can be associated with any comparator +* - one generator per PWM signal for independent mode +* - two generators per pair of PWM signals for complementary mode (not used in simplefoc) +* - dead-time can be set for each generator pair in complementary mode +* +* Docs +* ------- +* More info here: https:*www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf#mcpwm +* 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" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#include "esp32_driver_mcpwm.h" + +// MCPWM driver hardware timer pointers +mcpwm_timer_handle_t timers[2][3] = {NULL}; +// 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}; +// last operator in the group +mcpwm_oper_handle_t last_operator[2]; + + + +// checking if group has pins available +bool _hasAvailablePins(int group, int no_pins){ + if(group_pins_used[group] + no_pins > 6){ + return false; + } + return true; +} + +// returns the index of the last timer in the group +// -1 if no timer instantiated yet +uint8_t _findLastTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i-1; + } + } + // return the last index + return i; +} +// returns the index of the next timer to instantiate +// -1 if no timers available +uint8_t _findNextTimer(int group){ + int i = 0; + for(; i<3; i++){ + if(timers[group][i] == NULL){ + return i; + } + } + return -1; +} + +/* + * find the best group for the pins + * if 6pwm + * - Only option is an an empty group + * if 3pwm + * - Best is an empty group (we can set a pwm frequency) + * - Second best is a group with 4pwms (2 operators) available (we can set the pwm frequency -> new timer+new operator) + * - Third best option is any group which has 3pwms available (but uses previously defined pwm_frequency) + * if 1pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms (one operator) available (we can set the pwm frequency -> new timer+new operator) + * - Third best is a group with 1pwm available (but uses previously defined pwm_frequency ) + * if 2pwm + * - Best option is an empty group (we can set the pwm frequency) + * - Second best is a group with 2pwms available (we can set the pwm frequency -> new timer+new operator) + * - Third best is one pin per group (but uses previously defined pwm_frequency ) + * if 4pwm + * - best option is an empty group (we can set the pwm frequency) + * - second best is a group with 4pwms available (we can set the pwm frequency -> new timer + new operators) + * - third best is 2pwms per group (we can set the pwm frequency -> new timers + new operators) + * + * PROBLEM: Skipping/loosing channels happens in some cases when the group has already used some odd number of pwm channels (for example 3pwm or 1pwm) + * 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 + * 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 + * - 1 if solution found in one group + * - 2 if solution requires using both groups + * - 0 if no solution possible +*/ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ + // an empty group is always the best option + for(int i=0; i<2; i++){ + if(!group_pins_used[i]){ + *group = i; + *timer=0; // use the first timer in an empty group + return 1; + } + } + + // 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 + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins+1)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findLastTimer(i); + return 1; + } + } + } + + // if 2 or 4 pwm + // check if there is available space in one of the groups + // if not check if they can be separated in two groups + if(no_pins == 2 || no_pins==4){ + // second best option is any group that has enough pins + for(int i=0; i<2; i++){ + if(_hasAvailablePins(i, no_pins)) { + *group=i; + *timer = _findNextTimer(i); + return 1; + } + } + // third best option is half pwms per group + int half_no_pins = (int)no_pins/2; + if(_hasAvailablePins(0,half_no_pins) && _hasAvailablePins(1 ,half_no_pins)){ + return 2; + } + } + + // otherwise fail + return 0; +} + + +// configuring center aligned pwm +// More info here: https://docs.espressif.com/projects/esp-idf/en/v5.1.4/esp32/api-reference/peripherals/mcpwm.html#symmetric-dual-edge-active-low +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted = false){ + if(inverted) + return mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); + else + return mcpwm_generator_set_actions_on_compare_event(gena, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpa, MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, cmpa, MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION_END()); +} + + + +// Helper function calculating the pwm period from the pwm frequency +// - pwm_frequency - pwm frequency in hertz +// returns pwm period in ticks (uint32_t) +uint32_t _calcPWMPeriod(long pwm_frequency) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_frequency); +} +/* + Helper function calculating the pwm frequency from the pwm period + - pwm_period - pwm period in ticks + returns pwm frequency in hertz (long) +*/ +long _calcPWMFreq(long pwm_period) { + return (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / pwm_period / 2); +} + +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins){ + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + mcpwm_timer_config_t pwm_config; + 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.period_ticks = _calcPWMPeriod(pwm_frequency); + + CHECK_ERR(mcpwm_new_timer(&pwm_config, &timers[mcpwm_group][timer_no]), "Could not initialize the timer in group: " + String(mcpwm_group)); + pwm_periods[mcpwm_group][timer_no] = pwm_config.period_ticks / 2; + params->timers[0] = timers[mcpwm_group][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_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; + operator_config.flags.update_gen_action_on_tez = true; + for (int i = 0; i < no_operators; i++) { + 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)); + } + +#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}; + for (int i = 0; i < no_operators; i++) { + 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"); + + int no_pins = 6; + 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++) { + int oper_index = (int)floor(i / 2); + 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 + + int no_generators = 6; // one per pwm + 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++) { + generator_config.gen_gpio_num = pins[i]; + int oper_index = (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 6 pwm."); + +#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)); + + } + // only available for 6pwm + 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; + dt_config_high.negedge_delay_ticks = 0; + dt_config_high.flags.invert_output = !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH; + 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; + 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)); + } +#endif + + 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_DRV_DEBUG("MCPWM configured!"); + params->dead_zone = dead_zone; + // save the configuration variables for later + group_pins_used[mcpwm_group] = 6; + return params; +} + + +/* + function configuring the pins for the mcpwm + - pwm_frequency - pwm frequency + - mcpwm_group - mcpwm group + - timer_no - timer number + - no_pins - number of pins + - pins - array of pins + - dead_zone - dead zone + + returns the driver parameters +*/ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins){ + + ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams{ + .pwm_frequency = pwm_frequency, + .group_id = mcpwm_group + }; + + bool shared_timer = false; + // check if timer is configured + if (timers[mcpwm_group][timer_no] == NULL){ + mcpwm_timer_config_t pwm_config; + 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.period_ticks = _calcPWMPeriod(pwm_frequency); + // initialise the timer + 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]; + // 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 + if(group_pins_used[mcpwm_group] %2) group_pins_used[mcpwm_group]++; + }else{ + // we will use an already instantiated timer + params->timers[0] = timers[mcpwm_group][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_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!"); + + shared_timer = true; + } + + uint8_t no_operators = ceil(no_pins / 2.0); + 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; + operator_config.flags.update_gen_action_on_tez = true; + for (int i = 0; i < no_operators; i++) { + if (shared_timer && i == 0) { // first operator already configured + params->oper[0] = last_operator[mcpwm_group]; + continue; + } + 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]; + + 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++) { + int oper_index = shared_timer ? (int)floor((i + 1) / 2) : (int)floor(i / 2); + 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)); + } + + 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++) { + generator_config.gen_gpio_num = pins[i]; + 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++) { + CHECK_ERR(_configureCenterAlign(params->generator[i],params->comparator[i], !SIMPLEFOC_PWM_ACTIVE_HIGH), "Failed to configure center align pwm: " + String(i)); + } + + 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_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; + return params; +} + +// 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)); +} + +// 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 + // 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); +} + +#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 10497876..5726e906 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h @@ -5,92 +5,154 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) +#include "driver/mcpwm_prelude.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" +#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 < 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 -#pragma message("") -#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") -#pragma message("") +#ifndef SIMPLEFOC_ESP32_HW_DEADTIME + #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. +#endif +//!< ESP32 MCPWM driver parameters +typedef struct ESP32MCPWMDriverParams { + long pwm_frequency; //!< frequency of the pwm signal + int group_id; //!< group of the mcpwm + mcpwm_timer_handle_t timers[2]; //!< timers of the mcpwm + mcpwm_oper_handle_t oper[3]; //!< operators of the mcpwm + mcpwm_cmpr_handle_t comparator[6]; //!< comparators of the mcpwm + mcpwm_gen_handle_t generator[6]; //!< generators of the mcpwm + uint32_t mcpwm_period; //!< period of the pwm signal + float dead_zone; //!< dead zone of the pwm signal +} ESP32MCPWMDriverParams; -#include "driver/mcpwm.h" -#include "soc/mcpwm_reg.h" -#include "soc/mcpwm_struct.h" -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 +#define SIMPLEFOC_ESP32_DEBUG(tag, str)\ + SimpleFOCDebug::println( "ESP32-"+String(tag)+ ": "+ String(str)); -// ABI bus frequency - would be better to take it from somewhere -// but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6f +#define SIMPLEFOC_ESP32_DRV_DEBUG(str)\ + SIMPLEFOC_ESP32_DEBUG("DRV", str);\ -// preferred pwm resolution default -#define _PWM_RES_DEF 4096 -// min resolution -#define _PWM_RES_MIN 3000 -// max resolution -#define _PWM_RES_MAX 8000 -// pwm frequency -#define _PWM_FREQUENCY 25000 // default -#define _PWM_FREQUENCY_MAX 50000 // mqx - -// structure containing motor slot configuration -// this library supports up to 4 motors -typedef struct { - int pinA; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; - mcpwm_io_signals_t mcpwm_c; -} bldc_3pwm_motor_slots_t; - -typedef struct { - int pin1A; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_1a; - mcpwm_io_signals_t mcpwm_1b; - mcpwm_io_signals_t mcpwm_2a; - mcpwm_io_signals_t mcpwm_2b; -} stepper_4pwm_motor_slots_t; - -typedef struct { - int pin1pwm; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator; - mcpwm_io_signals_t mcpwm_a; - mcpwm_io_signals_t mcpwm_b; -} stepper_2pwm_motor_slots_t; - -typedef struct { - int pinAH; - mcpwm_dev_t* mcpwm_num; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - mcpwm_io_signals_t mcpwm_ah; - mcpwm_io_signals_t mcpwm_bh; - mcpwm_io_signals_t mcpwm_ch; - mcpwm_io_signals_t mcpwm_al; - mcpwm_io_signals_t mcpwm_bl; - mcpwm_io_signals_t mcpwm_cl; -} bldc_6pwm_motor_slots_t; +// 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_DRV_DEBUG("ERROR - " + String(message)); \ + return SIMPLEFOC_DRIVER_INIT_FAILED; \ + } -typedef struct ESP32MCPWMDriverParams { - long pwm_frequency; - mcpwm_dev_t* mcpwm_dev; - mcpwm_unit_t mcpwm_unit; - mcpwm_operator_t mcpwm_operator1; - mcpwm_operator_t mcpwm_operator2; - float deadtime; -} ESP32MCPWMDriverParams; - +// ABI bus frequency - would be better to take it from somewhere +// but I did nto find a good exposed variable +#define _MCPWM_FREQ 160e6f +#define _PWM_TIMEBASE_RESOLUTION_HZ (_MCPWM_FREQ) /*!< Resolution of MCPWM */ +// pwm frequency settings +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50kHz + + +// low-level configuration API + +/** + * checking if group has pins available + * @param group - group of the mcpwm + * @param no_pins - number of pins + * @returns true if pins are available, false otherwise + */ +bool _hasAvailablePins(int group, int no_pins); +/** + * function finding the last timer in the group + * @param group - group of the mcpwm + * @returns index of the last timer in the group + * -1 if no timer instantiated yet + */ +uint8_t _findLastTimer(int group); + +/** + * function finding the next timer in the group + * @param group - group of the mcpwm + * @returns index of the next timer in the group + * -1 if all timers are used + */ +uint8_t _findNextTimer(int group); + + +/** + * function finding the best group and timer for the pwm signals + * + * @param no_pins - number of pins + * @param pwm_freq - frequency of the pwm signal + * @param group - pointer to the group + * @param timer - pointer to the timer + * @returns + * 1 if solution found in one group + * 2 if solution requires using both groups + * 0 if no solution possible + */ +int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer); + + +/** + * function configuring the center alignement and inversion of a pwm signal + * @param gena - mcpwm generator handle + * @param cmpa - mcpwm comparator handle + * @param inverted - true if the signal is inverted, false otherwise + */ +int _configureCenterAlign(mcpwm_gen_handle_t gena, mcpwm_cmpr_handle_t cmpa, bool inverted); + +/** + * function calculating the pwm period + * @param pwm_frequency - frequency of the pwm signal + * @return uint32_t - period of the pwm signal + */ +uint32_t _calcPWMPeriod(long pwm_frequency); +/** + * function calculating the pwm frequency + * @param pwm_period - period of the pwm signal + * @return long - frequency of the pwm signal + */ +long _calcPWMFreq(long pwm_period); + +/** + * function configuring the MCPWM for 6pwm + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param dead_zone - dead zone of the pwm signal + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configure6PWMPinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, float dead_zone, int* pins); +/** + * function configuring the MCPWM for pwm generation + * @param pwm_frequency - frequency of the pwm signal + * @param mcpwm_group - group of the mcpwm + * @param timer_no - timer number + * @param no_pins - number of pins + * @param pins - array of pins + * @return ESP32MCPWMDriverParams* - pointer to the driver parameters if successful, -1 if failed + */ +void* _configurePinsMCPWM(long pwm_frequency, int mcpwm_group, int timer_no, int no_pins, int* pins); +/** + * function setting the duty cycle to the MCPWM channel + * @param cmpr - mcpwm channel handle + * @param mcpwm_period - period of the pwm signal + * @param duty_cycle - duty cycle of the pwm signal + */ +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 a454c052..dc667ab3 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -7,18 +7,21 @@ #pragma message("") #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) +#endif #define _PWM_FREQUENCY 25000 // 25khz #define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution #define _PWM_RES_BIT 10 // 10 bir resolution -#define _PWM_RES 1023 // 2^10-1 = 1023-1 - +#define _PWM_RES 1023 // 2^10-1 = 1024-1 -// empty motor slot -#define _EMPTY_SLOT -20 -#define _TAKEN_SLOT -21 -// figure out how many ledc channels are avaible +// figure out how many ledc channels are available // esp32 - 2x8=16 // esp32s2 - 8 // esp32c3 - 6 @@ -29,51 +32,129 @@ #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM) #endif +#define LEDC_CHANNELS_GROUP0 (LEDC_CHANNELS < 8 ? LEDC_CHANNELS : 8) +#define LEDC_CHANNELS_GROUP1 (LEDC_CHANNELS < 8 ? 0 : LEDC_CHANNELS - 8) -// current channel stack index + +// currently used ledc channels // support for multiple motors // esp32 has 16 channels // esp32s2 has 8 channels // esp32c3 has 6 channels -int channel_index = 0; - - +// 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}; typedef struct ESP32LEDCDriverParams { - int channels[6]; + ledc_channel_t channels[6]; + ledc_mode_t groups[6]; long pwm_frequency; + float dead_zone; } ESP32LEDCDriverParams; - - - -// configure High PWM frequency -void _setHighFrequency(const long freq, const int pin, const int channel){ - ledcSetup(channel, freq, _PWM_RES_BIT ); - ledcAttachPin(pin, channel); +/* + Function to attach a channel to a pin with advanced settings + - freq - pwm frequency + - resolution - pwm resolution + - channel - ledc channel + - 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 + 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. + + Function returns true if the channel was successfully attached, false otherwise. +*/ +bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t freq, uint8_t resolution, bool inverted) { + + + ledc_channel_t channel = static_cast(_channel); + ledc_mode_t group = static_cast(_group); + + ledc_timer_bit_t res = static_cast(resolution); + ledc_timer_config_t ledc_timer; + ledc_timer.speed_mode = group; + ledc_timer.timer_num = LEDC_TIMER_0; + ledc_timer.duty_resolution = res; + ledc_timer.freq_hz = freq; + ledc_timer.clk_cfg = LEDC_AUTO_CLK; + if (ledc_timer_config(&ledc_timer) != ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure the timer:", LEDC_TIMER_0); + return false; + } + + // if active high is false invert + int pin_high_level = SIMPLEFOC_PWM_ACTIVE_HIGH ? 0 : 1; + if (inverted) pin_high_level = !pin_high_level; + + uint32_t duty = ledc_get_duty(group, channel); + ledc_channel_config_t ledc_channel; + ledc_channel.speed_mode = group; + ledc_channel.channel = channel; + ledc_channel.timer_sel = LEDC_TIMER_0; + ledc_channel.intr_type = LEDC_INTR_DISABLE; + ledc_channel.gpio_num = pin; + ledc_channel.duty = duty; + ledc_channel.hpoint = 0; + ledc_channel.flags.output_invert = pin_high_level; // 0 is active high, 1 is active low + if (ledc_channel_config(&ledc_channel)!= ESP_OK) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to attach channel:", _channel); + return false; + } + + return true; } +// returns the number of available channels in the group +int _availableGroupChannels(int group){ + if(group == 0) return LEDC_CHANNELS_GROUP0 - group_channels_used[0]; + else if(group == 1) return LEDC_CHANNELS_GROUP1 - group_channels_used[1]; + return 0; +} - +// 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 +int _findGroupWithChannelsAvailable(int no_channels){ + if(no_channels <= _availableGroupChannels(0)) return 0; + if(no_channels <= _availableGroupChannels(1)) return 1; + return -1; +} void* _configure1PWM(long pwm_frequency, const int pinA) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 1PWM"); // check if enough channels available - if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - - int ch1 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - + int group = _findGroupWithChannelsAvailable(1); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup in group: ", (group)); + + // configure the channel + group_channels_used[group] += 1; + if(!_ledcAttachChannelAdvanced(pinA, group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pinA); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1 }, + .channels = { static_cast(group_channels_used[group]) }, + .groups = { (ledc_mode_t)group }, .pwm_frequency = pwm_frequency }; + SIMPLEFOC_DEBUG("EP32-DRV: 1PWM setup successful in group: ", (group)); return params; } @@ -88,18 +169,34 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 2PWM"); - int ch1 = channel_index++; - int ch2 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(2); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[2] = {pinA, pinB}; + for(int i = 0; i < 2; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + 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; + } + SIMPLEFOC_DEBUG("EP32-DRV: 2PWM setup successful in group: ", (group)); return params; } @@ -109,20 +206,34 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channel_index + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 3PWM"); - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(3); + if (group < 0) { + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup in group: ", (group)); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + int pins[3] = {pinA, pinB, pinC}; + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, false)){ + 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; + } + SIMPLEFOC_DEBUG("EP32-DRV: 3PWM setup successful in group: ", (group)); return params; } @@ -132,54 +243,162 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - // check if enough channels available - if ( channel_index + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; - - int ch1 = channel_index++; - int ch2 = channel_index++; - int ch3 = channel_index++; - int ch4 = channel_index++; - _setHighFrequency(pwm_frequency, pinA, ch1); - _setHighFrequency(pwm_frequency, pinB, ch2); - _setHighFrequency(pwm_frequency, pinC, ch3); - _setHighFrequency(pwm_frequency, pinD, ch4); ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { - .channels = { ch1, ch2, ch3, ch4 }, + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)0 }, .pwm_frequency = pwm_frequency }; + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 4PWM"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(4); + if (group < 0){ + // not enough channels available on any individual group + // check if their combined number is enough (two channels per group) + if(_availableGroupChannels(0) >=2 && _availableGroupChannels(1) >=2){ + group = 2; + SIMPLEFOC_DEBUG("EP32-DRV: WARNING: Not enough available ledc channels for 4pwm in a single group! Using two groups!"); + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in groups: 0 and 1!"); + params->groups[2] = (ledc_mode_t)1; + params->groups[3] = (ledc_mode_t)1; + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough available ledc channels for 4pwm!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + }else{ + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup in group: ", (group)); + params->groups[0] = (ledc_mode_t)group; + params->groups[1] = (ledc_mode_t)group; + params->groups[2] = (ledc_mode_t)group; + params->groups[3] = (ledc_mode_t)group; + } + + + + int pins[4] = {pinA, pinB, pinC, pinD}; + for(int i = 0; i < 4; i++){ + group_channels_used[params->groups[i]]++; + if(!_ledcAttachChannelAdvanced(pins[i], group_channels_used[params->groups[i]], params->groups[i], pwm_frequency, _PWM_RES_BIT, false)){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Failed to configure pin:", pins[i]); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + params->channels[i] = static_cast(group_channels_used[params->groups[i]]); + } + SIMPLEFOC_DEBUG("EP32-DRV: 4PWM setup successful!"); return params; } - +void _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){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); } void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); } void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES)); + _writeDutyCycle(dc_a, params, 0); + _writeDutyCycle(dc_b, params, 1); + _writeDutyCycle(dc_c, params, 2); } void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES)); - ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES)); + _writeDutyCycle(dc_1a, params, 0); + _writeDutyCycle(dc_1b, params, 1); + _writeDutyCycle(dc_2a, params, 2); + _writeDutyCycle(dc_2b, params, 3); +} + + +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + SIMPLEFOC_DEBUG("EP32-DRV: Configuring 6PWM"); + SIMPLEFOC_DEBUG("EP32-DRV: WARNING - 6PWM on LEDC is poorly supported and not tested, consider using MCPWM driver instead!"); + // check if enough channels available + int group = _findGroupWithChannelsAvailable(6); + if (group < 0){ + SIMPLEFOC_DEBUG("EP32-DRV: ERROR - Not enough channels available!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + SIMPLEFOC_DEBUG("EP32-DRV: 6PWM setup in group: ", (group)); + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { static_cast(0)}, + .groups = { (ledc_mode_t)group }, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + int high_side_invert = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? false : true; + int low_side_invert = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? true : false; + + int pin_pairs[6][2] = { + {pinA_h, pinA_l}, + {pinB_h, pinB_l}, + {pinC_h, pinC_l} + }; + + for(int i = 0; i < 3; i++){ + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][0], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, high_side_invert)){ + 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; + + group_channels_used[group]++; + if(!_ledcAttachChannelAdvanced(pin_pairs[i][1], group_channels_used[group], group, pwm_frequency, _PWM_RES_BIT, low_side_invert)){ + 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); + + // determine the phase state and set the pwm accordingly + // deactivate phases if needed + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){ + _writeDutyCycle(0, params, ind_h); + }else{ + _writeDutyCycle(pwm_h, params, ind_h); + } + if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){ + _writeDutyCycle(0, params, ind_l); + }else{ + _writeDutyCycle(pwm_l, params, ind_l); + } +} + +void _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]); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp new file mode 100644 index 00000000..e2c621c5 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -0,0 +1,226 @@ +#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("") + +// function setting the high pwm frequency to the supplied pins +// - DC motor - 1PWM setting +// - hardware specific +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(1, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 1PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + 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); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware specific +void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + int ret = _findBestGroup(2, pwm_frequency, &group, &timer); + if(!ret) { + 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_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_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + 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_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_DRV_DEBUG("Error configuring 1PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + }; + for(int i =0; i<2; i++){ + ret_params->timers[i] = params[i]->timers[0]; + ret_params->oper[i] = params[i]->oper[0]; + ret_params->comparator[i] = params[i]->comparator[0]; + ret_params->generator[i] = params[i]->generator[0]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; + } +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware specific +void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(3, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 3PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + 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); +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware specific +void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD){ + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + int ret = _findBestGroup(4, pwm_frequency, &group, &timer); + if(!ret) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 4PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if(ret == 1){ + 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_DRV_DEBUG("Configuring 4PWM as two 2PWM drivers"); + ESP32MCPWMDriverParams* params[2]; + + // the code is a bit huge for what it does + // it just instantiates two 2PMW drivers and combines the returned params + 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_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_DRV_DEBUG("Error configuring 2PWM"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + }else{ + params[i] = (ESP32MCPWMDriverParams*)p; + } + } + // combine the driver parameters + ESP32MCPWMDriverParams* ret_params = new ESP32MCPWMDriverParams{ + .pwm_frequency = params[0]->pwm_frequency, + .group_id = 2, // both groups + .timers = {params[0]->timers[0], params[1]->timers[0]}, + .oper = {params[0]->oper[0], params[1]->oper[0]} + }; + for(int i =0; i<4; i++){ + ret_params->comparator[i] = params[(int)floor(i/2)]->comparator[i%2]; + ret_params->generator[i] = params[(int)floor(i/2)]->generator[i%2]; + } + ret_params->mcpwm_period = params[0]->mcpwm_period; + return ret_params; + } +} + + +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + + int group, timer; + if(!_findBestGroup(6, pwm_frequency, &group, &timer)) { + SIMPLEFOC_ESP32_DRV_DEBUG("Not enough pins available for 6PWM!"); + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + 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); +} + +// 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){ + _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); +} + +// function setting the pwm duty cycle to the hardware +// - DCMotor -1PWM setting +// - hardware specific +void _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){ + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[0], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_a); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[1], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_b); +} + + + +// 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){ + // 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); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[2], ((ESP32MCPWMDriverParams*)params)->mcpwm_period, dc_2a); + _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){ +#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); + _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; + _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); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[3], period, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? dc_b+dead_zone : 1.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[4], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? dc_c-dead_zone : 0.0f); + _setDutyCycle(((ESP32MCPWMDriverParams*)params)->comparator[5], period, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? dc_c+dead_zone : 1.0f); +#endif +} +#endif diff --git a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcu.cpp deleted file mode 100644 index 0dc23c10..00000000 --- a/src/drivers/hardware_specific/esp32/esp32_mcu.cpp +++ /dev/null @@ -1,431 +0,0 @@ -#include "esp32_driver_mcpwm.h" - -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) - -#ifndef SIMPLEFOC_ESP32_HW_DEADTIME - #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use. -#endif - -// define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B - }; - -// define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; - -// define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; - -// define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A - {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A - {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B - }; - - -// configuring high frequency pwm timer -// a lot of help from this post from Paul Gauld -// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller -// a short tutorial for v2.0.1+ -// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html -// -void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ - - mcpwm_config_t pwm_config; - pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) - pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0) - pwm_config.frequency = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76 - mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings - mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - - if (_isset(dead_zone)){ - // dead zone is configured - - // When using hardware deadtime, setting the phase_state parameter is not supported. - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_type_t pwm_mode; - if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;} // Normal, noninverting driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;} // Inverted lowside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;} // Inverted highside driver - else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;} // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low! - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0); - #else // Software deadtime - for (int i = 0; i < 3; i++){ - if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);} // Normal, noninverted highside - else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver - - if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);} // Normal, complementary lowside - else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver - } - #endif - - } - _delay(100); - - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); - - // manual configuration due to the lack of config flexibility in mcpwm_init() - mcpwm_num->clk_cfg.clk_prescale = 0; - // calculate prescaler and period - // step 1: calculate the prescaler using the default pwm resolution - // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; - // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); - // now calculate the real resolution timer period necessary (pwm resolution) - // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); - // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); - resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); - - // set prescaler - mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler; - mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler; - _delay(1); - //set period - mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected; - mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected; - _delay(1); - mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0; - mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0; - _delay(1); - // _delay(1); - //restart the timers - mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); - mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); - - mcpwm_sync_config_t sync_conf = { - .sync_sig = MCPWM_SELECT_TIMER0_SYNC, - .timer_val = 0, - .count_direction = MCPWM_TIMER_DIRECTION_UP - }; - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf); - mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf); - - // Enable sync event for all timers to be the TEZ of timer0 - mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ); -} - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 2PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - - stepper_2pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; - m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; - break; - } - } - - // disable all the slots with the same MCPWM - // disable 3pwm bldc motor which would go in the same slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; -} - - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 -void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - - bldc_3pwm_motor_slots_t m_slot = {}; - - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 4; slot_num++){ - if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; - m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; - break; - } - } - // disable all the slots with the same MCPWM - // disable 2pwm steppr motor which would go in the same slot - esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; - if( slot_num < 2 ){ - // slot 0 of the 4pwm stepper - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slot 1 of the stepper - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator - }; - return params; -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 4PWM setting -// - hardware speciffic -void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - stepper_4pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; - m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; - break; - } - } - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slots 0 and 1 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slots 2 and 3 of the 2pwm stepper taken - esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; - esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); - - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2 - }; - return params; -} - - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 2PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); -} - - - - -// function setting the pwm duty cycle to the hardware -// - BLDC motor - 3PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0); -} - - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 4PWM setting -// - hardware speciffic -// ESP32 uses MCPWM -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - // se the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100] - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0); -} - - - - -// Configuring PWM frequency, resolution and alignment -// - BLDC driver - 6PWM setting -// - hardware specific -void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency - bldc_6pwm_motor_slots_t m_slot = {}; - // determine which motor are we connecting - // and set the appropriate configuration parameters - int slot_num; - for(slot_num = 0; slot_num < 2; slot_num++){ - if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot - esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; - m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; - break; - } - } - // if no slots available - if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED; - - // disable all the slots with the same MCPWM - if( slot_num == 0 ){ - // slots 0 and 1 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; - // slot 0 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; - }else{ - // slots 2 and 3 of the 3pwm bldc - esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; - esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; - // slot 1 of the 6pwm bldc - esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } - - // configure pins - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); - mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); - - // configure the timer - _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return - ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams { - .pwm_frequency = pwm_frequency, - .mcpwm_dev = m_slot.mcpwm_num, - .mcpwm_unit = m_slot.mcpwm_unit, - .mcpwm_operator1 = m_slot.mcpwm_operator1, - .mcpwm_operator2 = m_slot.mcpwm_operator2, - .deadtime = _isset(dead_zone) ? dead_zone : 0 - }; - return params; -} - - - - -// Function setting the duty cycle to the pwm pin (ex. analogWrite()) -// - BLDC driver - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - // set the PWM on the slot timers - // transform duty cycle from [0,1] to [0,100.0] - #if SIMPLEFOC_ESP32_HW_DEADTIME == true - // Hardware deadtime does deadtime insertion internally - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f); - _UNUSED(phase_state); - #else - float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime; - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0); - mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f); - #endif -} - -#endif 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..dbf48970 --- /dev/null +++ b/src/drivers/hardware_specific/esp32/mcpwm_private.h @@ -0,0 +1,82 @@ +/* + * 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 + + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC) + +#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 + +#endif /* MCPWM_PRIVATE_H */ \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new deleted file mode 100644 index 5dcac90d..00000000 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new +++ /dev/null @@ -1,543 +0,0 @@ -#include "teensy_mcu.h" -#include "teensy4_mcu.h" -#include "../../../communication/SimpleFOCDebug.h" - -// if defined -// - Teensy 4.0 -// - Teensy 4.1 -#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - - -#pragma message("") -#pragma message("SimpleFOC: compiling for Teensy 4.x") -#pragma message("") - - - -// function finding the TRIG event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 -int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; - } - return -1; -} - -// function finding the EXT_SYNC event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 -int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 - } - return -1; -} - -// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper -// function to make code more readable. -void xbar_connect(unsigned int input, unsigned int output) -{ - if (input >= 88) return; - if (output >= 132) return; - volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); - uint16_t val = *xbar; - if (!(output & 1)) { - val = (val & 0xFF00) | input; - } else { - val = (val & 0x00FF) | (input << 8); - } - *xbar = val; -} - -void xbar_init() { - CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 -} - -// half_cycle of the PWM variable -int half_cycle = 0; - -// function which finds the flexpwm instance for a pin -// if it does not belong to the flexpwm timer it returns a nullpointer -IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } - info = pwm_pin_info + pin; - // FlexPWM pin - IMXRT_FLEXPWM_t *flexpwm; - switch ((info->module >> 4) & 3) { - case 0: flexpwm = &IMXRT_FLEXPWM1; break; - case 1: flexpwm = &IMXRT_FLEXPWM2; break; - case 2: flexpwm = &IMXRT_FLEXPWM3; break; - default: flexpwm = &IMXRT_FLEXPWM4; - } - if(flexpwm != nullptr){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); - SIMPLEFOC_DEBUG(s); -#endif - return flexpwm; - } - return nullptr; -} - - -// function which finds the timer submodule for a pin -// if it does not belong to the submodule it returns a -1 -int get_submodule(uint8_t pin){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int sm1 = info->module&0x3; - - if (sm1 >= 0 && sm1 < 4) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); - SIMPLEFOC_DEBUG(s); -#endif - return sm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[50]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } -} - -// function which finds the flexpwm instance for a pair of pins -// if they do not belong to the same timer it returns a nullpointer -IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } - info = pwm_pin_info + pin; - // FlexPWM pin - IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; - switch ((info->module >> 4) & 3) { - case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; - case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; - case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; - default: flexpwm1 = &IMXRT_FLEXPWM4; - } - - info = pwm_pin_info + pin1; - switch ((info->module >> 4) & 3) { - case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; - case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; - case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; - default: flexpwm2 = &IMXRT_FLEXPWM4; - } - if(flexpwm1 == flexpwm2){ - char s[60]; - sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); - SIMPLEFOC_DEBUG(s); - return flexpwm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return nullptr; - } -} - - -// function which finds the timer submodule for a pair of pins -// if they do not belong to the same submodule it returns a -1 -int get_submodule(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int sm1 = info->module&0x3; - info = pwm_pin_info + pin1; - int sm2 = info->module&0x3; - - if (sm1 == sm2) { - char s[60]; - sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); - SIMPLEFOC_DEBUG(s); - return sm1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[50]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } -} - - -// function which finds the timer submodule for a pair of pins -// if they do not belong to the same submodule it returns a -1 -int get_inverted_channel(uint8_t pin, uint8_t pin1){ - - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } - - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; - - if (ch1 != 1) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } else if (ch2 != 2) { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); - SIMPLEFOC_DEBUG(s); -#endif - return -1; - } else { -#ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; - sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); - SIMPLEFOC_DEBUG(s); -#endif -return ch2; - } -} - -// Helper to set up A/B pair on a FlexPWM submodule. -// can configure sync, prescale and B inversion. -// sets the desired frequency of the PWM -// sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) -{ - int submodule_mask = 1 << submodule ; - flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running - flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK - - - // calculate the counter and prescaler for the desired pwm frequency - uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); - uint32_t prescale = 0; - //printf(" div=%lu\n", newdiv); - while (newdiv > 65535 && prescale < 7) { - newdiv = newdiv >> 1; - prescale = prescale + 1; - } - if (newdiv > 65535) { - newdiv = 65535; - } else if (newdiv < 2) { - newdiv = 2; - } - - // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); - int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% - int mid_pwm = int((half_cycle)/2.0f); - - // if the timer should be externally synced with the master timer - int sel = ext_sync ? 3 : 0; - - // setup the timer - // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h - flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); - flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; - // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; - if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer - flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) - flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0; - flexpwm->SM[submodule].VAL1 = half_cycle ; - flexpwm->SM[submodule].VAL2 = -mid_pwm ; - flexpwm->SM[submodule].VAL3 = +mid_pwm ; - - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled - flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running -} - - -// staring the PWM on A and B channels of the submodule -void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) -{ - int submodule_mask = 1 << submodule ; - - flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output - flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output -} - - - -// PWM setting on the high and low pair of the PWM channels -void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - int mid_pwm = int((half_cycle)/2.0f); - int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - - flexpwm->SM[submodule].VAL2 = -count_pwm; // A on - flexpwm->SM[submodule].VAL3 = count_pwm ; // A off - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; - int count_pwm = int(count*duty); - - SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); - SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); - SIMPLEFOC_DEBUG("count: ",count_pwm); - - // flexpwm->SM[submodule].VAL1 = 0; // A on - flexpwm->SM[submodule].VAL2 = count_pwm ; // A off - flexpwm->SM[submodule].VAL3 = count_pwm; // A on - flexpwm->SM[submodule].VAL4 = count_pwm ; // A off - flexpwm->SM[submodule].VAL5 = count_pwm ; // A off - flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; - uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; - if (cval > modulo) cval = modulo; // TODO: is this check correct? - - //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", - //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); - p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); - switch (channel) { - case 0: // X - p->SM[submodule].VAL0 = modulo - cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); - //printf(" write channel X\n"); - break; - case 1: // A - p->SM[submodule].VAL3 = cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); - //printf(" write channel A\n"); - break; - case 2: // B - p->SM[submodule].VAL5 = cval; - p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); - //printf(" write channel B\n"); - } - p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); -} - - -// function setting the high pwm frequency to the supplied pins -// - Stepper motor - 6PWM setting -// - hardware specific -void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - - IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; - int submoduleA,submoduleB,submoduleC; - int inverted_channelA,inverted_channelB,inverted_channelC; - flexpwmA = get_flexpwm(pinA_h,pinA_l); - submoduleA = get_submodule(pinA_h,pinA_l); - inverted_channelA = get_inverted_channel(pinA_h,pinA_l); - flexpwmB = get_flexpwm(pinB_h,pinB_l); - submoduleB = get_submodule(pinB_h,pinB_l); - inverted_channelB = get_inverted_channel(pinB_h,pinB_l); - flexpwmC = get_flexpwm(pinC_h,pinC_l); - submoduleC = get_submodule(pinC_h,pinC_l); - inverted_channelC = get_inverted_channel(pinC_h,pinC_l); - - if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ -#ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); -#endif - return SIMPLEFOC_DRIVER_INIT_FAILED; - } - - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone - }; - - - // Configure FlexPWM units, each driving A/B pair, B inverted. - // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz - setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced - setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced - setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; - delayMicroseconds (100) ; - - // turn on XBAR1 clock for all but stop mode - xbar_init() ; - - // // Connect trigger to synchronize all timers - xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; - xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; - - startup_pwm_pair (flexpwmA, submoduleA) ; - startup_pwm_pair (flexpwmB, submoduleB) ; - startup_pwm_pair (flexpwmC, submoduleC) ; - - - delayMicroseconds(50) ; - // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. - *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; - *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; - *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; - *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; - *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; - *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; - - return params; -} - - - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 6PWM setting -// - hardware specific -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ - _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); -} - - -// function setting the high pwm frequency to the supplied pins -// - BLDC motor - 3PWM setting -// - hardware speciffic -// in generic case dont do anything - void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - - if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - - IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; - int submoduleA,submoduleB,submoduleC; - int inverted_channelA,inverted_channelB,inverted_channelC; - flexpwmA = get_flexpwm(pinA); - submoduleA = get_submodule(pinA); - flexpwmB = get_flexpwm(pinB); - submoduleB = get_submodule(pinB); - flexpwmC = get_flexpwm(pinC); - submoduleC = get_submodule(pinC); - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA, pinB, pinC }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - }; - - startup_pwm_pair (flexpwmA, submoduleA) ; - startup_pwm_pair (flexpwmB, submoduleB) ; - startup_pwm_pair (flexpwmC, submoduleC) ; - - // analogWriteFrequency(pinA, pwm_frequency); - // analogWriteFrequency(pinB, pwm_frequency); - // analogWriteFrequency(pinC, pwm_frequency); - // analogWrite(pinA, 0); - // analogWrite(pinB, 0); - // analogWrite(pinC, 0); - - // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. - // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; - // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; - // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; - - return params; -} - -// function setting the pwm duty cycle to the hardware -// - Stepper motor - 6PWM setting -// - hardware specific -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); -} - - -#endif \ No newline at end of file