From c28ad9de58e8fed0bdab52dcec1dd273292d86b9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 10 Jun 2024 17:56:59 +0200 Subject: [PATCH 01/10] fix for MagneticSensorI2C uses incorrect bit mask calculation #402402 --- src/sensors/MagneticSensorI2C.cpp | 99 ++++++++++++++----------------- src/sensors/MagneticSensorI2C.h | 26 ++++---- 2 files changed, 54 insertions(+), 71 deletions(-) diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 2b3db0c2..64b7dd44 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -5,7 +5,10 @@ MagneticSensorI2CConfig_s AS5600_I2C = { .chip_address = 0x36, .bit_resolution = 12, .angle_register = 0x0C, - .data_start_bit = 11 + .msb_mask = 0x0F, + .msb_shift = 8, + .lsb_mask = 0xFF, + .lsb_shift = 0 }; /** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */ @@ -13,7 +16,10 @@ MagneticSensorI2CConfig_s AS5048_I2C = { .chip_address = 0x40, // highly configurable. if A1 and A2 are held low, this is probable value .bit_resolution = 14, .angle_register = 0xFE, - .data_start_bit = 15 + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0x3F, + .lsb_shift = 0 }; /** Typical configuration for the 12bit MT6701 magnetic sensor over I2C interface */ @@ -21,7 +27,10 @@ MagneticSensorI2CConfig_s MT6701_I2C = { .chip_address = 0x06, .bit_resolution = 14, .angle_register = 0x03, - .data_start_bit = 15 + .msb_mask = 0xFF, + .msb_shift = 6, + .lsb_mask = 0xFC, + .lsb_shift = 2 }; @@ -30,57 +39,49 @@ MagneticSensorI2CConfig_s MT6701_I2C = { // @param _bit_resolution bit resolution of the sensor // @param _angle_register_msb angle read register // @param _bits_used_msb number of used bits in msb -MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ - // chip I2C address - chip_address = _chip_address; - // angle read register of the magnetic sensor - angle_register_msb = _angle_register_msb; - // register maximum value (counts per revolution) +MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb, bool lsb_right_aligned){ + _conf.chip_address = _chip_address; + _conf.bit_resolution = _bit_resolution; + _conf.angle_register = _angle_register_msb; + _conf.msb_mask = (uint8_t)( (1 << _bits_used_msb) - 1 ); + + uint8_t lsb_used = _bit_resolution - _bits_used_msb; // used bits in LSB + _conf.lsb_mask = (uint8_t)( (1 << (lsb_used)) - 1 ); + if (!lsb_right_aligned) + _conf.lsb_shift = 8-lsb_used; + else + _conf.lsb_shift = 0; + _conf.msb_shift = lsb_used; + cpr = _powtwo(_bit_resolution); - // depending on the sensor architecture there are different combinations of - // LSB and MSB register used bits - // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB - // MT6701 uses 0..5 LSB and 9..15 MSB - // used bits in LSB - lsb_used = _bit_resolution - _bits_used_msb; - // extraction masks - lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); - msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); wire = &Wire; } -MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ - chip_address = config.chip_address; - // angle read register of the magnetic sensor - angle_register_msb = config.angle_register; - // register maximum value (counts per revolution) - cpr = _powtwo(config.bit_resolution); - int bits_used_msb = config.data_start_bit - 7; - lsb_used = config.bit_resolution - bits_used_msb; - // extraction masks - lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); - msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 ); +MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ + _conf = config; + cpr = _powtwo(config.bit_resolution); wire = &Wire; } + + MagneticSensorI2C MagneticSensorI2C::AS5600() { return {AS5600_I2C}; } -void MagneticSensorI2C::init(TwoWire* _wire){ - - wire = _wire; - // I2C communication begin - wire->begin(); +void MagneticSensorI2C::init(TwoWire* _wire){ + wire = _wire; + wire->begin(); // I2C communication begin this->Sensor::init(); // call base class init } + + // Shaft angle calculation // angle is in radians [rad] float MagneticSensorI2C::getSensorAngle(){ @@ -90,39 +91,25 @@ float MagneticSensorI2C::getSensorAngle(){ -// function reading the raw counter of the magnetic sensor -int MagneticSensorI2C::getRawCount(){ - return (int)MagneticSensorI2C::read(angle_register_msb); -} - // I2C functions /* -* Read a register from the sensor -* Takes the address of the register as a uint8_t -* Returns the value of the register +* Read an angle from the angle register of the sensor */ -int MagneticSensorI2C::read(uint8_t angle_reg_msb) { +int MagneticSensorI2C::getRawCount() { // read the angle register first MSB then LSB byte readArray[2]; uint16_t readValue = 0; // notify the device that is aboout to be read - wire->beginTransmission(chip_address); - wire->write(angle_reg_msb); + wire->beginTransmission(_conf.chip_address); + wire->write(_conf.angle_register); currWireError = wire->endTransmission(false); - // read the data msb and lsb - wire->requestFrom(chip_address, (uint8_t)2); + wire->requestFrom(_conf.chip_address, 2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - - // depending on the sensor architecture there are different combinations of - // LSB and MSB register used bits - // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB - // MT6701 uses 0..5 LSB and 6..13 MSB - readValue = ( readArray[1] & lsb_mask ); - readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); + readValue = (readArray[0] & _conf.msb_mask) << _conf.msb_shift; + readValue |= (readArray[1] & _conf.lsb_mask) >> _conf.lsb_shift; return readValue; } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index f8b189fa..381a950a 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -8,13 +8,17 @@ #include "../common/time_utils.h" struct MagneticSensorI2CConfig_s { - int chip_address; - int bit_resolution; - int angle_register; - int data_start_bit; + uint8_t chip_address; + uint8_t bit_resolution; + uint8_t angle_register; + uint8_t msb_mask; + uint8_t msb_shift; + uint8_t lsb_mask; + uint8_t lsb_shift; }; + // some predefined structures -extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C; +extern MagneticSensorI2CConfig_s AS5600_I2C, AS5048_I2C, MT6701_I2C; #if defined(TARGET_RP2040) #define SDA I2C_SDA @@ -31,7 +35,7 @@ class MagneticSensorI2C: public Sensor{ * @param angle_register_msb angle read register msb * @param _bits_used_msb number of used bits in msb */ - MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used); + MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used, bool lsb_right_aligned = true); /** * MagneticSensorI2C class constructor @@ -56,13 +60,7 @@ class MagneticSensorI2C: public Sensor{ private: float cpr; //!< Maximum range of the magnetic sensor - uint16_t lsb_used; //!< Number of bits used in LSB register - uint8_t lsb_mask; - uint8_t msb_mask; - - // I2C variables - uint8_t angle_register_msb; //!< I2C angle register to read - uint8_t chip_address; //!< I2C chip select pins + MagneticSensorI2CConfig_s _conf; // I2C functions /** Read one I2C register value */ @@ -76,8 +74,6 @@ class MagneticSensorI2C: public Sensor{ /* the two wire instance for this sensor */ TwoWire* wire; - - }; From 1d1c5ddec80e9365cae19e4c518ec1eeb5e5097d Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 24 Aug 2024 22:57:07 +0200 Subject: [PATCH 02/10] HAL / LL only PWM driver --- .../hardware_specific/stm32/stm32_mcu.h | 2 +- .../stm32/stm32f4/stm32f4_hal.cpp | 8 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 12 +- .../stm32/stm32f4/stm32f4_utils.cpp | 18 +- .../stm32/stm32f4/stm32f4_utils.h | 4 +- .../stm32/stm32g4/stm32g4_hal.cpp | 6 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 12 +- .../stm32/stm32g4/stm32g4_utils.cpp | 40 +- .../stm32/stm32g4/stm32g4_utils.h | 4 +- .../esp32/esp32_ledc_mcu.cpp | 12 +- .../esp32/esp32_mcpwm_mcu.cpp | 20 +- .../hardware_specific/portenta_h7_mcu.cpp | 2 +- .../hardware_specific/stm32/stm32_mcu.cpp | 1245 +++++------------ .../hardware_specific/stm32/stm32_mcu.h | 55 +- .../stm32/stm32_searchtimers.cpp | 193 +++ .../stm32/stm32_searchtimers.h | 11 + .../stm32/stm32_timerutils.cpp | 902 ++++++++++++ .../stm32/stm32_timerutils.h | 33 + 18 files changed, 1650 insertions(+), 929 deletions(-) create mode 100644 src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp create mode 100644 src/drivers/hardware_specific/stm32/stm32_searchtimers.h create mode 100644 src/drivers/hardware_specific/stm32/stm32_timerutils.cpp create mode 100644 src/drivers/hardware_specific/stm32/stm32_timerutils.h diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h index 6e238170..055f20db 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.h +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h @@ -14,7 +14,7 @@ typedef struct Stm32CurrentSenseParams { int pins[3] = {(int)NOT_SET}; float adc_voltage_conv; ADC_HandleTypeDef* adc_handle = NP; - HardwareTimer* timer_handle = NP; + TIM_HandleTypeDef* timer_handle = NP; } Stm32CurrentSenseParams; #endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index bd0df4b6..5320cc3e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -76,8 +76,8 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels // if the code comes here, it has found the timer available @@ -85,7 +85,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive sConfigInjected.ExternalTrigInjecConv = trigger_flag; // this will be the timer with which the ADC will sync - cs_params->timer_handle = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } @@ -99,7 +99,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // display which timer is being used #ifdef SIMPLEFOC_STM32_DEBUG // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); + SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->Instance) + 1); #endif 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 6b597d4e..d3c7bd7f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -48,17 +48,17 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // if timer has repetition counter - it will downsample using it // and it does not need the software downsample - if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->Instance) ){ // adjust the initial timer state such that the trigger // - for DMA transfer aligns with the pwm peaks instead of throughs. // - for interrupt based ADC transfer // - only necessary for the timers that have repetition counters - cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -71,7 +71,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // start the adc if (use_adc_interrupt){ @@ -85,7 +85,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp index 20793d8c..8a636c84 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -133,19 +133,19 @@ uint32_t _getADCChannel(PinName pin) // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; #endif #ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->Instance == TIM5) + else if(timer->Instance == TIM5) return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; #endif else @@ -154,15 +154,15 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM2) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM2) return ADC_EXTERNALTRIGCONV_T2_TRGO; #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIGCONV_T3_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGCONV_T8_TRGO; #endif else diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h index b4549bad..05303965 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h @@ -19,11 +19,11 @@ uint32_t _getADCChannel(PinName pin); // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance int _adcToIndex(ADC_HandleTypeDef *AdcHandle); diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index fd1090ae..84a9560c 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -126,8 +126,8 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels // if the code comes here, it has found the timer available @@ -135,7 +135,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive sConfigInjected.ExternalTrigInjecConv = trigger_flag; // this will be the timer with which the ADC will sync - cs_params->timer_handle = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } 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 9c73f6d7..9be98860 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -50,17 +50,17 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // if timer has repetition counter - it will downsample using it // and it does not need the software downsample - if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->Instance) ){ // adjust the initial timer state such that the trigger // - for DMA transfer aligns with the pwm peaks instead of throughs. // - for interrupt based ADC transfer // - only necessary for the timers that have repetition counters - cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -74,7 +74,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); @@ -122,7 +122,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp index 89a9bc34..2f9e7e4d 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp @@ -133,39 +133,39 @@ uint32_t _getADCChannel(PinName pin) // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJEC_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJEC_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIGINJEC_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJEC_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGINJEC_T6_TRGO; #endif #ifdef TIM7 // if defined timer 7 - else if(timer->getHandle()->Instance == TIM7) + else if(timer->Instance == TIM7) return ADC_EXTERNALTRIGINJEC_T7_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGINJEC_T8_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIGINJEC_T15_TRGO; #endif #ifdef TIM20 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM20) + else if(timer->Instance == TIM20) return ADC_EXTERNALTRIGINJEC_T20_TRGO; #endif else @@ -174,39 +174,39 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIG_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIG_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIG_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIG_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIG_T6_TRGO; #endif #ifdef TIM7 // if defined timer 7 - else if(timer->getHandle()->Instance == TIM7) + else if(timer->Instance == TIM7) return ADC_EXTERNALTRIG_T7_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIG_T7_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIG_T15_TRGO; #endif #ifdef TIM20 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM20) + else if(timer->Instance == TIM20) return ADC_EXTERNALTRIG_T20_TRGO; #endif else diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h index fa857bd0..5a3aca29 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h @@ -19,11 +19,11 @@ uint32_t _getADCChannel(PinName pin); // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance int _adcToIndex(ADC_HandleTypeDef *AdcHandle); diff --git a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp index dc667ab3..2354fff9 100644 --- a/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp @@ -54,6 +54,16 @@ typedef struct ESP32LEDCDriverParams { } ESP32LEDCDriverParams; + +int esp32_gpio_nr(int pin) { + #if defined(BOARD_HAS_PIN_REMAP) && !defined(BOARD_USES_HW_GPIO_NUMBERS) + return digitalPinToGPIONumber(pin); + #else + return pin; + #endif +} + + /* Function to attach a channel to a pin with advanced settings - freq - pwm frequency @@ -96,7 +106,7 @@ bool _ledcAttachChannelAdvanced(uint8_t pin, int _channel, int _group, uint32_t 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.gpio_num = esp32_gpio_nr(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 diff --git a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp index e2c621c5..2b18417f 100644 --- a/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_mcpwm_mcu.cpp @@ -6,6 +6,16 @@ #pragma message("SimpleFOC: compiling for ESP32 MCPWM driver") #pragma message("") + +int esp32_gpio_nr(int pin) { + #if defined(BOARD_HAS_PIN_REMAP) && !defined(BOARD_USES_HW_GPIO_NUMBERS) + return digitalPinToGPIONumber(pin); + #else + return pin; + #endif +} + + // function setting the high pwm frequency to the supplied pins // - DC motor - 1PWM setting // - hardware specific @@ -20,7 +30,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 1PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - int pins[1] = {pinA}; + int pins[1] = { esp32_gpio_nr(pinA) }; return _configurePinsMCPWM(pwm_frequency, group, timer, 1, pins); } @@ -42,7 +52,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // 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}; + int pins[2] = { esp32_gpio_nr(pinA), esp32_gpio_nr(pinB) }; return _configurePinsMCPWM(pwm_frequency, group, timer, 2, pins); }else{ SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 2PWM as two 1PWM drivers"); @@ -92,7 +102,7 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i } SIMPLEFOC_ESP32_DRV_DEBUG("Configuring 3PWM in group: "+String(group)+" on timer: "+String(timer)); // configure the timer - int pins[3] = {pinA, pinB, pinC}; + int pins[3] = { esp32_gpio_nr(pinA), esp32_gpio_nr(pinB), esp32_gpio_nr(pinC) }; return _configurePinsMCPWM(pwm_frequency, group, timer, 3, pins); } @@ -122,7 +132,7 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in // 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}}; + int pins[2][2] = {{ esp32_gpio_nr(pinA), esp32_gpio_nr(pinB) },{ esp32_gpio_nr(pinC), esp32_gpio_nr(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)); @@ -162,7 +172,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons } 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}; + int pins[6] = { esp32_gpio_nr(pinA_h), esp32_gpio_nr(pinA_l), esp32_gpio_nr(pinB_h), esp32_gpio_nr(pinB_l), esp32_gpio_nr(pinC_h), esp32_gpio_nr(pinC_l) }; return _configure6PWMPinsMCPWM(pwm_frequency, group, timer, dead_zone, pins); } diff --git a/src/drivers/hardware_specific/portenta_h7_mcu.cpp b/src/drivers/hardware_specific/portenta_h7_mcu.cpp index ad16646a..e9807609 100644 --- a/src/drivers/hardware_specific/portenta_h7_mcu.cpp +++ b/src/drivers/hardware_specific/portenta_h7_mcu.cpp @@ -1,7 +1,7 @@ #include "../hardware_api.h" -#if defined(TARGET_PORTENTA_H7) +#if defined(TARGET_PORTENTA_H7) && false #pragma message("") diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 4009281e..91246d94 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -1,115 +1,182 @@ #include "../../hardware_api.h" -#include "stm32_mcu.h" +#include "./stm32_mcu.h" +#include "./stm32_timerutils.h" +#include "./stm32_searchtimers.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) -#define SIMPLEFOC_STM32_DEBUG #pragma message("") #pragma message("SimpleFOC: compiling for STM32") #pragma message("") -#ifdef SIMPLEFOC_STM32_DEBUG -void printTimerCombination(int numPins, PinMap* timers[], int score); -int getTimerNumber(int timerIndex); -#endif - - +/* + * Timer management + * SimpleFOC manages the timers using only STM32 HAL and LL APIs, and does not use the HardwareTimer API. + * This is because the HardwareTimer API is not available on all STM32 boards, and does not provide all + * the functionality that SimpleFOC requires anyway. + * By using the HAL and LL APIs directly, we can ensure that SimpleFOC works on all STM32 boards, specifically + * also those that use MBED with Arduino (Portenta H7, Giga, Nicla). + * + * When using stm32duino, the HardwareTimer API is available, and can be used in parallel with SimpleFOC, + * provided you don't use the same timers for both. + */ + +// track timers initialized via SimpleFOC +int numTimersUsed = 0; +TIM_HandleTypeDef* timersUsed[SIMPLEFOC_STM32_MAX_TIMERSUSED]; + +// track drivers initialized via SimpleFOC - used to know which timer channels are used +int numMotorsUsed = 0; +STM32DriverParams* motorsUsed[SIMPLEFOC_STM32_MAX_MOTORSUSED]; + +// query functions to check which timers are used +int stm32_getNumTimersUsed() { + return numTimersUsed; +} +int stm32_getNumMotorsUsed() { + return numMotorsUsed; +} +bool stm32_isTimerUsed(TIM_HandleTypeDef* timer) { + for (int i=0; itimers_handle[j] == NULL) break; + if (motorsUsed[i]->channels[j] == STM_PIN_CHANNEL(pin->function) && ((TIM_TypeDef*)pin->peripheral) == motorsUsed[i]->timers_handle[j]->Instance) + return true; + } + } + return false; +} +TIM_HandleTypeDef* stm32_getTimer(PinMap* timer) { + for (int i=0; iInstance == (TIM_TypeDef*)timer->peripheral) + return timersUsed[i]; + } + return NULL; +} +// function to get a timer handle given the pinmap entry of the pin you want to use +// after calling this function, the timer is marked as used by SimpleFOC +TIM_HandleTypeDef* stm32_useTimer(PinMap* timer) { + TIM_HandleTypeDef* handle = stm32_getTimer(timer); + if (handle != NULL) return handle; + if (numTimersUsed >= SIMPLEFOC_STM32_MAX_TIMERSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many timers used"); + return NULL; + } + handle = new TIM_HandleTypeDef(); + handle->Instance = (TIM_TypeDef*)timer->peripheral; + handle->Channel = HAL_TIM_ACTIVE_CHANNEL_CLEARED; + handle->Lock = HAL_UNLOCKED; + handle->State = HAL_TIM_STATE_RESET; + handle->hdma[0] = NULL; + handle->hdma[1] = NULL; + handle->hdma[2] = NULL; + handle->hdma[3] = NULL; + handle->hdma[4] = NULL; + handle->hdma[5] = NULL; + handle->hdma[6] = NULL; + handle->Init.Prescaler = 0; + handle->Init.Period = ((1 << 16) - 1); + handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + #if defined(TIM_RCR_REP) + handle->Init.RepetitionCounter = 1; + #endif + enableTimerClock(handle); + HAL_TIM_Base_Init(handle); + stm32_pauseTimer(handle); + timersUsed[numTimersUsed++] = handle; + return handle; +} -#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED -#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12 -#endif -int numTimerPinsUsed; -PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; bool _getPwmState(void* params) { // assume timers are synchronized and that there's at least one timer - HardwareTimer* pHT = ((STM32DriverParams*)params)->timers[0]; - TIM_HandleTypeDef* htim = pHT->getHandle(); - - bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim); - + bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(((STM32DriverParams*)params)->timers_handle[0]); return dir; } -// setting pwm to hardware pin - instead analogWrite() -void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution) -{ - // TODO - remove commented code - // PinName pin = digitalPinToPinName(ulPin); - // TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - // uint32_t index = get_timer_index(Instance); - // HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); -} - - -int getLLChannel(PinMap* timer) { -#if defined(TIM_CCER_CC1NE) - if (STM_PIN_INVERTED(timer->function)) { - switch (STM_PIN_CHANNEL(timer->function)) { - case 1: return LL_TIM_CHANNEL_CH1N; - case 2: return LL_TIM_CHANNEL_CH2N; - case 3: return LL_TIM_CHANNEL_CH3N; -#if defined(LL_TIM_CHANNEL_CH4N) - case 4: return LL_TIM_CHANNEL_CH4N; -#endif - default: return -1; - } - } else -#endif - { - switch (STM_PIN_CHANNEL(timer->function)) { - case 1: return LL_TIM_CHANNEL_CH1; - case 2: return LL_TIM_CHANNEL_CH2; - case 3: return LL_TIM_CHANNEL_CH3; - case 4: return LL_TIM_CHANNEL_CH4; - default: return -1; +void stm32_pause(STM32DriverParams* params) { + if (params->master_timer != NULL) { + stm32_pauseTimer(params->master_timer); + } + else { + for (int i=0; inum_timers; i++) { + stm32_pauseTimer(params->timers_handle[i]); } } - return -1; } +void stm32_resume(STM32DriverParams* params) { + if (params->master_timer != NULL) { + stm32_resumeTimer(params->master_timer); + } + else { + for (int i=0; inum_timers; i++) { + stm32_resumeTimer(params->timers_handle[i]); + } + } +} + // init pin pwm -HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) { +TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t mode = TIM_OCMODE_PWM1, uint32_t polarity = TIM_OCPOLARITY_HIGH, uint32_t Npolarity = TIM_OCNPOLARITY_HIGH) { // sanity check - if (timer==NP) - return NP; - uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); - bool init = false; - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - init = true; + if (timer==NULL) + return NULL; + TIM_HandleTypeDef* handle = stm32_getTimer(timer); + if (handle==NULL) { + handle = stm32_useTimer(timer); + stm32_setClockAndARR(handle, PWM_freq); // TODO add checks for PWM frequency limits } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); uint32_t channel = STM_PIN_CHANNEL(timer->function); - HT->pause(); - if (init) - HT->setOverflow(PWM_freq, HERTZ_FORMAT); - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin); - #if SIMPLEFOC_PWM_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); - #endif + TIM_OC_InitTypeDef channelOC; + channelOC.OCMode = TIM_OCMODE_PWM1; + channelOC.Pulse = 0; //__HAL_TIM_GET_COMPARE(handle, channel); + channelOC.OCPolarity = polarity; + channelOC.OCFastMode = TIM_OCFAST_DISABLE; +#if defined(TIM_CR2_OIS1) + channelOC.OCIdleState = TIM_OCIDLESTATE_RESET; //(polarity==TIM_OCPOLARITY_HIGH)?TIM_OCIDLESTATE_RESET:TIM_OCIDLESTATE_SET; +#endif +#if defined(TIM_CCER_CC1NE) + channelOC.OCNPolarity = Npolarity; +#if defined(TIM_CR2_OIS1) + channelOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; //(Npolarity==TIM_OCNPOLARITY_HIGH)?TIM_OCNIDLESTATE_RESET:TIM_OCNIDLESTATE_SET; +#endif +#endif + HAL_TIM_PWM_ConfigChannel(handle, &channelOC, stm32_getHALChannel(channel)); + pinmap_pinout(timer->pin, PinMap_TIM); + LL_TIM_CC_EnableChannel(handle->Instance, stm32_getLLChannel(timer)); + if (IS_TIM_BREAK_INSTANCE(handle->Instance)) { + __HAL_TIM_MOE_ENABLE(handle); + } + #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Configuring high timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance))); - SIMPLEFOC_DEBUG("STM32-DRV: Configuring high channel ", (int)channel); + SIMPLEFOC_DEBUG("STM32-DRV: Configuring timer ", (int)stm32_getTimerNumber(handle->Instance)); + SIMPLEFOC_DEBUG("STM32-DRV: Configuring channel ", (int)channel); #endif - return HT; + return handle; } @@ -119,380 +186,89 @@ HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) { // init high side pin -HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { - HardwareTimer* HT = _initPinPWM(PWM_freq, timer); - #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false && SIMPLEFOC_PWM_ACTIVE_HIGH==true - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); - #endif - return HT; +TIM_HandleTypeDef* _stm32_initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { + uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; + TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, timer, TIM_OCMODE_PWM1, polarity); + return handle; } // init low side pin -HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer) -{ - uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral); - - bool init = false; - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - init = true; - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - uint32_t channel = STM_PIN_CHANNEL(timer->function); - -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Configuring low timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance))); - SIMPLEFOC_DEBUG("STM32-DRV: Configuring low channel ", (int)channel); -#endif - - HT->pause(); - if (init) - HT->setOverflow(PWM_freq, HERTZ_FORMAT); - // sets internal fields of HT, but we can't set polarity here - HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin); - #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW); - #endif - return HT; +TIM_HandleTypeDef* _stm32_initPinPWMLow(uint32_t PWM_freq, PinMap* timer) { + uint32_t polarity = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; + TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, timer, TIM_OCMODE_PWM2, polarity); + return handle; } -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); -} +// // align the timers to end the init +// void _startTimers(TIM_HandleTypeDef *timers_to_start[], int timer_num) { +// stm32_alignTimers(timers_to_start, timer_num); +// } -// align the timers to end the init -void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4) -{ - HT1->pause(); - HT1->refresh(); - HT2->pause(); - HT2->refresh(); - HT3->pause(); - HT3->refresh(); - HT4->pause(); - HT4->refresh(); - HT1->resume(); - HT2->resume(); - HT3->resume(); - HT4->resume(); -} +// void _stopTimers(TIM_HandleTypeDef **timers_to_stop, int timer_num) { +// TIM_HandleTypeDef* timers_distinct[6]; +// timer_num = stm32_distinctTimers(timers_to_stop, timer_num, timers_distinct); +// for (int i=0; i < timer_num; i++) { +// if(timers_distinct[i] == NULL) return; +// stm32_pauseTimer(timers_distinct[i]); +// stm32_refreshTimer(timers_distinct[i]); +// #ifdef SIMPLEFOC_STM32_DEBUG +// SIMPLEFOC_DEBUG("STM32-DRV: Stopping timer ", stm32_getTimerNumber(timers_distinct[i]->Instance)); +// #endif +// } +// } -// align the timers to end the init -void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) -{ - // TODO - stop each timer only once - // stop timers - for (int i=0; i < timer_num; i++) { - if(timers_to_stop[i] == NP) return; - timers_to_stop[i]->pause(); - timers_to_stop[i]->refresh(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Stopping timer ", getTimerNumber(get_timer_index(timers_to_stop[i]->getHandle()->Instance))); - #endif - } -} -#if defined(STM32G4xx) -// function finds the appropriate timer source trigger for the master/slave timer combination -// returns -1 if no trigger source is found -// currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows - TIM_TypeDef *TIM_master = master->getHandle()->Instance; - #if defined(TIM1) && defined(LL_TIM_TS_ITR0) - if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; - #endif - #if defined(TIM2) && defined(LL_TIM_TS_ITR1) - else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; - #endif - #if defined(TIM3) && defined(LL_TIM_TS_ITR2) - else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; - #endif - #if defined(TIM4) && defined(LL_TIM_TS_ITR3) - else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; - #endif - #if defined(TIM5) && defined(LL_TIM_TS_ITR4) - else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; - #endif - #if defined(TIM8) && defined(LL_TIM_TS_ITR5) - else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; - #endif - return -1; -} -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) - -// function finds the appropriate timer source trigger for the master/slave timer combination -// returns -1 if no trigger source is found -// currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { - // put master and slave in temp variables to avoid arrows - TIM_TypeDef *TIM_master = master->getHandle()->Instance; - TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; - #if defined(TIM1) && defined(LL_TIM_TS_ITR0) - if (TIM_master == TIM1){ - #if defined(TIM2) - if(TIM_slave == TIM2) return LL_TIM_TS_ITR0; - #endif - #if defined(TIM3) - else if(TIM_slave == TIM3) return LL_TIM_TS_ITR0; - #endif - #if defined(TIM4) - else if(TIM_slave == TIM4) return LL_TIM_TS_ITR0; - #endif - #if defined(TIM8) - else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; - #endif - } - #endif - #if defined(TIM2) && defined(LL_TIM_TS_ITR1) - else if (TIM_master == TIM2){ - #if defined(TIM1) - if(TIM_slave == TIM1) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM3) - else if(TIM_slave == TIM3) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM4) - else if(TIM_slave == TIM4) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM8) - else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; - #endif - } - #endif - #if defined(TIM3) && defined(LL_TIM_TS_ITR2) - else if (TIM_master == TIM3){ - #if defined(TIM1) - if(TIM_slave == TIM1) return LL_TIM_TS_ITR2; - #endif - #if defined(TIM2) - else if(TIM_slave == TIM2) return LL_TIM_TS_ITR2; - #endif - #if defined(TIM4) - else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; - #endif - #if defined(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; - #endif - } - #endif - #if defined(TIM4) && defined(LL_TIM_TS_ITR3) - else if (TIM_master == TIM4){ - #if defined(TIM1) - if(TIM_slave == TIM1) return LL_TIM_TS_ITR3; - #endif - #if defined(TIM2) - else if(TIM_slave == TIM2) return LL_TIM_TS_ITR3; - #endif - #if defined(TIM3) - else if(TIM_slave == TIM3) return LL_TIM_TS_ITR3; - #endif - #if defined(TIM8) - else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; - #endif - #if defined(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; - #endif - } - #endif - #if defined(TIM5) - else if (TIM_master == TIM5){ - #if !defined(STM32L4xx) // only difference between F4,F1 and L4 - #if defined(TIM1) - if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; - #endif - #if defined(TIM3) - else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; - #endif - #endif - #if defined(TIM8) - if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; - #endif - } - #endif - #if defined(TIM8) - else if (TIM_master == TIM8){ - #if defined(TIM2) - if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM4) - else if(TIM_slave == TIM4) return LL_TIM_TS_ITR3; - #endif - #if defined(TIM5) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; - #endif - } - #endif - return -1; // combination not supported -} -#else -// Alignment not supported for this architecture -int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { - return -1; -} -#endif -void syncTimerFrequency(long pwm_frequency, HardwareTimer *timers[], uint8_t num_timers) { - uint32_t max_frequency = 0; - uint32_t min_frequency = UINT32_MAX; - for (size_t i=0; igetTimerClkFreq(); - if (freq > max_frequency) { - max_frequency = freq; - } else if (freq < min_frequency) { - min_frequency = freq; - } - } - if (max_frequency==min_frequency) return; - uint32_t overflow_value = min_frequency/pwm_frequency; - for (size_t i=0; igetTimerClkFreq()/min_frequency; - #ifdef SIMPLEFOC_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Setting prescale to ", (float)prescale_factor); - SIMPLEFOC_DEBUG("STM32-DRV: Setting Overflow to ", (float)overflow_value); - #endif - timers[i]->setPrescaleFactor(prescale_factor); - timers[i]->setOverflow(overflow_value,TICK_FORMAT); - timers[i]->refresh(); - } -} -void _alignTimersNew() { - int numTimers = 0; - HardwareTimer *timers[numTimerPinsUsed]; - - // find the timers used - for (int i=0; iperipheral); - HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - bool found = false; - for (int j=0; jInstance->ARR; + uint32_t prescaler = timers_distinct[i]->Instance->PSC; + float pwm_freq = (float)freq/(prescaler+1.0f)/(arr+1.0f)/2.0f; + if (i==0) { + common_pwm_freq = pwm_freq; + } else { + if (pwm_freq != common_pwm_freq) { + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: ERR: Timer frequency different: TIM"); + SimpleFOCDebug::print(stm32_getTimerNumber(timers_distinct[0]->Instance)); + SimpleFOCDebug::print(" freq: "); + SimpleFOCDebug::print(common_pwm_freq); + SimpleFOCDebug::print(" != TIM"); + SimpleFOCDebug::print(stm32_getTimerNumber(timers_distinct[i]->Instance)); + SimpleFOCDebug::println(" freq: ", pwm_freq); + #endif + return -1; } } - if (!found) - timers[numTimers++] = timer; } - - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); - #endif - - // see if there is more then 1 timers used for the pwm - // if yes, try to align timers - if(numTimers > 1){ - // find the master timer - int16_t master_index = -1; - int triggerEvent = -1; - for (int i=0; igetHandle()->Instance)) { - // check if timer already configured in TRGO update mode (used for ADC triggering) - // in that case we should not change its TRGO configuration - if(timers[i]->getHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; - // check if the timer has the supported internal trigger for other timers - for (int slave_i=0; slave_i 1.0f) { #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: ERR: No master timer found, cannot align timers!"); + SIMPLEFOC_DEBUG("STM32-DRV: ERR: Common timer frequency unexpected: ", common_pwm_freq); #endif - }else{ - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Aligning PWM to master timer: ", getTimerNumber(get_timer_index(timers[master_index]->getHandle()->Instance))); - #endif - // make the master timer generate ITRGx event - // if it was already configured in slave mode - LL_TIM_SetSlaveMode(timers[master_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); - // Configure the master timer to send a trigger signal on enable - LL_TIM_SetTriggerOutput(timers[master_index]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); - // configure other timers to get the input trigger from the master timer - for (int slave_index=0; slave_index < numTimers; slave_index++) { - if (slave_index == master_index) - continue; - // Configure the slave timer to be triggered by the master enable signal - LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getInternalSourceTrigger(timers[master_index], timers[slave_index])); - LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); - } - } - } - - // enable timer clock - for (int i=0; ipause(); - // timers[i]->refresh(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); - #endif + return -1; } - - for (int i=0; iresume(); - } - -} - - - -// align the timers to end the init -void _startTimers(HardwareTimer **timers_to_start, int timer_num) -{ - // // TODO - start each timer only once - // // start timers - // for (int i=0; i < timer_num; i++) { - // if(timers_to_start[i] == NP) return; - // timers_to_start[i]->resume(); - // #ifdef SIMPLEFOC_STM32_DEBUG - // SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); - // #endif - // } - _alignTimersNew(); + return 0; } // configure hardware 6pwm for a complementary pair of channels -STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { +STM32DriverParams* _stm32_initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) { // sanity check - if (pinH==NP || pinL==NP) + if (pinH==NULL || pinL==NULL) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - #if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing #endif @@ -504,44 +280,28 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* if (channel1!=channel2 || pinH->peripheral!=pinL->peripheral) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - uint32_t index = get_timer_index((TIM_TypeDef*)pinH->peripheral); - - if (HardwareTimer_Handle[index] == NULL) { - HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)pinH->peripheral); - HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1; - // HardwareTimer_Handle[index]->handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)PWM_freq, HERTZ_FORMAT); - } - HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - - HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, pinH->pin); - HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, pinL->pin); + uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; + uint32_t Npolarity = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW; + TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, pinH, TIM_OCMODE_PWM1, polarity, Npolarity); + pinmap_pinout(pinL->pin, PinMap_TIM); // also init the low side pin // dead time is set in nanoseconds uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; - uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(stm32_getTimerClockFreq(handle), LL_TIM_GetClockDivision(handle->Instance), dead_time_ns); if (dead_time>255) dead_time = 255; if (dead_time==0 && dead_zone>0) { dead_time = 255; // LL_TIM_CALC_DEADTIME returns 0 if dead_time_ns is too large SIMPLEFOC_DEBUG("STM32-DRV: WARN: dead time too large, setting to max"); } - LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! - #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW); - #endif - #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false - LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinL), LL_TIM_OCPOLARITY_LOW); - #endif - LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL)); - HT->pause(); - // make sure timer output goes to LOW when timer channels are temporarily disabled - LL_TIM_SetOffStates(HT->getHandle()->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE); - - params->timers[paramsPos] = HT; - params->timers[paramsPos+1] = HT; + // TODO why init this here, and not generally in the initPWM or init timer functions? + // or, since its a rather specialized and hardware-speific setting, why not move it to + // another function? + LL_TIM_SetOffStates(handle->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE); + LL_TIM_OC_SetDeadTime(handle->Instance, dead_time); // deadtime is non linear! + LL_TIM_CC_EnableChannel(handle->Instance, stm32_getLLChannel(pinH) | stm32_getLLChannel(pinL)); + params->timers_handle[paramsPos] = handle; + params->timers_handle[paramsPos+1] = handle; params->channels[paramsPos] = channel1; params->channels[paramsPos+1] = channel2; return params; @@ -550,22 +310,24 @@ STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* -STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { +STM32DriverParams* _stm32_initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) { STM32DriverParams* params = new STM32DriverParams { - .timers = { NP, NP, NP, NP, NP, NP }, + .timers_handle = { NULL, NULL, NULL, NULL, NULL, NULL }, .channels = { 0, 0, 0, 0, 0, 0 }, + .llchannels = { 0, 0, 0, 0, 0, 0 }, .pwm_frequency = PWM_freq, + .num_timers = 0, + .master_timer = NULL, .dead_zone = dead_zone, .interface_type = _HARDWARE_6PWM }; - - if (_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) + if (_stm32_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - if(_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) + if(_stm32_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - if (_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) + if (_stm32_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - + params->num_timers = stm32_countTimers(params->timers_handle, 6); return params; } @@ -574,189 +336,9 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi -/* - timer combination scoring function - assigns a score, and also checks the combination is valid - returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better - for 6 pwm, hardware 6-pwm is preferred over software 6-pwm - hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel - inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things) -*/ -int scoreCombination(int numPins, PinMap* pinTimers[]) { - // check already used - TODO move this to outer loop also... - for (int i=0; iperipheral == timerPinsUsed[i]->peripheral - && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(timerPinsUsed[i]->function)) - return -2; // bad combination - timer channel already used - } - - // TODO LPTIM and HRTIM should be ignored for now - - // check for inverted channels - if (numPins < 6) { - for (int i=0; ifunction)) - return -3; // bad combination - inverted channel used in non-hardware 6pwm - } - } - // check for duplicated channels - for (int i=0; iperipheral == pinTimers[j]->peripheral - && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function) - && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function)) - return -4; // bad combination - duplicated channel - } - } - int score = 0; - for (int i=0; iperipheral == pinTimers[j]->peripheral) - found = true; - } - if (!found) score++; - } - if (numPins==6) { - // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels - // >1 timer, 3 channels, 3 matching inverted channels - // 1 timer, 6 channels (no inverted channels) - // >1 timer, 6 channels (no inverted channels) - // check for inverted high-side channels - TODO is this a configuration we should allow? what if all 3 high side channels are inverted and the low-side non-inverted? - if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) - return -5; // bad combination - inverted channel used on high-side channel - if (pinTimers[0]->peripheral == pinTimers[1]->peripheral - && pinTimers[2]->peripheral == pinTimers[3]->peripheral - && pinTimers[4]->peripheral == pinTimers[5]->peripheral - && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) - && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) - && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) - && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { - // hardware 6pwm, score <10 - - // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8 - // F301, F302 --> 6 channels, but only 1-3 have dead-time insertion - // TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion - // TIM15/TIM16/TIM17 do dead-time insertion only on channel 1 - - // TODO check these defines - #if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H) || defined(STM32G0x0_HAL_TIM_H) - if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 ) - return -8; // channel 4 does not have dead-time insertion - #endif - #ifdef STM32G4xx_HAL_TIM_H - if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 ) - return -8; // channels 5 & 6 do not have dead-time insertion - #endif - } - else { - // check for inverted low-side channels - if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function)) - return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm - if (pinTimers[0]->peripheral != pinTimers[1]->peripheral - || pinTimers[2]->peripheral != pinTimers[3]->peripheral - || pinTimers[4]->peripheral != pinTimers[5]->peripheral) - return -7; // bad combination - non-matching timers for H/L side in software 6-pwm - score += 10; // software 6pwm, score >10 - } - } - return score; -} - - - - -int findIndexOfFirstPinMapEntry(int pin) { - PinName pinName = digitalPinToPinName(pin); - int i = 0; - while (PinMap_TIM[i].pin!=NC) { - if (pinName == PinMap_TIM[i].pin) - return i; - i++; - } - return -1; -} - - -int findIndexOfLastPinMapEntry(int pin) { - PinName pinName = digitalPinToPinName(pin); - int i = 0; - while (PinMap_TIM[i].pin!=NC) { - if ( pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) - && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK)) - return i; - i++; - } - return -1; -} - - - - - - -#define NOT_FOUND 1000 - -int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) { - PinMap* searchArray[numPins]; - for (int j=0;j=0 && score= 0) { - #ifdef SIMPLEFOC_STM32_DEBUG - SimpleFOCDebug::print("STM32-DRV: best: "); - printTimerCombination(numPins, pinTimers, bestScore); - #endif - } - return bestScore; -}; - - - void* _configure1PWM(long pwm_frequency, const int pinA) { - if (numTimerPinsUsed+1 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -766,22 +348,24 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { pwm_frequency *=2; int pins[1] = { pinA }; - PinMap* pinTimers[1] = { NP }; - if (findBestTimerCombination(1, pins, pinTimers)<0) + PinMap* pinTimers[1] = { NULL }; + if (stm32_findBestTimerCombination(1, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ - // align the timers - _alignTimersNew(); - + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1 }, + .timers_handle = { HT1 }, .channels = { channel1 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]) }, + .pwm_frequency = pwm_frequency, + .num_timers = 1, + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; + // align the timers (in this case, just start them) + params->master_timer = stm32_alignTimers(params->timers_handle, 1); + motorsUsed[numMotorsUsed++] = params; return params; } @@ -793,8 +377,8 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // - Stepper motor - 2PWM setting // - hardware speciffic void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { - if (numTimerPinsUsed+2 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } @@ -804,76 +388,74 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { pwm_frequency *=2; int pins[2] = { pinA, pinB }; - PinMap* pinTimers[2] = { NP, NP }; - if (findBestTimerCombination(2, pins, pinTimers)<0) + PinMap* pinTimers[2] = { NULL, NULL }; + if (stm32_findBestTimerCombination(2, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - HardwareTimer *timers[2] = {HT1, HT2}; - syncTimerFrequency(pwm_frequency, timers, 2); - // allign the timers - _alignPWMTimers(HT1, HT2, HT2); + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT2 = stm32_initPinPWM(pwm_frequency, pinTimers[1], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef *timers[2] = {HT1, HT2}; + stm32_checkTimerFrequency(pwm_frequency, timers, 2); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2 }, + .timers_handle = { HT1, HT2 }, .channels = { channel1, channel2 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]) }, + .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 2), + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; + // align the timers + params->master_timer = stm32_alignTimers(timers, 2); + motorsUsed[numMotorsUsed++] = params; return params; } -TIM_MasterConfigTypeDef sMasterConfig; -TIM_SlaveConfigTypeDef sSlaveConfig; - // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { - if (numTimerPinsUsed+3 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } 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 // center-aligned frequency is uses two periods - pwm_frequency *=2; + //pwm_frequency *=2; int pins[3] = { pinA, pinB, pinC }; - PinMap* pinTimers[3] = { NP, NP, NP }; - if (findBestTimerCombination(3, pins, pinTimers)<0) + PinMap* pinTimers[3] = { NULL, NULL, NULL }; + if (stm32_findBestTimerCombination(3, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT2 = stm32_initPinPWM(pwm_frequency, pinTimers[1], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT3 = stm32_initPinPWM(pwm_frequency, pinTimers[2], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); - HardwareTimer *timers[3] = {HT1, HT2, HT3}; - syncTimerFrequency(pwm_frequency, timers, 3); + TIM_HandleTypeDef *timers[3] = {HT1, HT2, HT3}; + stm32_checkTimerFrequency(pwm_frequency, timers, 3); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2, HT3 }, + .timers_handle = { HT1, HT2, HT3 }, .channels = { channel1, channel2, channel3 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]), stm32_getLLChannel(pinTimers[2]) }, + .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 3), + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; - - _alignTimersNew(); - + params->master_timer = stm32_alignTimers(timers, 3); + motorsUsed[numMotorsUsed++] = params; return params; } @@ -883,8 +465,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in // - Stepper motor - 4PWM setting // - hardware speciffic void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { - if (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz @@ -893,18 +475,16 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in pwm_frequency *=2; int pins[4] = { pinA, pinB, pinC, pinD }; - PinMap* pinTimers[4] = { NP, NP, NP, NP }; - if (findBestTimerCombination(4, pins, pinTimers)<0) + PinMap* pinTimers[4] = { NULL, NULL, NULL, NULL }; + if (stm32_findBestTimerCombination(4, pins, pinTimers)<0) return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; - HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); - HardwareTimer *timers[4] = {HT1, HT2, HT3, HT4}; - syncTimerFrequency(pwm_frequency, timers, 4); - // allign the timers - _alignPWMTimers(HT1, HT2, HT3, HT4); + TIM_HandleTypeDef* HT1 = stm32_initPinPWM(pwm_frequency, pinTimers[0], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT2 = stm32_initPinPWM(pwm_frequency, pinTimers[1], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT3 = stm32_initPinPWM(pwm_frequency, pinTimers[2], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef* HT4 = stm32_initPinPWM(pwm_frequency, pinTimers[3], TIM_OCMODE_PWM1, (SIMPLEFOC_PWM_ACTIVE_HIGH)?TIM_OCPOLARITY_HIGH:TIM_OCPOLARITY_LOW); + TIM_HandleTypeDef *timers[4] = {HT1, HT2, HT3, HT4}; + stm32_checkTimerFrequency(pwm_frequency, timers, 4); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); @@ -912,14 +492,15 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function); STM32DriverParams* params = new STM32DriverParams { - .timers = { HT1, HT2, HT3, HT4 }, + .timers_handle = { HT1, HT2, HT3, HT4 }, .channels = { channel1, channel2, channel3, channel4 }, - .pwm_frequency = pwm_frequency + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]), stm32_getLLChannel(pinTimers[2]), stm32_getLLChannel(pinTimers[3]) }, + .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 4), + .master_timer = NULL }; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[0]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[1]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[2]; - timerPinsUsed[numTimerPinsUsed++] = pinTimers[3]; + params->master_timer = stm32_alignTimers(timers, 4); + motorsUsed[numMotorsUsed++] = params; return params; } @@ -927,43 +508,55 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in // function setting the pwm duty cycle to the hardware // - DC motor - 1PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle1PWM(float dc_a, void* params){ - // transform duty cycle from [0,1] to [0,255] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); + uint32_t duty = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -//- hardware speciffic +//- hardware specific void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ - // transform duty cycle from [0,1] to [0,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); + uint32_t duty1 = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + uint32_t duty2 = dc_b*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty2); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -//- hardware speciffic +//- hardware specific void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // transform duty cycle from [0,1] to [0,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION); + uint32_t duty1 = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + uint32_t duty2 = dc_b*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + uint32_t duty3 = dc_c*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty2); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty3); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting -//- hardware speciffic +//- hardware specific void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ - // transform duty cycle from [0,1] to [0,4095] - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_1a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_1b, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_2a, _PWM_RESOLUTION); - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION); + uint32_t duty1 = dc_1a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + uint32_t duty2 = dc_1b*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + uint32_t duty3 = dc_2a*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + uint32_t duty4 = dc_2b*(((STM32DriverParams*)params)->timers_handle[3]->Instance->ARR+1); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty2); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty3); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[3], ((STM32DriverParams*)params)->channels[3], duty4); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); } @@ -973,8 +566,8 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo // - 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 (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) { - SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used"); + if (numMotorsUsed+1 > SIMPLEFOC_STM32_MAX_MOTORSUSED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz @@ -984,24 +577,24 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // find configuration int pins[6] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }; - PinMap* pinTimers[6] = { NP, NP, NP, NP, NP, NP }; - int score = findBestTimerCombination(6, pins, pinTimers); + PinMap* pinTimers[6] = { NULL, NULL, NULL, NULL, NULL, NULL }; + int score = stm32_findBestTimerCombination(6, pins, pinTimers); STM32DriverParams* params; // configure accordingly if (score<0) params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; else if (score<10) // hardware pwm - params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]); + params = _stm32_initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]); else { // software pwm - HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]); - HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]); - HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]); - HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); - HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); - HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); - HardwareTimer *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; - syncTimerFrequency(pwm_frequency, timers, 6); + TIM_HandleTypeDef* HT1 = _stm32_initPinPWMHigh(pwm_frequency, pinTimers[0]); + TIM_HandleTypeDef* HT2 = _stm32_initPinPWMLow(pwm_frequency, pinTimers[1]); + TIM_HandleTypeDef* HT3 = _stm32_initPinPWMHigh(pwm_frequency, pinTimers[2]); + TIM_HandleTypeDef* HT4 = _stm32_initPinPWMLow(pwm_frequency, pinTimers[3]); + TIM_HandleTypeDef* HT5 = _stm32_initPinPWMHigh(pwm_frequency, pinTimers[4]); + TIM_HandleTypeDef* HT6 = _stm32_initPinPWMLow(pwm_frequency, pinTimers[5]); + TIM_HandleTypeDef *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; + stm32_checkTimerFrequency(pwm_frequency, timers, 6); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -1009,33 +602,41 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function); uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function); params = new STM32DriverParams { - .timers = { HT1, HT2, HT3, HT4, HT5, HT6 }, + .timers_handle = { HT1, HT2, HT3, HT4, HT5, HT6 }, .channels = { channel1, channel2, channel3, channel4, channel5, channel6 }, + .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]), stm32_getLLChannel(pinTimers[2]), stm32_getLLChannel(pinTimers[3]), stm32_getLLChannel(pinTimers[4]), stm32_getLLChannel(pinTimers[5]) }, .pwm_frequency = pwm_frequency, + .num_timers = stm32_countTimers(timers, 6), + .master_timer = NULL, .dead_zone = dead_zone, .interface_type = _SOFTWARE_6PWM }; } if (score>=0) { - for (int i=0; i<6; i++) - timerPinsUsed[numTimerPinsUsed++] = pinTimers[i]; - _alignTimersNew(); + params->master_timer = stm32_alignTimers(params->timers_handle, 6); + motorsUsed[numMotorsUsed++] = params; } return params; // success } -void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int channel2) { - _UNUSED(channel2); +void _setSinglePhaseState(PhaseState state, TIM_HandleTypeDef *HT, int llchannel_hi, int llchannel_lo) { switch (state) { case PhaseState::PHASE_OFF: - // Due to a weird quirk in the arduino timer API, pauseChannel only disables the complementary channel (e.g. CC1NE). - // To actually make the phase floating, we must also set pwm to 0. - HT->pauseChannel(channel1); + stm32_pauseChannel(HT, llchannel_hi | llchannel_lo); + break; + case PhaseState::PHASE_HI: + stm32_pauseChannel(HT, llchannel_lo); + stm32_resumeChannel(HT, llchannel_hi); + break; + case PhaseState::PHASE_LO: + stm32_pauseChannel(HT, llchannel_hi); + stm32_resumeChannel(HT, llchannel_lo); break; + case PhaseState::PHASE_ON: default: - HT->resumeChannel(channel1); + stm32_resumeChannel(HT, llchannel_hi | llchannel_lo); break; } } @@ -1045,146 +646,72 @@ void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState* phase_state, void* params){ + uint32_t duty1; + uint32_t duty2; + uint32_t duty3; switch(((STM32DriverParams*)params)->interface_type){ case _HARDWARE_6PWM: - // phase a - _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], ((STM32DriverParams*)params)->channels[1]); - if(phase_state[0] == PhaseState::PHASE_OFF) dc_a = 0.0f; - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION); - // phase b - _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], ((STM32DriverParams*)params)->channels[3]); - if(phase_state[1] == PhaseState::PHASE_OFF) dc_b = 0.0f; - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION); - // phase c - _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], ((STM32DriverParams*)params)->channels[5]); - if(phase_state[2] == PhaseState::PHASE_OFF) dc_c = 0.0f; - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION); + duty1 = dc_a*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + duty2 = dc_b*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + duty3 = dc_c*(((STM32DriverParams*)params)->timers_handle[4]->Instance->ARR+1); + if(phase_state[0] == PhaseState::PHASE_OFF) duty1 = 0; + if(phase_state[1] == PhaseState::PHASE_OFF) duty2 = 0; + if(phase_state[2] == PhaseState::PHASE_OFF) duty3 = 0; + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->llchannels[0], ((STM32DriverParams*)params)->llchannels[1]); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); + _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->llchannels[2], ((STM32DriverParams*)params)->llchannels[3]); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty2); + _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->llchannels[4], ((STM32DriverParams*)params)->llchannels[5]); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->channels[4], duty3); + if (((STM32DriverParams*)params)->num_timers == 1) LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); break; case _SOFTWARE_6PWM: float dead_zone = ((STM32DriverParams*)params)->dead_zone / 2.0f; + duty1 = _constrain(dc_a - dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[0]->Instance->ARR+1); + duty2 = _constrain(dc_b - dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[2]->Instance->ARR+1); + duty3 = _constrain(dc_c - dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[4]->Instance->ARR+1); + uint32_t duty1N = _constrain(dc_a + dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[1]->Instance->ARR+1); + uint32_t duty2N = _constrain(dc_b + dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[3]->Instance->ARR+1); + uint32_t duty3N = _constrain(dc_c + dead_zone, 0.0f, 1.0f)*(((STM32DriverParams*)params)->timers_handle[5]->Instance->ARR+1); + + LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); // timers for high and low side assumed to be the same timer if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_HI) - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], duty1); else - _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[0], ((STM32DriverParams*)params)->channels[0], 0); if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_LO) - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], duty1N); else - _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[1], ((STM32DriverParams*)params)->channels[1], 0); + LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[2]->Instance); // timers for high and low side assumed to be the same timer if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_HI) - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], duty2); else - _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[2], ((STM32DriverParams*)params)->channels[2], 0); if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_LO) - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[3], ((STM32DriverParams*)params)->channels[3], duty2N); else - _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[3], ((STM32DriverParams*)params)->channels[3], 0); + LL_TIM_DisableUpdateEvent(((STM32DriverParams*)params)->timers_handle[4]->Instance); // timers for high and low side assumed to be the same timer if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_HI) - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->channels[4], duty3); else - _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[4], ((STM32DriverParams*)params)->channels[4], 0); if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_LO) - _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[5], ((STM32DriverParams*)params)->channels[5], duty3N); else - _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], 0.0f, _PWM_RESOLUTION); + stm32_setPwm(((STM32DriverParams*)params)->timers_handle[5], ((STM32DriverParams*)params)->channels[5], 0); + + LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[0]->Instance); + LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[2]->Instance); + LL_TIM_EnableUpdateEvent(((STM32DriverParams*)params)->timers_handle[4]->Instance); break; } - _UNUSED(phase_state); -} - - - -#ifdef SIMPLEFOC_STM32_DEBUG - -int getTimerNumber(int timerIndex) { - #if defined(TIM1_BASE) - if (timerIndex==TIMER1_INDEX) return 1; - #endif - #if defined(TIM2_BASE) - if (timerIndex==TIMER2_INDEX) return 2; - #endif - #if defined(TIM3_BASE) - if (timerIndex==TIMER3_INDEX) return 3; - #endif - #if defined(TIM4_BASE) - if (timerIndex==TIMER4_INDEX) return 4; - #endif - #if defined(TIM5_BASE) - if (timerIndex==TIMER5_INDEX) return 5; - #endif - #if defined(TIM6_BASE) - if (timerIndex==TIMER6_INDEX) return 6; - #endif - #if defined(TIM7_BASE) - if (timerIndex==TIMER7_INDEX) return 7; - #endif - #if defined(TIM8_BASE) - if (timerIndex==TIMER8_INDEX) return 8; - #endif - #if defined(TIM9_BASE) - if (timerIndex==TIMER9_INDEX) return 9; - #endif - #if defined(TIM10_BASE) - if (timerIndex==TIMER10_INDEX) return 10; - #endif - #if defined(TIM11_BASE) - if (timerIndex==TIMER11_INDEX) return 11; - #endif - #if defined(TIM12_BASE) - if (timerIndex==TIMER12_INDEX) return 12; - #endif - #if defined(TIM13_BASE) - if (timerIndex==TIMER13_INDEX) return 13; - #endif - #if defined(TIM14_BASE) - if (timerIndex==TIMER14_INDEX) return 14; - #endif - #if defined(TIM15_BASE) - if (timerIndex==TIMER15_INDEX) return 15; - #endif - #if defined(TIM16_BASE) - if (timerIndex==TIMER16_INDEX) return 16; - #endif - #if defined(TIM17_BASE) - if (timerIndex==TIMER17_INDEX) return 17; - #endif - #if defined(TIM18_BASE) - if (timerIndex==TIMER18_INDEX) return 18; - #endif - #if defined(TIM19_BASE) - if (timerIndex==TIMER19_INDEX) return 19; - #endif - #if defined(TIM20_BASE) - if (timerIndex==TIMER20_INDEX) return 20; - #endif - #if defined(TIM21_BASE) - if (timerIndex==TIMER21_INDEX) return 21; - #endif - #if defined(TIM22_BASE) - if (timerIndex==TIMER22_INDEX) return 22; - #endif - return -1; } -void printTimerCombination(int numPins, PinMap* timers[], int score) { - for (int i=0; iperipheral))); - SimpleFOCDebug::print("-CH"); - SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function)); - if (STM_PIN_INVERTED(timers[i]->function)) - SimpleFOCDebug::print("N"); - } - SimpleFOCDebug::print(" "); - } - SimpleFOCDebug::println("score: ", score); -} - -#endif #endif diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index 411c43b2..6909071f 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -1,8 +1,28 @@ -#ifndef STM32_DRIVER_MCU_DEF -#define STM32_DRIVER_MCU_DEF +#pragma once + #include "../../hardware_api.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) + +#ifndef SIMPLEFOC_STM32_MAX_TIMERSUSED +#define SIMPLEFOC_STM32_MAX_TIMERSUSED 6 +#endif +#ifndef SIMPLEFOC_STM32_MAX_MOTORSUSED +#define SIMPLEFOC_STM32_MAX_MOTORSUSED 4 +#endif + + +#ifndef SIMPLEFOC_STM32_DEBUG +// comment me out to disable debug output +#define SIMPLEFOC_STM32_DEBUG +#endif + +#if defined(__MBED__) +#define PinMap_TIM PinMap_PWM +#define ALTX_MASK 0 +#endif + + // default pwm parameters #define _PWM_RESOLUTION 12 // 12bit @@ -17,19 +37,34 @@ typedef struct STM32DriverParams { - HardwareTimer* timers[6] = {NULL}; + TIM_HandleTypeDef* timers_handle[6] = {NULL}; uint32_t channels[6]; + uint32_t llchannels[6]; long pwm_frequency; + uint8_t num_timers; + TIM_HandleTypeDef* master_timer = NULL; float dead_zone; uint8_t interface_type; } STM32DriverParams; -// timer synchornisation functions -void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6); -void _startTimers(HardwareTimer **timers_to_start, int timer_num=6); -// timer query functions -bool _getPwmState(void* params); +// timer allocation functions +int stm32_getNumTimersUsed(); +int stm32_getNumMotorsUsed(); +STM32DriverParams* stm32_getMotorUsed(int index); +bool stm32_isTimerUsed(TIM_HandleTypeDef* timer); +bool stm32_isChannelUsed(PinMap* pin); +TIM_HandleTypeDef* stm32_getTimer(PinMap* timer); +TIM_HandleTypeDef* stm32_useTimer(PinMap* timer); + +void stm32_pause(STM32DriverParams* params); +void stm32_resume(STM32DriverParams* params); + +// // timer synchornisation functions +// void _stopTimers(TIM_HandleTypeDef **timers_to_stop, int timer_num=6); +// void _startTimers(TIM_HandleTypeDef **timers_to_start, int timer_num=6); + +// // timer query functions +// bool _getPwmState(void* params); -#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp b/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp new file mode 100644 index 00000000..052142a6 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp @@ -0,0 +1,193 @@ + +#include "./stm32_searchtimers.h" +#include "./stm32_timerutils.h" + +#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) + + + +/* + timer combination scoring function + assigns a score, and also checks the combination is valid + returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better + for 6 pwm, hardware 6-pwm is preferred over software 6-pwm + hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel + inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things) +*/ +int _stm32_scoreCombination(int numPins, PinMap* pinTimers[]) { + // check already used - TODO move this to outer loop also... + for (int i=0; ifunction)) + return -3; // bad combination - inverted channel used in non-hardware 6pwm + } + } + // check for duplicated channels + for (int i=0; iperipheral == pinTimers[j]->peripheral + && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function) + && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function)) + return -4; // bad combination - duplicated channel + } + } + int score = 0; + for (int i=0; iperipheral == pinTimers[j]->peripheral) + found = true; + } + if (!found) score++; + } + if (numPins==6) { + // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels + // >1 timer, 3 channels, 3 matching inverted channels + // 1 timer, 6 channels (no inverted channels) + // >1 timer, 6 channels (no inverted channels) + // check for inverted high-side channels + if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function)) + return -5; // bad combination - inverted channel used on high-side channel + if (pinTimers[0]->peripheral == pinTimers[1]->peripheral + && pinTimers[2]->peripheral == pinTimers[3]->peripheral + && pinTimers[4]->peripheral == pinTimers[5]->peripheral + && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function) + && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function) + && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function) + && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) { + // hardware 6pwm, score <10 + + // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8 + // F301, F302 --> 6 channels, but only 1-3 have dead-time insertion + // TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion + // TIM15/TIM16/TIM17 do dead-time insertion only on channel 1 + + // TODO check these defines + #if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H) || defined(STM32G0x0_HAL_TIM_H) + if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 ) + return -8; // channel 4 does not have dead-time insertion + #endif + #ifdef STM32G4xx_HAL_TIM_H + if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 ) + return -8; // channels 5 & 6 do not have dead-time insertion + #endif + } + else { + // check for inverted low-side channels + if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function)) + return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm + if (pinTimers[0]->peripheral != pinTimers[1]->peripheral + || pinTimers[2]->peripheral != pinTimers[3]->peripheral + || pinTimers[4]->peripheral != pinTimers[5]->peripheral) + return -7; // bad combination - non-matching timers for H/L side in software 6-pwm + score += 10; // software 6pwm, score >10 + } + } + return score; +} + + + + +int _stm32_findIndexOfFirstPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if (pinName == PinMap_TIM[i].pin) + return i; + i++; + } + return -1; +} + + +int _stm32_findIndexOfLastPinMapEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_TIM[i].pin!=NC) { + if ( pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) + && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} + + + + + + +#define NOT_FOUND 1000 + +int _stm32_findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) { + PinMap* searchArray[6] = { NULL, NULL, NULL, NULL, NULL, NULL }; + for (int j=0;j=0 && score= 0) { + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: best: "); + stm32_printTimerCombination(numPins, pinTimers, bestScore); + #endif + } + return bestScore; +}; + + + + + +#endif + diff --git a/src/drivers/hardware_specific/stm32/stm32_searchtimers.h b/src/drivers/hardware_specific/stm32/stm32_searchtimers.h new file mode 100644 index 00000000..68c3fcd7 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_searchtimers.h @@ -0,0 +1,11 @@ +#pragma once + +#include "./stm32_mcu.h" + +#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) + + +int stm32_findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]); + + +#endif diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp new file mode 100644 index 00000000..31d0eab6 --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp @@ -0,0 +1,902 @@ + +#include "./stm32_timerutils.h" +#include + +#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) + + +void stm32_pauseTimer(TIM_HandleTypeDef* handle){ + /* Disable timer unconditionally. Required to guarantee timer is stopped, + * even if some channels are still running */ + LL_TIM_DisableCounter(handle->Instance); +// handle->State = HAL_TIM_STATE_READY; +} + + +void stm32_resumeTimer(TIM_HandleTypeDef* handle){ + LL_TIM_EnableCounter(handle->Instance); +} + + +void stm32_refreshTimer(TIM_HandleTypeDef* handle){ + handle->Instance->EGR = TIM_EVENTSOURCE_UPDATE; +} + + +void stm32_pauseChannel(TIM_HandleTypeDef* handle, uint32_t llchannels){ + LL_TIM_CC_DisableChannel(handle->Instance, llchannels); +} + + +void stm32_resumeChannel(TIM_HandleTypeDef* handle, uint32_t llchannels){ + LL_TIM_CC_EnableChannel(handle->Instance, llchannels); +} + + + + + + + + +uint32_t stm32_setClockAndARR(TIM_HandleTypeDef* handle, uint32_t PWM_freq) { + // set the clock + uint32_t arr_value = 0; + uint32_t cycles = stm32_getTimerClockFreq(handle) / PWM_freq / 2; + uint32_t prescaler = (cycles / 0x10000) + 1; // TODO consider 32 bit timers + LL_TIM_SetPrescaler(handle->Instance, prescaler - 1); + uint32_t ticks = cycles / prescaler; + if (ticks > 0) { + arr_value = ticks - 1; + } + __HAL_TIM_SET_AUTORELOAD(handle, arr_value); + stm32_refreshTimer(handle); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Timer clock: ", (int)stm32_getTimerClockFreq(handle)); + SIMPLEFOC_DEBUG("STM32-DRV: Timer prescaler: ", (int)prescaler); + SIMPLEFOC_DEBUG("STM32-DRV: Timer ARR: ", (int)arr_value); + #endif + return arr_value; +} + + + + + +// setting pwm to hardware pin - instead analogWrite() +void stm32_setPwm(TIM_HandleTypeDef *timer, uint32_t channel, uint32_t value) { + // value assumed to be in correct range + switch (channel) { + case 1: timer->Instance->CCR1 = value; break; + case 2: timer->Instance->CCR2 = value; break; + case 3: timer->Instance->CCR3 = value; break; + case 4: timer->Instance->CCR4 = value; break; + #ifdef LL_TIM_CHANNEL_CH5 + case 5: timer->Instance->CCR5 = value; break; + #endif + #ifdef LL_TIM_CHANNEL_CH6 + case 6: timer->Instance->CCR6 = value; break; + #endif + default: break; + } +} + + +uint32_t stm32_getHALChannel(uint32_t channel) { + uint32_t return_value; + switch (channel) { + case 1: + return_value = TIM_CHANNEL_1; + break; + case 2: + return_value = TIM_CHANNEL_2; + break; + case 3: + return_value = TIM_CHANNEL_3; + break; + case 4: + return_value = TIM_CHANNEL_4; + break; + #ifdef TIM_CHANNEL_5 + case 5: + return_value = TIM_CHANNEL_5; + break; + #endif + #ifdef TIM_CHANNEL_6 + case 6: + return_value = TIM_CHANNEL_6; + break; + #endif + default: + return_value = -1; + } + return return_value; +} + + + +uint32_t stm32_getLLChannel(PinMap* timer) { +#if defined(TIM_CCER_CC1NE) + if (STM_PIN_INVERTED(timer->function)) { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1N; + case 2: return LL_TIM_CHANNEL_CH2N; + case 3: return LL_TIM_CHANNEL_CH3N; +#if defined(LL_TIM_CHANNEL_CH4N) + case 4: return LL_TIM_CHANNEL_CH4N; +#endif + default: return -1; + } + } else +#endif + { + switch (STM_PIN_CHANNEL(timer->function)) { + case 1: return LL_TIM_CHANNEL_CH1; + case 2: return LL_TIM_CHANNEL_CH2; + case 3: return LL_TIM_CHANNEL_CH3; + case 4: return LL_TIM_CHANNEL_CH4; + #ifdef LL_TIM_CHANNEL_CH5 + case 5: return LL_TIM_CHANNEL_CH5; + #endif + #ifdef LL_TIM_CHANNEL_CH6 + case 6: return LL_TIM_CHANNEL_CH6; + #endif + default: return -1; + } + } + return -1; +} + + + +uint8_t stm32_countTimers(TIM_HandleTypeDef *timers[], uint8_t num_timers) { + uint8_t count = 1; + for (int i=1; iInstance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + #endif + #if defined(TIM5) && defined(LL_TIM_TS_ITR4) + else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + #endif + #if defined(TIM8) && defined(LL_TIM_TS_ITR5) + else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + #endif + return -1; +} +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) + +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* slave) { + // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->Instance; + TIM_TypeDef *TIM_slave = slave->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1){ + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM2) + else if(TIM_slave == TIM2) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM5) + else if (TIM_master == TIM5){ + #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; + #endif + } + #endif + #if defined(TIM8) + else if (TIM_master == TIM8){ + #if defined(TIM2) + if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM4) + else if(TIM_slave == TIM4) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif + } + #endif + return -1; // combination not supported +} +#else +// Alignment not supported for this architecture +int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* slave) { + return -1; +} +#endif + + + + + +// call with the timers used for a motor. The function will align the timers and +// start them at the same time (if possible). Results of this function can be: +// - success, no changes needed +// - success, different timers aligned and started concurrently +// - failure, cannot align timers +// in each case, the timers are started. +// +// TODO: this topic is quite complex if we allow multiple motors and multiple timers per motor. +// that's because the motors could potentially share the same timer. In this case aligning +// the timers is problematic. +// Even more problematic is stopping and resetting the timers. Even with a single motor, +// this would cause problems and mis-align the PWM signals. +// We can handle three cases: +// - only one timer needed, no need to align +// - master timer can be found, and timers used by only this motor: alignment possible +// - TODO all timers for all motors can be aligned to one master: alignment possible +// - otherwise, alignment not possible +// frequency alignment is based on checking their pwm frequencies match. +// pwm alignment is based on linking timers to start them at the same time, and making sure they are reset in sync. +// On newer chips supporting it (STM32G4) we use gated + reset mode to start/stop only the master timer. Slaves +// are started/stopped automatically with the master. TODO probably just using gated mode is better +// On older chips (STM32F1) we used gated mode to start/stop the slave timers with the master, but have to take +// care of the reset manually. TODO is it really needed to reset the timers? +TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num_timers_in) { + // find the timers used + TIM_HandleTypeDef *timers[6]; + int numTimers = stm32_distinctTimers(timers_in, num_timers_in, timers); + + // compare with the existing timers used for other motors... + for (int i=0; itimers_handle[k] == NULL) break; + if (params->timers_handle[k] == timers[i]) { + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: ERR: Timer already used by another motor: TIM", stm32_getTimerNumber(timers[i]->Instance)); + #endif + // TODO handle this case, and make it a warning + // two sub-cases we could handle at the moment: + // - the other motor does not have a master timer, so we can initialize this motor without a master also + } + } + } + } + + + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); + #endif + + // see if there is more then 1 timers used for the pwm + // if yes, try to align timers + if(numTimers > 1){ + // find the master timer + int16_t master_index = -1; + int triggerEvent = -1; + for (int i=0; iInstance)) { + // check if timer already configured in TRGO update mode (used for ADC triggering) + // in that case we should not change its TRGO configuration + if(timers[i]->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; + // check if the timer has the supported internal trigger for other timers + for (int slave_i=0; slave_iInstance)); + #endif + // make the master timer generate ITRGx event + // if it was already configured in slave mode, disable the slave mode on the master + LL_TIM_SetSlaveMode(timers[master_index]->Instance, LL_TIM_SLAVEMODE_DISABLED ); + // Configure the master timer to send a trigger signal on enable + LL_TIM_SetTriggerOutput(timers[master_index]->Instance, LL_TIM_TRGO_ENABLE); + LL_TIM_EnableMasterSlaveMode(timers[master_index]->Instance); + + // configure other timers to get the input trigger from the master timer + for (int slave_index=0; slave_index < numTimers; slave_index++) { + if (slave_index == master_index) + continue; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: slave timer: TIM", stm32_getTimerNumber(timers[slave_index]->Instance)); + #endif + // Configure the slave timer to be triggered by the master enable signal + LL_TIM_SetTriggerInput(timers[slave_index]->Instance, stm32_getInternalSourceTrigger(timers[master_index], timers[slave_index])); + #if defined(STM32G4xx) + LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_COMBINED_GATEDRESET); + #else + LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_GATED); + #endif + } + stm32_resumeTimer(timers[master_index]); // start the master to start all timers + return timers[master_index]; + } + } + + // if we had only one timer, or there was no master timer found + for (int i=0; iInstance); + uint64_t clkSelection = timerClkSrc == 1 ? RCC_PERIPHCLK_TIMG1 : RCC_PERIPHCLK_TIMG2; + return HAL_RCCEx_GetPeriphCLKFreq(clkSelection); +#else + RCC_ClkInitTypeDef clkconfig = {}; + uint32_t pFLatency = 0U; + uint32_t uwTimclock = 0U, uwAPBxPrescaler = 0U; + + /* Get clock configuration */ + HAL_RCC_GetClockConfig(&clkconfig, &pFLatency); + switch (getTimerClkSrc(handle->Instance)) { + case 1: + uwAPBxPrescaler = clkconfig.APB1CLKDivider; + uwTimclock = HAL_RCC_GetPCLK1Freq(); + break; +#if !defined(STM32C0xx) && !defined(STM32F0xx) && !defined(STM32G0xx) + case 2: + uwAPBxPrescaler = clkconfig.APB2CLKDivider; + uwTimclock = HAL_RCC_GetPCLK2Freq(); + break; +#endif + default: + case 0: // Unknown timer clock source + return 0; + } + +#if defined(STM32H7xx) + /* When TIMPRE bit of the RCC_CFGR register is reset, + * if APBx prescaler is 1 or 2 then TIMxCLK = HCLK, + * otherwise TIMxCLK = 2x PCLKx. + * When TIMPRE bit in the RCC_CFGR register is set, + * if APBx prescaler is 1,2 or 4, then TIMxCLK = HCLK, + * otherwise TIMxCLK = 4x PCLKx + */ + RCC_PeriphCLKInitTypeDef PeriphClkConfig = {}; + HAL_RCCEx_GetPeriphCLKConfig(&PeriphClkConfig); + + if (PeriphClkConfig.TIMPresSelection == RCC_TIMPRES_ACTIVATED) { + switch (uwAPBxPrescaler) { + default: + case RCC_APB1_DIV1: + case RCC_APB1_DIV2: + case RCC_APB1_DIV4: + /* case RCC_APB2_DIV1: */ + case RCC_APB2_DIV2: + case RCC_APB2_DIV4: + /* Note: in such cases, HCLK = (APBCLK * DIVx) */ + uwTimclock = HAL_RCC_GetHCLKFreq(); + break; + case RCC_APB1_DIV8: + case RCC_APB1_DIV16: + case RCC_APB2_DIV8: + case RCC_APB2_DIV16: + uwTimclock *= 4; + break; + } + } else { + switch (uwAPBxPrescaler) { + default: + case RCC_APB1_DIV1: + case RCC_APB1_DIV2: + /* case RCC_APB2_DIV1: */ + case RCC_APB2_DIV2: + /* Note: in such cases, HCLK = (APBCLK * DIVx) */ + uwTimclock = HAL_RCC_GetHCLKFreq(); + break; + case RCC_APB1_DIV4: + case RCC_APB1_DIV8: + case RCC_APB1_DIV16: + case RCC_APB2_DIV4: + case RCC_APB2_DIV8: + case RCC_APB2_DIV16: + uwTimclock *= 2; + break; + } + } +#else + /* When TIMPRE bit of the RCC_DCKCFGR register is reset, + * if APBx prescaler is 1, then TIMxCLK = PCLKx, + * otherwise TIMxCLK = 2x PCLKx. + * When TIMPRE bit in the RCC_DCKCFGR register is set, + * if APBx prescaler is 1,2 or 4, then TIMxCLK = HCLK, + * otherwise TIMxCLK = 4x PCLKx + */ +#if defined(STM32F4xx) || defined(STM32F7xx) +#if !defined(STM32F405xx) && !defined(STM32F415xx) &&\ + !defined(STM32F407xx) && !defined(STM32F417xx) + RCC_PeriphCLKInitTypeDef PeriphClkConfig = {}; + HAL_RCCEx_GetPeriphCLKConfig(&PeriphClkConfig); + + if (PeriphClkConfig.TIMPresSelection == RCC_TIMPRES_ACTIVATED) + switch (uwAPBxPrescaler) { + default: + case RCC_HCLK_DIV1: + case RCC_HCLK_DIV2: + case RCC_HCLK_DIV4: + /* Note: in such cases, HCLK = (APBCLK * DIVx) */ + uwTimclock = HAL_RCC_GetHCLKFreq(); + break; + case RCC_HCLK_DIV8: + case RCC_HCLK_DIV16: + uwTimclock *= 4; + break; + } else +#endif +#endif + switch (uwAPBxPrescaler) { + default: + case RCC_HCLK_DIV1: + // uwTimclock*=1; + break; + case RCC_HCLK_DIV2: + case RCC_HCLK_DIV4: + case RCC_HCLK_DIV8: + case RCC_HCLK_DIV16: + uwTimclock *= 2; + break; + } +#endif /* STM32H7xx */ + return uwTimclock; +#endif /* STM32MP1xx */ +} + + + + + + + +#if defined(__MBED__) + +void enableTimerClock(TIM_HandleTypeDef *htim) { + // Enable TIM clock +#if defined(TIM1_BASE) + if (htim->Instance == TIM1) { + __HAL_RCC_TIM1_CLK_ENABLE(); + } +#endif +#if defined(TIM2_BASE) + if (htim->Instance == TIM2) { + __HAL_RCC_TIM2_CLK_ENABLE(); + } +#endif +#if defined(TIM3_BASE) + if (htim->Instance == TIM3) { + __HAL_RCC_TIM3_CLK_ENABLE(); + } +#endif +#if defined(TIM4_BASE) + if (htim->Instance == TIM4) { + __HAL_RCC_TIM4_CLK_ENABLE(); + } +#endif +#if defined(TIM5_BASE) + if (htim->Instance == TIM5) { + __HAL_RCC_TIM5_CLK_ENABLE(); + } +#endif +#if defined(TIM6_BASE) + if (htim->Instance == TIM6) { + __HAL_RCC_TIM6_CLK_ENABLE(); + } +#endif +#if defined(TIM7_BASE) + if (htim->Instance == TIM7) { + __HAL_RCC_TIM7_CLK_ENABLE(); + } +#endif +#if defined(TIM8_BASE) + if (htim->Instance == TIM8) { + __HAL_RCC_TIM8_CLK_ENABLE(); + } +#endif +#if defined(TIM9_BASE) + if (htim->Instance == TIM9) { + __HAL_RCC_TIM9_CLK_ENABLE(); + } +#endif +#if defined(TIM10_BASE) + if (htim->Instance == TIM10) { + __HAL_RCC_TIM10_CLK_ENABLE(); + } +#endif +#if defined(TIM11_BASE) + if (htim->Instance == TIM11) { + __HAL_RCC_TIM11_CLK_ENABLE(); + } +#endif +#if defined(TIM12_BASE) + if (htim->Instance == TIM12) { + __HAL_RCC_TIM12_CLK_ENABLE(); + } +#endif +#if defined(TIM13_BASE) + if (htim->Instance == TIM13) { + __HAL_RCC_TIM13_CLK_ENABLE(); + } +#endif +#if defined(TIM14_BASE) + if (htim->Instance == TIM14) { + __HAL_RCC_TIM14_CLK_ENABLE(); + } +#endif +#if defined(TIM15_BASE) + if (htim->Instance == TIM15) { + __HAL_RCC_TIM15_CLK_ENABLE(); + } +#endif +#if defined(TIM16_BASE) + if (htim->Instance == TIM16) { + __HAL_RCC_TIM16_CLK_ENABLE(); + } +#endif +#if defined(TIM17_BASE) + if (htim->Instance == TIM17) { + __HAL_RCC_TIM17_CLK_ENABLE(); + } +#endif +#if defined(TIM18_BASE) + if (htim->Instance == TIM18) { + __HAL_RCC_TIM18_CLK_ENABLE(); + } +#endif +#if defined(TIM19_BASE) + if (htim->Instance == TIM19) { + __HAL_RCC_TIM19_CLK_ENABLE(); + } +#endif +#if defined(TIM20_BASE) + if (htim->Instance == TIM20) { + __HAL_RCC_TIM20_CLK_ENABLE(); + } +#endif +#if defined(TIM21_BASE) + if (htim->Instance == TIM21) { + __HAL_RCC_TIM21_CLK_ENABLE(); + } +#endif +#if defined(TIM22_BASE) + if (htim->Instance == TIM22) { + __HAL_RCC_TIM22_CLK_ENABLE(); + } +#endif +} + + +uint8_t getTimerClkSrc(TIM_TypeDef *tim) { + uint8_t clkSrc = 0; + + if (tim != (TIM_TypeDef *)NC) +#if defined(STM32C0xx) || defined(STM32F0xx) || defined(STM32G0xx) + /* TIMx source CLK is PCKL1 */ + clkSrc = 1; +#else + { + if ( + #if defined(TIM2_BASE) + tim == TIM2 || + #endif + #if defined(TIM3_BASE) + tim == TIM3 || + #endif + #if defined(TIM4_BASE) + tim == TIM4 || + #endif + #if defined(TIM5_BASE) + tim == TIM5 || + #endif + #if defined(TIM6_BASE) + tim == TIM6 || + #endif + #if defined(TIM7_BASE) + tim == TIM7 || + #endif + #if defined(TIM12_BASE) + tim == TIM12 || + #endif + #if defined(TIM13_BASE) + tim == TIM13 || + #endif + #if defined(TIM14_BASE) + tim == TIM14 || + #endif + #if defined(TIM18_BASE) + tim == TIM18 || + #endif + false) + clkSrc = 1; + else + if ( + #if defined(TIM1_BASE) + tim == TIM1 || + #endif + #if defined(TIM8_BASE) + tim == TIM8 || + #endif + #if defined(TIM9_BASE) + tim == TIM9 || + #endif + #if defined(TIM10_BASE) + tim == TIM10 || + #endif + #if defined(TIM11_BASE) + tim == TIM11 || + #endif + #if defined(TIM15_BASE) + tim == TIM15 || + #endif + #if defined(TIM16_BASE) + tim == TIM16 || + #endif + #if defined(TIM17_BASE) + tim == TIM17 || + #endif + #if defined(TIM19_BASE) + tim == TIM19 || + #endif + #if defined(TIM20_BASE) + tim == TIM20 || + #endif + #if defined(TIM21_BASE) + tim == TIM21 || + #endif + #if defined(TIM22_BASE) + tim == TIM22 || + #endif + false ) + clkSrc = 2; + else + return 0; + } +#endif + return clkSrc; +} + +#endif + + + + + + + + +#ifdef SIMPLEFOC_STM32_DEBUG + +/* + some utility functions to print out the timer combinations +*/ + +int stm32_getTimerNumber(TIM_TypeDef *instance) { + #if defined(TIM1_BASE) + if (instance==TIM1) return 1; + #endif + #if defined(TIM2_BASE) + if (instance==TIM2) return 2; + #endif + #if defined(TIM3_BASE) + if (instance==TIM3) return 3; + #endif + #if defined(TIM4_BASE) + if (instance==TIM4) return 4; + #endif + #if defined(TIM5_BASE) + if (instance==TIM5) return 5; + #endif + #if defined(TIM6_BASE) + if (instance==TIM6) return 6; + #endif + #if defined(TIM7_BASE) + if (instance==TIM7) return 7; + #endif + #if defined(TIM8_BASE) + if (instance==TIM8) return 8; + #endif + #if defined(TIM9_BASE) + if (instance==TIM9) return 9; + #endif + #if defined(TIM10_BASE) + if (instance==TIM10) return 10; + #endif + #if defined(TIM11_BASE) + if (instance==TIM11) return 11; + #endif + #if defined(TIM12_BASE) + if (instance==TIM12) return 12; + #endif + #if defined(TIM13_BASE) + if (instance==TIM13) return 13; + #endif + #if defined(TIM14_BASE) + if (instance==TIM14) return 14; + #endif + #if defined(TIM15_BASE) + if (instance==TIM15) return 15; + #endif + #if defined(TIM16_BASE) + if (instance==TIM16) return 16; + #endif + #if defined(TIM17_BASE) + if (instance==TIM17) return 17; + #endif + #if defined(TIM18_BASE) + if (instance==TIM18) return 18; + #endif + #if defined(TIM19_BASE) + if (instance==TIM19) return 19; + #endif + #if defined(TIM20_BASE) + if (instance==TIM20) return 20; + #endif + #if defined(TIM21_BASE) + if (instance==TIM21) return 21; + #endif + #if defined(TIM22_BASE) + if (instance==TIM22) return 22; + #endif + return -1; +} + + +void stm32_printTimerCombination(int numPins, PinMap* timers[], int score) { + for (int i=0; iperipheral)); + SimpleFOCDebug::print("-CH"); + SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function)); + if (STM_PIN_INVERTED(timers[i]->function)) + SimpleFOCDebug::print("N"); + } + SimpleFOCDebug::print(" "); + } + SimpleFOCDebug::println("score: ", score); +} + +#endif + + +#endif diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.h b/src/drivers/hardware_specific/stm32/stm32_timerutils.h new file mode 100644 index 00000000..545fbcfa --- /dev/null +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.h @@ -0,0 +1,33 @@ + +#pragma once + +#include "./stm32_mcu.h" + +#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) + +void stm32_pauseTimer(TIM_HandleTypeDef* handle); +void stm32_resumeTimer(TIM_HandleTypeDef* handle); +void stm32_refreshTimer(TIM_HandleTypeDef* handle); +void stm32_pauseChannel(TIM_HandleTypeDef* handle, uint32_t llchannels); +void stm32_resumeChannel(TIM_HandleTypeDef* handle, uint32_t llchannels); +uint32_t stm32_setClockAndARR(TIM_HandleTypeDef* handle, uint32_t PWM_freq); +uint8_t stm32_countTimers(TIM_HandleTypeDef *timers[], uint8_t num_timers); +uint8_t stm32_distinctTimers(TIM_HandleTypeDef* timers_in[], uint8_t num_timers, TIM_HandleTypeDef* timers_out[]); +uint32_t stm32_getHALChannel(uint32_t channel); +uint32_t stm32_getLLChannel(PinMap* timer); +int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* slave); +TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num_timers_in); +void stm32_setPwm(TIM_HandleTypeDef *timer, uint32_t channel, uint32_t value); +uint32_t stm32_getTimerClockFreq(TIM_HandleTypeDef* handle); + +#if defined(__MBED__) +void enableTimerClock(TIM_HandleTypeDef *htim); +uint8_t getTimerClkSrc(TIM_TypeDef *tim); +#endif + +#if defined(SIMPLEFOC_STM32_DEBUG) +void stm32_printTimerCombination(int numPins, PinMap* timers[], int score); +int stm32_getTimerNumber(TIM_TypeDef *instance); +#endif + +#endif From 5569f734e54e77bf70ad00536019dbf3ddf43089 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Aug 2024 18:01:49 +0200 Subject: [PATCH 03/10] update l4 current sense to new API --- .../stm32/stm32l4/stm32l4_hal.cpp | 6 ++-- .../stm32/stm32l4/stm32l4_mcu.cpp | 12 +++---- .../stm32/stm32l4/stm32l4_utils.cpp | 32 +++++++++---------- .../stm32/stm32l4/stm32l4_utils.h | 4 +-- 4 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index 67a0473b..8f7889c4 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -125,8 +125,8 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels // if the code comes here, it has found the timer available @@ -134,7 +134,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive sConfigInjected.ExternalTrigInjecConv = trigger_flag; // this will be the timer with which the ADC will sync - cs_params->timer_handle = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } 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 5de6432a..ed55b482 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -49,17 +49,17 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // if timer has repetition counter - it will downsample using it // and it does not need the software downsample - if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->Instance) ){ // adjust the initial timer state such that the trigger // - for DMA transfer aligns with the pwm peaks instead of throughs. // - for interrupt based ADC transfer // - only necessary for the timers that have repetition counters - cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -73,7 +73,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); @@ -119,7 +119,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized // TODO verify if success in future diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp index 376d9d68..f93e63a4 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp @@ -133,31 +133,31 @@ uint32_t _getADCChannel(PinName pin) // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_adc_ex.h#L210 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJEC_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJEC_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIGINJEC_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJEC_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGINJEC_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGINJEC_T8_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIGINJEC_T15_TRGO; #endif else @@ -166,31 +166,31 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIG_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIG_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) + else if(timer->Instance == TIM3) return ADC_EXTERNALTRIG_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIG_T4_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIG_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIG_T8_TRGO; #endif #ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) + else if(timer->Instance == TIM15) return ADC_EXTERNALTRIG_T15_TRGO; #endif else diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h index ceef9be7..e30bf0ce 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h @@ -19,11 +19,11 @@ uint32_t _getADCChannel(PinName pin); // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance int _adcToIndex(ADC_HandleTypeDef *AdcHandle); From 3d2688023e0cd7563371e65ac5ef3f6fcb5754db Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Aug 2024 18:02:53 +0200 Subject: [PATCH 04/10] change target to include all the mbed STM32H7s --- .../hardware_specific/stm32/stm32_mcu.cpp | 51 ++++++++----------- .../hardware_specific/stm32/stm32_mcu.h | 15 +++--- .../stm32/stm32_searchtimers.cpp | 2 +- .../stm32/stm32_searchtimers.h | 2 +- .../stm32/stm32_timerutils.cpp | 30 ++++++++--- .../stm32/stm32_timerutils.h | 2 +- 6 files changed, 57 insertions(+), 45 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 91246d94..68cd3f99 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,7 +4,7 @@ #include "./stm32_timerutils.h" #include "./stm32_searchtimers.h" -#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) #pragma message("") #pragma message("SimpleFOC: compiling for STM32") @@ -146,11 +146,24 @@ TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t m if (timer==NULL) return NULL; TIM_HandleTypeDef* handle = stm32_getTimer(timer); + uint32_t channel = STM_PIN_CHANNEL(timer->function); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Configuring timer ", (int)stm32_getTimerNumber(handle->Instance)); + SIMPLEFOC_DEBUG("STM32-DRV: Configuring channel ", (int)channel); + #endif if (handle==NULL) { handle = stm32_useTimer(timer); - stm32_setClockAndARR(handle, PWM_freq); // TODO add checks for PWM frequency limits + uint32_t arr = stm32_setClockAndARR(handle, PWM_freq); + if (arrfunction); TIM_OC_InitTypeDef channelOC; channelOC.OCMode = TIM_OCMODE_PWM1; channelOC.Pulse = 0; //__HAL_TIM_GET_COMPARE(handle, channel); @@ -171,11 +184,6 @@ TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t m if (IS_TIM_BREAK_INSTANCE(handle->Instance)) { __HAL_TIM_MOE_ENABLE(handle); } - -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Configuring timer ", (int)stm32_getTimerNumber(handle->Instance)); - SIMPLEFOC_DEBUG("STM32-DRV: Configuring channel ", (int)channel); -#endif return handle; } @@ -342,10 +350,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[1] = { pinA }; PinMap* pinTimers[1] = { NULL }; @@ -382,10 +387,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[2] = { pinA, pinB }; PinMap* pinTimers[2] = { NULL, NULL }; @@ -404,7 +406,7 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { .timers_handle = { HT1, HT2 }, .channels = { channel1, channel2 }, .llchannels = { stm32_getLLChannel(pinTimers[0]), stm32_getLLChannel(pinTimers[1]) }, - .pwm_frequency = pwm_frequency, + .pwm_frequency = pwm_frequency, // TODO set to actual frequency .num_timers = stm32_countTimers(timers, 2), .master_timer = NULL }; @@ -425,10 +427,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - //pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[3] = { pinA, pinB, pinC }; PinMap* pinTimers[3] = { NULL, NULL, NULL }; @@ -469,10 +468,7 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz int pins[4] = { pinA, pinB, pinC, pinD }; PinMap* pinTimers[4] = { NULL, NULL, NULL, NULL }; @@ -570,10 +566,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many drivers used"); return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; } - 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 |%0kHz max - // center-aligned frequency is uses two periods - pwm_frequency *=2; + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = SIMPLEFOC_STM32_PWM_FREQUENCY; // default frequency 25khz // find configuration int pins[6] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }; diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index 6909071f..5be9be3e 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -2,7 +2,7 @@ #include "../../hardware_api.h" -#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) #ifndef SIMPLEFOC_STM32_MAX_TIMERSUSED #define SIMPLEFOC_STM32_MAX_TIMERSUSED 6 @@ -23,12 +23,13 @@ #endif - -// default pwm parameters -#define _PWM_RESOLUTION 12 // 12bit -#define _PWM_RANGE 4095.0f // 2^12 -1 = 4095 -#define _PWM_FREQUENCY 25000 // 25khz -#define _PWM_FREQUENCY_MAX 50000 // 50khz +/** + * No limits are placed on PWM frequency, so very fast or very slow frequencies can be set. + * A warning is displayed to debug if you get less than 8bit resolution for the PWM duty cycle. + * If no pwm_frequency is set, the default value is 25kHz. + */ +#define SIMPLEFOC_STM32_PWM_FREQUENCY 25000 // 25khz +#define SIMPLEFOC_STM32_MIN_RESOLUTION 255 // 6pwm parameters #define _HARDWARE_6PWM 1 diff --git a/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp b/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp index 052142a6..e04fc719 100644 --- a/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_searchtimers.cpp @@ -2,7 +2,7 @@ #include "./stm32_searchtimers.h" #include "./stm32_timerutils.h" -#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) diff --git a/src/drivers/hardware_specific/stm32/stm32_searchtimers.h b/src/drivers/hardware_specific/stm32/stm32_searchtimers.h index 68c3fcd7..6001b637 100644 --- a/src/drivers/hardware_specific/stm32/stm32_searchtimers.h +++ b/src/drivers/hardware_specific/stm32/stm32_searchtimers.h @@ -2,7 +2,7 @@ #include "./stm32_mcu.h" -#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) int stm32_findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]); diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp index 31d0eab6..078efd56 100644 --- a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp @@ -2,7 +2,7 @@ #include "./stm32_timerutils.h" #include -#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) void stm32_pauseTimer(TIM_HandleTypeDef* handle){ @@ -10,6 +10,8 @@ void stm32_pauseTimer(TIM_HandleTypeDef* handle){ * even if some channels are still running */ LL_TIM_DisableCounter(handle->Instance); // handle->State = HAL_TIM_STATE_READY; + // on advanced control timers there is also the option of using the brake or the MOE? + // TIM1->EGR |= TIM_EGR_BG; // break generation } @@ -211,7 +213,7 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #endif return -1; } -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) || defined(TARGET_STM32H7) // function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found @@ -250,7 +252,7 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; #endif - #if defined(TIM5) + #if defined(TIM5) && !defined(TARGET_STM32H7) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; #endif } @@ -266,9 +268,12 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #if defined(TIM4) else if(TIM_slave == TIM4) return LL_TIM_TS_ITR2; #endif - #if defined(TIM5) + #if defined(TIM5) && !defined(TARGET_STM32H7) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; #endif + #if defined(TIM5) && defined(TARGET_STM32H7) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #endif } #endif #if defined(TIM4) && defined(LL_TIM_TS_ITR3) @@ -285,9 +290,12 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; #endif - #if defined(TIM5) + #if defined(TIM5) && !defined(TARGET_STM32H7) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; #endif + #if defined(TIM5) && defined(TARGET_STM32H7) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif } #endif #if defined(TIM5) @@ -318,6 +326,16 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #endif } #endif + #if defined(TIM15) && defined(TARGET_STM32H7) + else if (TIM_master == TIM15){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM3) + if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + } + #endif return -1; // combination not supported } #else @@ -484,7 +502,7 @@ uint32_t stm32_getTimerClockFreq(TIM_HandleTypeDef *handle) { return 0; } -#if defined(STM32H7xx) +#if defined(STM32H7xx) || defined(TARGET_STM32H7) /* When TIMPRE bit of the RCC_CFGR register is reset, * if APBx prescaler is 1 or 2 then TIMxCLK = HCLK, * otherwise TIMxCLK = 2x PCLKx. diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.h b/src/drivers/hardware_specific/stm32/stm32_timerutils.h index 545fbcfa..3ba1c558 100644 --- a/src/drivers/hardware_specific/stm32/stm32_timerutils.h +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.h @@ -3,7 +3,7 @@ #include "./stm32_mcu.h" -#if defined(_STM32_DEF_) || defined(TARGET_PORTENTA_H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) void stm32_pauseTimer(TIM_HandleTypeDef* handle); void stm32_resumeTimer(TIM_HandleTypeDef* handle); From afa8e3d701f0320165cfbeffc23436a7c4a7aa07 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Aug 2024 18:08:44 +0200 Subject: [PATCH 05/10] fix STM32F1 current sense --- .../stm32/stm32f1/stm32f1_hal.cpp | 24 +++++++++---------- .../stm32/stm32f1/stm32f1_mcu.cpp | 12 +++++----- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index d3bea81e..880fb3d6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -7,19 +7,19 @@ // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; #endif #ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->Instance == TIM5) + else if(timer->Instance == TIM5) return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; #endif else @@ -28,11 +28,11 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM3) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM3) return ADC_EXTERNALTRIGCONV_T3_TRGO; #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGCONV_T8_TRGO; #endif else @@ -82,8 +82,8 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; - while(driver_params->timers[tim_num] != NP && tim_num < 6){ - uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + while(driver_params->timers_handle[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers_handle[tim_num++]); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels // if the code comes here, it has found the timer available @@ -91,7 +91,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive sConfigInjected.ExternalTrigInjecConv = trigger_flag; // this will be the timer with which the ADC will sync - cs_params->timer_handle = driver_params->timers[tim_num-1]; + cs_params->timer_handle = driver_params->timers_handle[tim_num-1]; // done break; } @@ -105,7 +105,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // display which timer is being used #ifdef SIMPLEFOC_STM32_DEBUG // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); + SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->Instance) + 1); #endif // first channel 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 49f2f3d5..7d31f966 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -56,17 +56,17 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // if timer has repetition counter - it will downsample using it // and it does not need the software downsample - if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->Instance) ){ // adjust the initial timer state such that the trigger // - for DMA transfer aligns with the pwm peaks instead of throughs. // - for interrupt based ADC transfer // - only necessary for the timers that have repetition counters - cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; }else{ @@ -79,7 +79,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ } } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); @@ -96,7 +96,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized From 810218916bda9026466a3fec3dcb2e2394cfff12 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Aug 2024 18:36:36 +0200 Subject: [PATCH 06/10] fix B-G431-ESC1 current sense driver --- .../hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 46cb20be..697a2f0f 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 @@ -132,7 +132,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { .pins = { pinA, pinB, pinC }, .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), - .timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this) + .timer_handle = ((STM32DriverParams*)driver_params)->timers_handle[0], }; return params; @@ -153,21 +153,21 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // if timer has repetition counter - it will downsample using it // and it does not need the software downsample - if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->Instance) ){ // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs. // only necessary for the timers that have repetition counters - cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized From cd79e01f9f268fbfed6b439d7e7ca20a5cc04abc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Aug 2024 23:00:53 +0200 Subject: [PATCH 07/10] now working on Portenta H7 --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 13 ++++++++----- .../hardware_specific/stm32/stm32_timerutils.cpp | 13 +++++++------ 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 68cd3f99..0128f486 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -147,12 +147,11 @@ TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t m return NULL; TIM_HandleTypeDef* handle = stm32_getTimer(timer); uint32_t channel = STM_PIN_CHANNEL(timer->function); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Configuring timer ", (int)stm32_getTimerNumber(handle->Instance)); - SIMPLEFOC_DEBUG("STM32-DRV: Configuring channel ", (int)channel); - #endif if (handle==NULL) { handle = stm32_useTimer(timer); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Initializing TIM", (int)stm32_getTimerNumber(handle->Instance)); + #endif uint32_t arr = stm32_setClockAndARR(handle, PWM_freq); if (arrInstance)) { __HAL_TIM_MOE_ENABLE(handle); } + #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-DRV: Configured TIM"); + SimpleFOCDebug::print((int)stm32_getTimerNumber(handle->Instance)); + SIMPLEFOC_DEBUG("_CH", (int)channel); + #endif return handle; } diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp index 078efd56..576d6a4b 100644 --- a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp @@ -53,11 +53,11 @@ uint32_t stm32_setClockAndARR(TIM_HandleTypeDef* handle, uint32_t PWM_freq) { } __HAL_TIM_SET_AUTORELOAD(handle, arr_value); stm32_refreshTimer(handle); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Timer clock: ", (int)stm32_getTimerClockFreq(handle)); - SIMPLEFOC_DEBUG("STM32-DRV: Timer prescaler: ", (int)prescaler); - SIMPLEFOC_DEBUG("STM32-DRV: Timer ARR: ", (int)arr_value); - #endif + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: Timer clock: ", (int)stm32_getTimerClockFreq(handle)); + // SIMPLEFOC_DEBUG("STM32-DRV: Timer prescaler: ", (int)prescaler); + // SIMPLEFOC_DEBUG("STM32-DRV: Timer ARR: ", (int)arr_value); + // #endif return arr_value; } @@ -458,7 +458,8 @@ TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_GATED); #endif } - stm32_resumeTimer(timers[master_index]); // start the master to start all timers + for (int i=0; i Date: Fri, 30 Aug 2024 23:08:48 +0200 Subject: [PATCH 08/10] fix current sense for stm32f7 MCUs --- .../stm32/stm32f7/stm32f7_hal.cpp | 6 ++-- .../stm32/stm32f7/stm32f7_mcu.cpp | 12 ++++---- .../stm32/stm32f7/stm32f7_utils.cpp | 28 +++++++++---------- .../stm32/stm32f7/stm32f7_utils.h | 4 +-- 4 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp index d4cffec6..04575397 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -77,8 +77,8 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; for (size_t i=0; i<6; i++) { - HardwareTimer *timer_to_check = driver_params->timers[tim_num++]; - TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance; + TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->Instance; // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; // if(TRGO_already_configured) continue; @@ -110,7 +110,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // display which timer is being used #ifdef SIMPLEFOC_STM32_DEBUG // it would be better to use the getTimerNumber from driver - SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); + SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->Instance) + 1); #endif 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 f5ca70f3..e4b9c850 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -42,23 +42,23 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); + stm32_pause(driver_params); // if timer has repetition counter - it will downsample using it // and it does not need the software downsample - if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->Instance) ){ // adjust the initial timer state such that the trigger // - for DMA transfer aligns with the pwm peaks instead of throughs. // - for interrupt based ADC transfer // - only necessary for the timers that have repetition counters - cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; - cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + cs_params->timer_handle->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->Instance->ARR; // remember that this timer has repetition counter - no need to downasmple needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; } // set the trigger output event - LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); // start the adc #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT @@ -68,7 +68,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ #endif // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + stm32_resume(driver_params); // return the cs parameters // successfully initialized diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp index d5f8c6b2..2321c5da 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -163,28 +163,28 @@ ADC_EXTERNALTRIGINJECCONV_T6_TRGO */ // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ - if(timer->getHandle()->Instance == TIM1) + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; #endif #ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->Instance == TIM5) + else if(timer->Instance == TIM5) return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGINJECCONV_T8_TRGO; #endif else @@ -204,27 +204,27 @@ ADC_EXTERNALTRIGCONV_T6_TRGO // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM1) +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) return ADC_EXTERNALTRIGCONV_T1_TRGO; #ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->Instance == TIM2) return ADC_EXTERNALTRIGCONV_T2_TRGO; #endif #ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) + else if(timer->Instance == TIM4) return ADC_EXTERNALTRIGCONV_T4_TRGO; #endif #ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->Instance == TIM5) + else if(timer->Instance == TIM5) return ADC_EXTERNALTRIGCONV_T5_TRGO; #endif #ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) + else if(timer->Instance == TIM6) return ADC_EXTERNALTRIGCONV_T6_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) + else if(timer->Instance == TIM8) return ADC_EXTERNALTRIGCONV_T8_TRGO; #endif else diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h index 017ff464..47465e5d 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -17,11 +17,11 @@ uint32_t _getADCChannel(PinName pin); // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 -uint32_t _timerToInjectedTRGO(HardwareTimer* timer); +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); // timer to regular TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer); +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance int _adcToIndex(ADC_HandleTypeDef *AdcHandle); From e535bba6d1f68684a0b837439aecd2918841c8f5 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 31 Aug 2024 16:10:01 +0200 Subject: [PATCH 09/10] fix for software 6-PWM low side polarity --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 0128f486..d9e058ef 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -163,7 +163,7 @@ TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t m } } TIM_OC_InitTypeDef channelOC; - channelOC.OCMode = TIM_OCMODE_PWM1; + channelOC.OCMode = mode; channelOC.Pulse = 0; //__HAL_TIM_GET_COMPARE(handle, channel); channelOC.OCPolarity = polarity; channelOC.OCFastMode = TIM_OCFAST_DISABLE; @@ -195,7 +195,14 @@ TIM_HandleTypeDef* stm32_initPinPWM(uint32_t PWM_freq, PinMap* timer, uint32_t m - +/** +0110: PWM mode 1 - In upcounting, channel 1 is active as long as TIMx_CNTTIMx_CCR1 else active (OC1REF=’1’). +0111: PWM mode 2 - In upcounting, channel 1 is inactive as long as +TIMx_CNTTIMx_CCR1 else inactiv + */ // init high side pin TIM_HandleTypeDef* _stm32_initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; From 4d8fa4a6c15c3bce15d536235e0516851359fb34 Mon Sep 17 00:00:00 2001 From: runger1101001 Date: Thu, 3 Oct 2024 21:05:56 +0200 Subject: [PATCH 10/10] add comments for triggers --- .../hardware_specific/stm32/stm32_mcu.cpp | 10 +++++-- .../hardware_specific/stm32/stm32_mcu.h | 1 + .../stm32/stm32_timerutils.cpp | 29 +++++++++++++------ 3 files changed, 28 insertions(+), 12 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index d9e058ef..1a73ddfa 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -89,7 +89,7 @@ TIM_HandleTypeDef* stm32_useTimer(PinMap* timer) { handle->hdma[6] = NULL; handle->Init.Prescaler = 0; handle->Init.Period = ((1 << 16) - 1); - handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED2; handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; #if defined(TIM_RCR_REP) @@ -201,12 +201,13 @@ else inactive. In downcounting, channel 1 is inactive (OC1REF=‘0’) as long a TIMx_CNT>TIMx_CCR1 else active (OC1REF=’1’). 0111: PWM mode 2 - In upcounting, channel 1 is inactive as long as TIMx_CNTTIMx_CCR1 else inactiv +TIMx_CNT>TIMx_CCR1 else inactive */ // init high side pin TIM_HandleTypeDef* _stm32_initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { - uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; + uint32_t polarity = SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW ; TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, timer, TIM_OCMODE_PWM1, polarity); + LL_TIM_OC_EnablePreload(handle->Instance, stm32_getLLChannel(timer)); return handle; } @@ -214,6 +215,7 @@ TIM_HandleTypeDef* _stm32_initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) { TIM_HandleTypeDef* _stm32_initPinPWMLow(uint32_t PWM_freq, PinMap* timer) { uint32_t polarity = SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW; TIM_HandleTypeDef* handle = stm32_initPinPWM(PWM_freq, timer, TIM_OCMODE_PWM2, polarity); + LL_TIM_OC_EnablePreload(handle->Instance, stm32_getLLChannel(timer)); return handle; } @@ -322,6 +324,8 @@ STM32DriverParams* _stm32_initHardware6PWMPair(long PWM_freq, float dead_zone, P params->timers_handle[paramsPos+1] = handle; params->channels[paramsPos] = channel1; params->channels[paramsPos+1] = channel2; + params->llchannels[paramsPos] = stm32_getLLChannel(pinH); + params->llchannels[paramsPos+1] = stm32_getLLChannel(pinL); return params; } diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index 5be9be3e..ee0bf569 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -3,6 +3,7 @@ #include "../../hardware_api.h" #if defined(_STM32_DEF_) || defined(TARGET_STM32H7) +// TARGET_M4 / TARGET_M7 #ifndef SIMPLEFOC_STM32_MAX_TIMERSUSED #define SIMPLEFOC_STM32_MAX_TIMERSUSED 6 diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp index 576d6a4b..3eef0849 100644 --- a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp @@ -397,7 +397,9 @@ TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); + SimpleFOCDebug::print("STM32-DRV: Synchronising "); + SimpleFOCDebug::print(numTimers); + SimpleFOCDebug::println(" timers"); #endif // see if there is more then 1 timers used for the pwm @@ -441,7 +443,7 @@ TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num LL_TIM_SetSlaveMode(timers[master_index]->Instance, LL_TIM_SLAVEMODE_DISABLED ); // Configure the master timer to send a trigger signal on enable LL_TIM_SetTriggerOutput(timers[master_index]->Instance, LL_TIM_TRGO_ENABLE); - LL_TIM_EnableMasterSlaveMode(timers[master_index]->Instance); + //LL_TIM_EnableMasterSlaveMode(timers[master_index]->Instance); // configure other timers to get the input trigger from the master timer for (int slave_index=0; slave_index < numTimers; slave_index++) { @@ -451,15 +453,24 @@ TIM_HandleTypeDef* stm32_alignTimers(TIM_HandleTypeDef *timers_in[], uint8_t num SIMPLEFOC_DEBUG("STM32-DRV: slave timer: TIM", stm32_getTimerNumber(timers[slave_index]->Instance)); #endif // Configure the slave timer to be triggered by the master enable signal - LL_TIM_SetTriggerInput(timers[slave_index]->Instance, stm32_getInternalSourceTrigger(timers[master_index], timers[slave_index])); - #if defined(STM32G4xx) - LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_COMBINED_GATEDRESET); - #else + uint32_t trigger = stm32_getInternalSourceTrigger(timers[master_index], timers[slave_index]); + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: slave trigger ITR ", (int)trigger); + // #endif + LL_TIM_SetTriggerInput(timers[slave_index]->Instance, trigger); + // #if defined(STM32G4xx) + // LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_COMBINED_GATEDRESET); + // #else LL_TIM_SetSlaveMode(timers[slave_index]->Instance, LL_TIM_SLAVEMODE_GATED); - #endif + // #endif + } + for (int i=0; iInstance->CNT); } - for (int i=0; i