From c28ad9de58e8fed0bdab52dcec1dd273292d86b9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 10 Jun 2024 17:56:59 +0200 Subject: [PATCH 01/53] 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/53] 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/53] 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/53] 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/53] 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/53] 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/53] 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/53] 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/53] 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/53] 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 Date: Thu, 3 Oct 2024 21:08:08 +0200 Subject: [PATCH 11/53] working on rp2350 support --- src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp | 2 +- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 4 ++-- src/drivers/hardware_specific/rp2040/rp2040_mcu.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp index d2ed881b..302e3815 100644 --- a/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp @@ -1,5 +1,5 @@ -#if defined(TARGET_RP2040) +#if defined(TARGET_RP2040) || defined(TARGET_RP2350) #include "../../hardware_api.h" diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index 6afbf345..78fb442f 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -6,11 +6,11 @@ #include "./rp2040_mcu.h" -#if defined(TARGET_RP2040) +#if defined(TARGET_RP2040) || defined(TARGET_RP2350) #pragma message("") -#pragma message("SimpleFOC: compiling for RP2040") +#pragma message("SimpleFOC: compiling for RP2040/RP2350") #pragma message("") #if !defined(SIMPLEFOC_DEBUG_RP2040) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h index bbfb3873..3dab10bb 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.h +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.h @@ -4,7 +4,7 @@ #include "Arduino.h" -#if defined(TARGET_RP2040) +#if defined(TARGET_RP2040) || defined(TARGET_RP2350) From 58ec9781ba86cbfe252a6f4015446d1605b39414 Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Wed, 9 Oct 2024 14:34:58 +0200 Subject: [PATCH 12/53] Add motor characterisation --- src/BLDCMotor.h | 10 ++ src/StepperMotor.h | 11 ++ src/common/base_classes/FOCMotor.cpp | 221 +++++++++++++++++++++++++++ src/common/base_classes/FOCMotor.h | 9 ++ 4 files changed, 251 insertions(+) diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index c261e405..386c5f19 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -81,6 +81,16 @@ class BLDCMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + /** + * Measure resistance and inductance of a BLDCMotor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * @param voltage The voltage applied to the motor + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage){ + return FOCMotor::characteriseMotor(voltage, 1.5f); + } + private: // FOC methods diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7e7810d8..208e453e 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -83,6 +83,17 @@ class StepperMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + /** + * Measure resistance and inductance of a StepperMotor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * TODO: determine the correction factor + * @param voltage The voltage applied to the motor + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage){ + return FOCMotor::characteriseMotor(voltage, 1.0f); + } + private: /** Sensor alignment to electrical 0 angle of the motor */ diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 5d8f8127..06dfe63a 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -92,6 +92,227 @@ void FOCMotor::useMonitoring(Print &print){ #endif } +// Measure resistance and inductance of a motor +int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){ + if (!this->current_sense || !this->current_sense->initialized) + { + SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: CS unconfigured or not initialized"); + return 1; + } + + if (voltage <= 0.0f){ + SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: Voltage is negative or less than zero"); + return 2; + } + voltage = _constrain(voltage, 0.0f, voltage_limit); + + SIMPLEFOC_DEBUG("MOT: Measuring phase to phase resistance, keep motor still..."); + + float current_electric_angle = electricalAngle(); + + // Apply zero volts and measure a zero reference + setPhaseVoltage(0, 0, current_electric_angle); + _delay(500); + + PhaseCurrent_s zerocurrent_raw = current_sense->readAverageCurrents(); + DQCurrent_s zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle); + + + // Ramp and hold the voltage to measure resistance + // 300 ms of ramping + current_electric_angle = electricalAngle(); + for(int i=0; i < 100; i++){ + setPhaseVoltage(0, voltage/100.0*((float)i), current_electric_angle); + _delay(3); + } + _delay(10); + PhaseCurrent_s r_currents_raw = current_sense->readAverageCurrents(); + DQCurrent_s r_currents = current_sense->getDQCurrents(current_sense->getABCurrents(r_currents_raw), current_electric_angle); + + // Zero again + setPhaseVoltage(0, 0, current_electric_angle); + + + if (fabsf(r_currents.d - zerocurrent.d) < 0.2f) + { + SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: measured current too low"); + return 3; + } + + float resistance = voltage / (correction_factor * (r_currents.d - zerocurrent.d)); + if (resistance <= 0.0f) + { + SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: Calculated resistance <= 0"); + return 4; + } + + SIMPLEFOC_DEBUG("MOT: Estimated phase to phase resistance: ", 2.0f * resistance); + _delay(100); + + + // Start inductance measurement + SIMPLEFOC_DEBUG("MOT: Measuring inductance, keep motor still..."); + + unsigned long t0 = 0; + unsigned long t1 = 0; + float Ltemp = 0; + float Ld = 0; + float Lq = 0; + float d_electrical_angle = 0; + + uint iterations = 40; // how often the algorithm gets repeated. + uint cycles = 3; // averaged measurements for each iteration + uint risetime_us = 200; // initially short for worst case scenario with low inductance + uint settle_us = 100000; // initially long for worst case scenario with high inductance + + // Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it) + current_electric_angle += 0.5f * _PI; + current_electric_angle = _normalizeAngle(current_electric_angle); + + for (size_t axis = 0; axis < 2; axis++) + { + for (size_t i = 0; i < iterations; i++) + { + // current_electric_angle = i * _2PI / iterations; // <-- Do a sweep of the inductance. Use eg. for graphing + float inductanced = 0.0f; + + float qcurrent = 0.0f; + float dcurrent = 0.0f; + for (size_t j = 0; j < cycles; j++) + { + // read zero current + zerocurrent_raw = current_sense->readAverageCurrents(20); + zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle); + + // step the voltage + setPhaseVoltage(0, voltage, current_electric_angle); + t0 = micros(); + delayMicroseconds(risetime_us); // wait a little bit + + PhaseCurrent_s l_currents_raw = current_sense->getPhaseCurrents(); + t1 = micros(); + setPhaseVoltage(0, 0, current_electric_angle); + + DQCurrent_s l_currents = current_sense->getDQCurrents(current_sense->getABCurrents(l_currents_raw), current_electric_angle); + delayMicroseconds(settle_us); // wait a bit for the currents to go to 0 again + + if (t0 > t1) continue; // safety first + + // calculate the inductance + float dt = (t1 - t0)/1000000.0f; + if (l_currents.d - zerocurrent.d <= 0 || (voltage - resistance * (l_currents.d - zerocurrent.d)) <= 0) + { + continue; + } + + inductanced += fabsf(- (resistance * dt) / log((voltage - resistance * (l_currents.d - zerocurrent.d)) / voltage))/correction_factor; + + qcurrent+= l_currents.q - zerocurrent.q; // average the measured currents + dcurrent+= l_currents.d - zerocurrent.d; + } + qcurrent /= cycles; + dcurrent /= cycles; + + float delta = qcurrent / (fabsf(dcurrent) + fabsf(qcurrent)); + + + inductanced /= cycles; + Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4; + + float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R) + // SIMPLEFOC_DEBUG("MOT: Estimated time constant in us: ", 1000000.0f * timeconstant); + + // Wait as long as possible (due to limited timing accuracy & sample rate), but as short as needed (while the current still changes) + risetime_us = _constrain(risetime_us * 0.6f + 0.4f * 1000000 * 0.6f * timeconstant, 100, 10000); + settle_us = 10 * risetime_us; + + + // Serial.printf(">inductance:%f:%f|xy\n", current_electric_angle, Ltemp * 1000.0f); // <-- Plot an angle sweep + + + /** + * How this code works: + * If we apply a current spike in the d´-axis, there will be cross coupling to the q´-axis current, if we didn´t use the actual d-axis (ie. d´ != d). + * This has to do with saliency (Ld != Lq). + * The amount of cross coupled current is somewhat proportional to the angle error, which means that if we iteratively change the angle to min/maximise this current, we get the correct d-axis (and q-axis). + */ + if (axis) + { + qcurrent *= -1.0f; // to d or q axis + } + + if (qcurrent < 0) + { + current_electric_angle+=fabsf(delta); + } else + { + current_electric_angle-=fabsf(delta); + } + current_electric_angle = _normalizeAngle(current_electric_angle); + + + // Average the d-axis angle further for calculating the electrical zero later + if (axis) + { + d_electrical_angle = i < 2 ? current_electric_angle : d_electrical_angle * 0.9 + current_electric_angle * 0.1; + } + + } + + // We know the minimum is 0.5*PI radians away, so less iterations are needed. + current_electric_angle += 0.5f * _PI; + current_electric_angle = _normalizeAngle(current_electric_angle); + iterations /= 2; + + if (axis == 0) + { + Lq = Ltemp; + }else + { + Ld = Ltemp; + } + + } + + if (sensor) + { + /** + * The d_electrical_angle should now be aligned to the d axis or the -d axis. We can therefore calculate two possible electrical zero angles. + * We then report the one closest to the actual value. This could be useful if the zero search method is not reliable enough (eg. high pole count). + */ + + float estimated_zero_electric_angle_A = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle); + float estimated_zero_electric_angle_B = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI); + float estimated_zero_electric_angle = 0.0f; + if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle)) + { + estimated_zero_electric_angle = estimated_zero_electric_angle_A; + } else + { + estimated_zero_electric_angle = estimated_zero_electric_angle_B; + } + + SIMPLEFOC_DEBUG("MOT: Newly estimated electrical zero: ", estimated_zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Current electrical zero: ", zero_electric_angle); + } + + + SIMPLEFOC_DEBUG("MOT: Inductance measurement complete!"); + SIMPLEFOC_DEBUG("MOT: Measured D-inductance in mH: ", Ld * 1000.0f); + SIMPLEFOC_DEBUG("MOT: Measured Q-inductance in mH: ", Lq * 1000.0f); + if (Ld > Lq) + { + SIMPLEFOC_DEBUG("WARN: MOT: Measured inductance is larger in D than in Q axis. This is normally a sign of a measurement error."); + } + if (Ld * 2.0f < Lq) + { + SIMPLEFOC_DEBUG("WARN: MOT: Measured Q inductance is more than twice the D inductance. This is probably wrong. From experience, the lower value is probably close to reality."); + } + + return 0; + +} + // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 8ae0e8af..93ee1106 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -149,6 +149,15 @@ class FOCMotor */ float electricalAngle(); + /** + * Measure resistance and inductance of a motor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * @param voltage The voltage applied to the motor + * @param correction_factor Is 1.5 for 3 phase motors, because we measure over a series-parallel connection. TODO: what about 2 phase motors? + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage, float correction_factor); + // state variables float target; //!< current target value - depends of the controller float feed_forward_velocity = 0.0f; //!< current feed forward velocity From 65eff47c5126a814ba5731fbbbf87a1dab092910 Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Wed, 9 Oct 2024 15:34:01 +0200 Subject: [PATCH 13/53] change uint to unsigned int for unit tests --- src/common/base_classes/FOCMotor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 06dfe63a..090034ce 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -160,10 +160,10 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){ float Lq = 0; float d_electrical_angle = 0; - uint iterations = 40; // how often the algorithm gets repeated. - uint cycles = 3; // averaged measurements for each iteration - uint risetime_us = 200; // initially short for worst case scenario with low inductance - uint settle_us = 100000; // initially long for worst case scenario with high inductance + unsigned int iterations = 40; // how often the algorithm gets repeated. + unsigned int cycles = 3; // averaged measurements for each iteration + unsigned int risetime_us = 200; // initially short for worst case scenario with low inductance + unsigned int settle_us = 100000; // initially long for worst case scenario with high inductance // Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it) current_electric_angle += 0.5f * _PI; From 6ba2cf2d4bf5453b7f0d82ee41aeda752f347d70 Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Thu, 10 Oct 2024 20:24:33 +0200 Subject: [PATCH 14/53] Add characterisation example --- .../measure_inductance_and_resistance.ino | 119 ++++++++++++++++++ 1 file changed, 119 insertions(+) create mode 100644 examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino diff --git a/examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino b/examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino new file mode 100644 index 00000000..83a70a80 --- /dev/null +++ b/examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino @@ -0,0 +1,119 @@ +/** + * + * Motor characterisation example sketch. + * + */ +#include + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +// current sensor +InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); + +// characterisation voltage set point variable +float characteriseVolts = 0.0f; + +// instantiate the commander +Commander command = Commander(Serial); +void onMotor(char* cmd){command.motor(&motor,cmd);} +void characterise(char* cmd) { + command.scalar(&characteriseVolts, cmd); + motor.characteriseMotor(characteriseVolts); +} + +void setup() { + + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); + + // current sense init hardware + current_sense.init(); + // link the current sense to the motor + motor.linkCurrentSense(¤t_sense); + + // set torque mode: + // TorqueControlType::dc_current + // TorqueControlType::voltage + // TorqueControlType::foc_current + motor.torque_controller = TorqueControlType::foc_current; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // foc current control parameters (Arduino UNO/Mega) + motor.PID_current_q.P = 5; + motor.PID_current_q.I= 300; + motor.PID_current_d.P= 5; + motor.PID_current_d.I = 300; + motor.LPF_current_q.Tf = 0.01f; + motor.LPF_current_d.Tf = 0.01f; + // foc current control parameters (stm/esp/due/teensy) + // motor.PID_current_q.P = 5; + // motor.PID_current_q.I= 1000; + // motor.PID_current_d.P= 5; + // motor.PID_current_d.I = 1000; + // motor.LPF_current_q.Tf = 0.002f; // 1ms default + // motor.LPF_current_d.Tf = 0.002f; // 1ms default + + // comment out if not needed + motor.useMonitoring(Serial); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add commands M & L + command.add('M',&onMotor,"Control motor"); + command.add('L', characterise, "Characterise motor L & R with the given voltage"); + + motor.disable(); + + Serial.println(F("Motor disabled and ready.")); + Serial.println(F("Control the motor and measure the inductance using the terminal. Type \"?\" for available commands:")); + _delay(1000); +} + +void loop() { + + // main FOC algorithm function + // the faster you run this function the better + // Arduino UNO loop ~1kHz + // Bluepill loop ~10kHz + motor.loopFOC(); + + // Motion control function + // velocity, position or torque (defined in motor.controller) + // this function can be run at much lower frequency than loopFOC() function + // You can also use motor.move() and set the motor.target in the code + motor.move(); + + // user communication + command.run(); +} \ No newline at end of file From 6473a1728e8da1f7bce91c905dd31a1545215d84 Mon Sep 17 00:00:00 2001 From: runger1101001 Date: Thu, 7 Nov 2024 19:16:43 +0100 Subject: [PATCH 15/53] add ability to reserve timers so they don't get used for motors --- .../hardware_specific/stm32/stm32_mcu.cpp | 30 ++++++++++++++++++- .../hardware_specific/stm32/stm32_mcu.h | 6 ++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 1a73ddfa..ed9da6be 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -27,6 +27,10 @@ int numTimersUsed = 0; TIM_HandleTypeDef* timersUsed[SIMPLEFOC_STM32_MAX_TIMERSUSED]; +// reserve timers for other uses, so SimpleFOC doesn't use them for motors +int numTimersReserved = 0; +TIM_TypeDef* reservedTimers[SIMPLEFOC_STM32_MAX_TIMERSRESERVED]; + // track drivers initialized via SimpleFOC - used to know which timer channels are used int numMotorsUsed = 0; STM32DriverParams* motorsUsed[SIMPLEFOC_STM32_MAX_MOTORSUSED]; @@ -38,6 +42,16 @@ int stm32_getNumTimersUsed() { int stm32_getNumMotorsUsed() { return numMotorsUsed; } +int stm32_getNumTimersReserved() { + return numTimersReserved; +} +bool stm32_isTimerReserved(TIM_TypeDef* timer) { + for (int i=0; iperipheral)) { + return true; + } for (int i=0; itimers_handle[j] == NULL) break; @@ -65,7 +82,14 @@ TIM_HandleTypeDef* stm32_getTimer(PinMap* timer) { } return NULL; } - +bool stm32_reserveTimer(TIM_TypeDef* timer) { + if (numTimersReserved >= SIMPLEFOC_STM32_MAX_TIMERSRESERVED) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many timers reserved"); + return false; + } + reservedTimers[numTimersReserved++] = timer; + return true; +} // 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) { @@ -75,6 +99,10 @@ TIM_HandleTypeDef* stm32_useTimer(PinMap* timer) { SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many timers used"); return NULL; } + if (stm32_isTimerReserved((TIM_TypeDef*)timer->peripheral)) { + SIMPLEFOC_DEBUG("STM32-DRV: ERR: timer reserved"); + return NULL; + } handle = new TIM_HandleTypeDef(); handle->Instance = (TIM_TypeDef*)timer->peripheral; handle->Channel = HAL_TIM_ACTIVE_CHANNEL_CLEARED; diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index ee0bf569..feee6ba2 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -8,6 +8,9 @@ #ifndef SIMPLEFOC_STM32_MAX_TIMERSUSED #define SIMPLEFOC_STM32_MAX_TIMERSUSED 6 #endif +#ifndef SIMPLEFOC_STM32_MAX_TIMERSRESERVED +#define SIMPLEFOC_STM32_MAX_TIMERSRESERVED 4 +#endif #ifndef SIMPLEFOC_STM32_MAX_MOTORSUSED #define SIMPLEFOC_STM32_MAX_MOTORSUSED 4 #endif @@ -53,11 +56,14 @@ typedef struct STM32DriverParams { // timer allocation functions int stm32_getNumTimersUsed(); int stm32_getNumMotorsUsed(); +int stm32_getNumTimersReserved(); STM32DriverParams* stm32_getMotorUsed(int index); bool stm32_isTimerUsed(TIM_HandleTypeDef* timer); bool stm32_isChannelUsed(PinMap* pin); +bool stm32_isTimerReserved(TIM_TypeDef* timer); TIM_HandleTypeDef* stm32_getTimer(PinMap* timer); TIM_HandleTypeDef* stm32_useTimer(PinMap* timer); +bool stm32_reserveTimer(TIM_TypeDef* timer); void stm32_pause(STM32DriverParams* params); void stm32_resume(STM32DriverParams* params); From f6362e7bbd859ee9607a82f8d3834b99c6a0f766 Mon Sep 17 00:00:00 2001 From: runger1101001 Date: Thu, 7 Nov 2024 19:17:46 +0100 Subject: [PATCH 16/53] small fixes --- src/BLDCMotor.cpp | 4 +--- src/sensors/Encoder.h | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 501a8bc9..5ad8a93f 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -290,9 +290,7 @@ int BLDCMotor::alignSensor() { zero_electric_angle = electricalAngle(); //zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); _delay(20); - if(monitor_port){ - SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); - } + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); // stop everything setPhaseVoltage(0, 0, 0); _delay(200); diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index af6a5ab6..ab494d13 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -68,7 +68,7 @@ class Encoder: public Sensor{ /** * returns 0 if it does need search for absolute zero * 0 - encoder without index - * 1 - ecoder with index + * 1 - encoder with index */ int needsSearch() override; From c507b711017a4106dbe9e143b9295063dca44405 Mon Sep 17 00:00:00 2001 From: runger1101001 Date: Thu, 7 Nov 2024 19:20:57 +0100 Subject: [PATCH 17/53] fix for ESP32 C6 with >1 motors --- src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp index a481c6ff..80d936d1 100644 --- a/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp +++ b/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.cpp @@ -181,7 +181,7 @@ uint8_t _findNextTimer(int group){ */ int _findBestGroup(int no_pins, long pwm_freq, int* group, int* timer){ // an empty group is always the best option - for(int i=0; i<2; i++){ + for(int i=0; i Date: Thu, 7 Nov 2024 19:54:38 +0100 Subject: [PATCH 18/53] update readme with changes for 2.3.5 release --- README.md | 28 +++++++--------------------- 1 file changed, 7 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 4fbb7517..3dcc7385 100644 --- a/README.md +++ b/README.md @@ -29,27 +29,13 @@ Therefore this is an attempt to: - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) -> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.4 -> - ESP32 MCUs extended support [#414](https://github.com/simplefoc/Arduino-FOC/pull/414) -> - Transition to the arduino-esp32 version v3.x (ESP-IDF v5.x) [#387](https://github.com/espressif/arduino-esp32/releases) -> - New support for MCPWM driver -> - New support for LEDC drivers - center-aligned PWM and 6PWM available -> - Rewritten and simplified the fast ADC driver code (`adcRead`) - for low-side and inline current sensing. -> - Stepper motors current sensing support [#421](https://github.com/simplefoc/Arduino-FOC/pull/421) -> - Support for current sensing (low-side and inline) - [see in docs](https://docs.simplefoc.com/current_sense) -> - Support for true FOC control - `foc_current` torque control - [see in docs](https://docs.simplefoc.com/motion_control) -> - New current sense alignment procedure [#422](https://github.com/simplefoc/Arduino-FOC/pull/422) - [see in docs](https://docs.simplefoc.com/current_sense_align) -> - Support for steppers -> - Much more robust and reliable -> - More verbose and informative -> - Support for HallSensors without interrupts [#424](https://docs.simplefoc.com/https://github.com/simplefoc/Arduino-FOC/pull/424) - [see in docs](hall_sensors) -> - Docs -> - A short guide to debugging of common issues -> - A short guide to the units in the library - [see in docs](https://docs.simplefoc.com/library_units) -> - See the complete list of bugfixes and new features of v2.3.4 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/11) - - -## Arduino *SimpleFOClibrary* v2.3.4 +> NEXT RELEASE 📢 : SimpleFOClibrary v2.3.5 +> - Motor characterization code thanks to @mcells +> - Bugfix for ESP32 C6 thanks to @kondor1622 +> - See the complete list of bugfixes and new features of v2.3.5 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/12) + + +## Arduino *SimpleFOClibrary* v2.3.5

From 20e4481565a2e3fcaf3ee33fd6897e14c2655382 Mon Sep 17 00:00:00 2001 From: runger1101001 Date: Thu, 7 Nov 2024 19:55:23 +0100 Subject: [PATCH 19/53] bump library version for release --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 5daa49d9..354a2118 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.4 +version=2.3.5 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 1a267af6c90797601d548e972ab87d2b9badd966 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Mon, 3 Feb 2025 08:13:27 +0100 Subject: [PATCH 20/53] Update open_loop_velocity_6pwm.ino --- .../Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 9143dde7..64651392 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -38,7 +38,7 @@ void setup() { // limiting motor movements // limit the voltage to be set to the motor // start very low for high resistance motors - // currnet = resistance*voltage, so try to be well under 1Amp + // currnet = voltage/resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] // open loop control config From e11ab31083115d798ef64291fec4e72903147ab9 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Mon, 3 Feb 2025 08:14:02 +0100 Subject: [PATCH 21/53] Update open_loop_position_example.ino --- .../open_loop_position_example/open_loop_position_example.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino index c1ff7bbc..6db1b979 100644 --- a/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino +++ b/examples/motion_control/open_loop_motor_control/open_loop_position_example/open_loop_position_example.ino @@ -46,7 +46,7 @@ void setup() { // limiting motor movements // limit the voltage to be set to the motor // start very low for high resistance motors - // currnet = resistance*voltage, so try to be well under 1Amp + // currnet = voltage/resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] // limit/set the velocity of the transition in between // target angles @@ -78,4 +78,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} From 83c341b6321d6498ba94cc3a7dd1e44cd046fd08 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Mon, 3 Feb 2025 08:14:22 +0100 Subject: [PATCH 22/53] Update open_loop_velocity_6pwm.ino --- .../Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino index 5ff53311..c4627020 100644 --- a/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino +++ b/examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino @@ -53,7 +53,7 @@ void setup() { // limiting motor movements // limit the voltage to be set to the motor // start very low for high resistance motors - // currnet = resistance*voltage, so try to be well under 1Amp + // currnet = voltage/resistance, so try to be well under 1Amp motor.voltage_limit = 3; // [V] // open loop control config From 1f0be50f81f1b6151d5d629a2386a9a5a02ebd5f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 18 Feb 2025 22:51:11 +0100 Subject: [PATCH 23/53] #451 change DriverType "Unknown" to "UnknownDriver" --- src/common/base_classes/CurrentSense.h | 2 +- src/common/base_classes/FOCDriver.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index d3f7f8ed..f3d15b31 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -34,7 +34,7 @@ class CurrentSense{ FOCDriver* driver = nullptr; //!< driver link bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing - DriverType driver_type = DriverType::Unknown; //!< driver type (BLDC or Stepper) + DriverType driver_type = DriverType::UnknownDriver; //!< driver type (BLDC or Stepper) // ADC measurement gain for each phase diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 944251a4..15ba1c47 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -13,7 +13,7 @@ enum PhaseState : uint8_t { enum DriverType{ - Unknown=0, + UnknownDriver=0, BLDC=1, Stepper=2 }; From b434d8000adaa1802ec6d8c052f6e26732682809 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 18 Feb 2025 23:39:47 +0100 Subject: [PATCH 24/53] fix ESP32 example compilation --- .../esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index 0516ede1..0cb5d3cf 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -7,6 +7,8 @@ MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); +// example of esp32 defining 2nd bus +TwoWire Wire1(1); void setup() { From c5bfd51a7b37e936f8bdaf9d24d246af7ca6c9e1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 18 Feb 2025 23:48:46 +0100 Subject: [PATCH 25/53] fix compilation for ESP32S3 and DoIt DevKit --- .../esp32_i2c_dual_bus_example.ino | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino index 0cb5d3cf..a25e1c04 100644 --- a/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino +++ b/examples/utils/sensor_test/magnetic_sensors/magnetic_sensor_i2c/magnetic_sensor_i2c_dual_bus_examples/esp32_i2c_dual_bus_example/esp32_i2c_dual_bus_example.ino @@ -1,4 +1,5 @@ #include +#include /** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus * This example shows how a second i2c bus can be used to communicate with a second sensor. @@ -7,8 +8,8 @@ MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C); MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C); -// example of esp32 defining 2nd bus -TwoWire Wire1(1); +// example of esp32 defining 2nd bus, if not already defined +//TwoWire Wire1(1); void setup() { From 820ae072b46d813dedc28b3a054c33ad68d2214a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 18 Feb 2025 23:55:16 +0100 Subject: [PATCH 26/53] remove dual I2C bus example from ESP32 C3 compile --- .github/workflows/esp32.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml index db551b41..2f5accd5 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -33,7 +33,7 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32c3 # esp32c3 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino + sketch-names: esp32_position_control.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone.ino - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json From 8b288a9399c0e7cdf7edd50af0f32285250b4788 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 19 Feb 2025 00:08:31 +0100 Subject: [PATCH 27/53] exclude inductance/resistance example from UNO - too big --- .github/workflows/arduino.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml index cadd8ccc..9a4bc6c4 100644 --- a/.github/workflows/arduino.yml +++ b/.github/workflows/arduino.yml @@ -25,7 +25,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + sketches-exclude: measure_inductance_and_resistance, teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From 4ef37385dbcdce75592c5e4d3e2ca2c225541859 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 08:03:34 +0100 Subject: [PATCH 28/53] added the trgo info to the ADC --- .../hardware_specific/stm32/stm32_mcu.h | 2 ++ .../stm32/stm32f1/stm32f1_hal.cpp | 14 ++++++++++---- .../stm32/stm32f1/stm32f1_hal.h | 2 -- .../stm32/stm32f4/stm32f4_hal.cpp | 16 +++++++++++----- .../stm32/stm32f4/stm32f4_hal.h | 2 -- .../stm32/stm32f7/stm32f7_hal.cpp | 19 +++++++++++-------- .../stm32/stm32f7/stm32f7_hal.h | 2 -- .../stm32/stm32g4/stm32g4_hal.cpp | 11 +++++++++++ .../stm32/stm32g4/stm32g4_hal.h | 2 -- .../stm32/stm32l4/stm32l4_hal.cpp | 11 +++++++++++ .../stm32/stm32l4/stm32l4_hal.h | 2 -- 11 files changed, 56 insertions(+), 27 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h index 055f20db..5c7b1a28 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.h +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h @@ -3,6 +3,8 @@ #define STM32_CURRENTSENSE_MCU_DEF #include "../../hardware_api.h" #include "../../../common/foc_utils.h" +#include "../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../drivers/hardware_specific/stm32/stm32_timerutils.h" #if defined(_STM32_DEF_) 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 880fb3d6..2cc629fa 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -86,6 +86,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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 + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -101,12 +108,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; - } - // display which timer is being used + }else{ #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->Instance) + 1); + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif + } // first channel sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1; diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h index b0f4f83b..ae9de250 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h @@ -5,8 +5,6 @@ #if defined(STM32F1xx) #include "stm32f1xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); 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 5320cc3e..db7777c9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -80,6 +80,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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 + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -95,12 +102,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif } - // 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->Instance) + 1); - #endif // first channel diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h index 71071a56..b80a115a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -5,8 +5,6 @@ #if defined(STM32F4xx) #include "stm32f4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32f4_utils.h" 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 04575397..b9ab7f14 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -80,12 +80,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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; - uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + // check if TRGO used already - if yes use the next timer + if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -106,12 +110,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif } - // 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->Instance) + 1); - #endif // first channel diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h index 0a3614b5..63de7504 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -4,8 +4,6 @@ #if defined(STM32F7xx) #include "stm32f7xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32f7_utils.h" 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 84a9560c..6794a9f5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -130,6 +130,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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 + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels sConfigInjected.ExternalTrigInjecConv = trigger_flag; @@ -145,6 +152,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h index 2298b17c..3e00f0fa 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -6,8 +6,6 @@ #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) #include "stm32g4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32g4_utils.h" 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 8f7889c4..126e4dbb 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -128,6 +128,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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 + + // check if TRGO used already - if yes use the next timer + if((driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (driver_params->timers_handle[tim_num-1]->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } // if the code comes here, it has found the timer available // timer does have trgo flag for injected channels @@ -144,6 +151,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); #endif return -1; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h index 0317b74b..76146467 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -6,8 +6,6 @@ #if defined(STM32L4xx) #include "stm32l4xx_hal.h" -#include "../../../../common/foc_utils.h" -#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../stm32_mcu.h" #include "stm32l4_utils.h" From d91a96fcf3fa27bac2c92d8de1293c6e68f655a7 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 10:00:15 +0100 Subject: [PATCH 29/53] show error if not using ADC pins --- .../stm32/stm32f1/stm32f1_hal.cpp | 50 +++++++++++++------ .../stm32/stm32f1/stm32f1_hal.h | 2 +- .../stm32/stm32f1/stm32f1_mcu.cpp | 2 +- .../stm32/stm32f4/stm32f4_hal.cpp | 48 +++++++++++++----- .../stm32/stm32f4/stm32f4_hal.h | 2 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 2 +- .../stm32/stm32f7/stm32f7_hal.cpp | 49 +++++++++++++----- .../stm32/stm32f7/stm32f7_hal.h | 2 +- .../stm32/stm32f7/stm32f7_mcu.cpp | 2 +- .../stm32/stm32g4/stm32g4_hal.cpp | 49 +++++++++++++----- .../stm32/stm32g4/stm32g4_hal.h | 2 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 2 +- .../stm32/stm32l4/stm32l4_hal.cpp | 48 +++++++++++++----- .../stm32/stm32l4/stm32l4_hal.h | 2 +- .../stm32/stm32l4/stm32l4_mcu.cpp | 2 +- 15 files changed, 188 insertions(+), 76 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 2cc629fa..f8fd6fbf 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -41,6 +41,14 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; @@ -135,24 +143,38 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } - + return 0; } + extern "C" { void ADC1_2_IRQHandler(void) { diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h index ae9de250..fbcf4e99 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h @@ -8,7 +8,7 @@ #include "../stm32_mcu.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif 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 7d31f966..b52b48bd 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -42,7 +42,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_F1) / (_ADC_RESOLUTION_F1) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } 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 db7777c9..6fc97d69 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -9,6 +9,14 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; @@ -145,21 +153,35 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } extern "C" { diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h index b80a115a..ca73f181 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -9,7 +9,7 @@ #include "stm32f4_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #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 d3c7bd7f..85d142bc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -34,7 +34,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_F4) / (_ADC_RESOLUTION_F4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } 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 b9ab7f14..5d036cab 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -9,6 +9,14 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; @@ -159,21 +167,36 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) + +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h index 63de7504..ebd9e75e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -8,6 +8,6 @@ #include "stm32f7_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #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 e4b9c850..868aafd0 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -28,7 +28,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } 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 6794a9f5..79f6fd4f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -8,6 +8,14 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; @@ -195,23 +203,38 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } + extern "C" { void ADC1_2_IRQHandler(void) { diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h index 3e00f0fa..34fb947e 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -10,7 +10,7 @@ #include "stm32g4_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif 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 9be98860..a4d66b60 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -36,7 +36,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_G4) / (_ADC_RESOLUTION_G4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } 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 126e4dbb..2d5e5661 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -8,6 +8,14 @@ ADC_HandleTypeDef hadc; +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; @@ -194,21 +202,35 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return 0; } -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { - uint8_t cnt = 0; - if(_isset(pinA)){ - pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); - cs_params->pins[cnt++] = pinA; - } - if(_isset(pinB)){ - pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); - cs_params->pins[cnt++] = pinB; - } - if(_isset(pinC)){ - pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); - cs_params->pins[cnt] = pinC; + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } } + return 0; } extern "C" { diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h index 76146467..2004ff69 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -10,7 +10,7 @@ #include "stm32l4_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); -void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif 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 ed55b482..4ddc5484 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -35,7 +35,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA, const int .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, .adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4) }; - _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; return cs_params; } From 9b1de235b1491e78245c68d8618e782cc19536d9 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 15:31:33 +0100 Subject: [PATCH 30/53] moved the hybrid stepper to main + current sense support --- src/HybridStepperMotor.cpp | 588 ++++++++++++++++++ src/HybridStepperMotor.h | 113 ++++ src/SimpleFOC.h | 1 + src/common/base_classes/CurrentSense.cpp | 268 +++++++- src/common/base_classes/CurrentSense.h | 5 + src/common/base_classes/FOCDriver.h | 3 +- .../stm32/stm32f4/stm32f4_hal.cpp | 50 +- 7 files changed, 989 insertions(+), 39 deletions(-) create mode 100644 src/HybridStepperMotor.cpp create mode 100644 src/HybridStepperMotor.h diff --git a/src/HybridStepperMotor.cpp b/src/HybridStepperMotor.cpp new file mode 100644 index 00000000..2e70965f --- /dev/null +++ b/src/HybridStepperMotor.cpp @@ -0,0 +1,588 @@ +#include "HybridStepperMotor.h" +#include "./communication/SimpleFOCDebug.h" + +// HybridStepperMotor(int pp) +// - pp - pole pair number +// - R - motor phase resistance +// - KV - motor kv rating (rmp/v) +// - L - motor phase inductance [H] +HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _inductance) + : FOCMotor() +{ + // number od pole pairs + pole_pairs = pp; + // save phase resistance number + phase_resistance = _R; + // save back emf constant KV = 1/K_bemf + // usually used rms + KV_rating = _KV * _SQRT2; + // save phase inductance + phase_inductance = _inductance; + + // torque control type is voltage by default + // current and foc_current not supported yet + torque_controller = TorqueControlType::voltage; +} + +/** + Link the driver which controls the motor +*/ +void HybridStepperMotor::linkDriver(BLDCDriver *_driver) +{ + driver = _driver; +} + +// init hardware pins +int HybridStepperMotor::init() +{ + if (!driver || !driver->initialized) + { + motor_status = FOCMotorStatus::motor_init_failed; + SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized"); + return 0; + } + motor_status = FOCMotorStatus::motor_initializing; + SIMPLEFOC_DEBUG("MOT: Init"); + + // sanity check for the voltage limit configuration + if (voltage_limit > driver->voltage_limit) + voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if (voltage_sensor_align > voltage_limit) + voltage_sensor_align = voltage_limit; + + // update the controller limits + if (_isset(phase_resistance)) + { + // velocity control loop controls current + PID_velocity.limit = current_limit; + } + else + { + PID_velocity.limit = voltage_limit; + } + P_angle.limit = velocity_limit; + + // if using open loop control, set a CW as the default direction if not already set + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } + + _delay(500); + // enable motor + SIMPLEFOC_DEBUG("MOT: Enable driver."); + enable(); + _delay(500); + + motor_status = FOCMotorStatus::motor_uncalibrated; + return 1; +} + +// disable motor driver +void HybridStepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable driver + driver->disable(); + // motor status update + enabled = 0; +} +// enable motor driver +void HybridStepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + // motor status update + enabled = 1; +} + +/** + * FOC functions + */ +int HybridStepperMotor::initFOC() { + int exit_flag = 1; + + motor_status = FOCMotorStatus::motor_calibrating; + + // align motor if necessary + // alignment necessary for encoders! + // sensor and motor alignment - can be skipped + // by setting motor.sensor_direction and motor.zero_electric_angle + if(sensor){ + exit_flag *= alignSensor(); + // added the shaft_angle update + sensor->update(); + shaft_angle = shaftAngle(); + + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + current_sense->driver_type = DriverType::Hybrid; + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } + } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor + } + } + + if(exit_flag){ + SIMPLEFOC_DEBUG("MOT: Ready."); + motor_status = FOCMotorStatus::motor_ready; + }else{ + SIMPLEFOC_DEBUG("MOT: Init FOC failed."); + motor_status = FOCMotorStatus::motor_calib_failed; + disable(); + } + + return exit_flag; +} + +// Calibrate the motor and current sense phases +int HybridStepperMotor::alignCurrentSense() { + int exit_flag = 1; // success + + SIMPLEFOC_DEBUG("MOT: Align current sense."); + + // align current sense and the driver + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); + if(!exit_flag){ + // error in current sense - phase either not measured or bad connection + SIMPLEFOC_DEBUG("MOT: Align error!"); + exit_flag = 0; + }else{ + // output the alignment status flag + SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag); + } + + return exit_flag > 0; +} + +// Encoder alignment to electrical 0 angle +int HybridStepperMotor::alignSensor() +{ + int exit_flag = 1; // success + SIMPLEFOC_DEBUG("MOT: Align sensor."); + + // if unknown natural direction + if(sensor_direction==Direction::UNKNOWN) { + // check if sensor needs zero search + if (sensor->needsSearch()) + exit_flag = absoluteZeroSearch(); + // stop init if not found index + if (!exit_flag) + return exit_flag; + + // find natural direction + // move one electrical revolution forward + for (int i = 0; i <= 500; i++) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + // take and angle in the middle + sensor->update(); + float mid_angle = sensor->getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >= 0; i--) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); + _delay(2); + } + sensor->update(); + float end_angle = sensor->getAngle(); + setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + if (mid_angle == end_angle) + { + SIMPLEFOC_DEBUG("MOT: Failed to notice movement"); + return 0; // failed calibration + } + else if (mid_angle < end_angle) + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW"); + sensor_direction = Direction::CCW; + } + else + { + SIMPLEFOC_DEBUG("MOT: sensor_direction==CW"); + sensor_direction = Direction::CW; + } + // check pole pair number + float moved = fabs(mid_angle - end_angle); + if (fabs(moved * pole_pairs - _2PI) > 0.5f) + { // 0.5f is arbitrary number it can be lower or higher! + SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI / moved); + } + else + SIMPLEFOC_DEBUG("MOT: PP check: OK!"); + } + else + SIMPLEFOC_DEBUG("MOT: Skip dir calib."); + + // zero electric angle not known + if (!_isset(zero_electric_angle)) + { + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + _delay(700); + // read the sensor + sensor->update(); + // get the current zero electric angle + zero_electric_angle = 0; + zero_electric_angle = electricalAngle(); + _delay(20); + if (monitor_port) + { + SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle); + } + // stop everything + setPhaseVoltage(0, 0, 0); + _delay(200); + } + else + SIMPLEFOC_DEBUG("MOT: Skip offset calib."); + return exit_flag; +} + +// Encoder alignment the absolute zero angle +// - to the index +int HybridStepperMotor::absoluteZeroSearch() +{ + + SIMPLEFOC_DEBUG("MOT: Index search..."); + // search the absolute zero with small velocity + float limit_vel = velocity_limit; + float limit_volt = voltage_limit; + velocity_limit = velocity_index_search; + voltage_limit = voltage_sensor_align; + shaft_angle = 0; + while (sensor->needsSearch() && shaft_angle < _2PI) + { + angleOpenloop(1.5f * _2PI); + // call important for some sensors not to loose count + // not needed for the search + sensor->update(); + } + // disable motor + setPhaseVoltage(0, 0, 0); + // reinit the limits + velocity_limit = limit_vel; + voltage_limit = limit_volt; + // check if the zero found + if (monitor_port) + { + if (sensor->needsSearch()) + SIMPLEFOC_DEBUG("MOT: Error: Not found!"); + else + SIMPLEFOC_DEBUG("MOT: Success!"); + } + return !sensor->needsSearch(); +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void HybridStepperMotor::loopFOC() { + + // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track + // of full rotations otherwise. + if (sensor) sensor->update(); + + // if open-loop do nothing + if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; + + // if disabled do nothing + if(!enabled) return; + + // Needs the update() to be called first + // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() + // which is in range 0-2PI + electrical_angle = electricalAngle(); + switch (torque_controller) { + case TorqueControlType::voltage: + // no need to do anything really + break; + case TorqueControlType::dc_current: + if(!current_sense) return; + // read overall current magnitude + current.q = current_sense->getDCCurrent(electrical_angle); + // filter the value values + current.q = LPF_current_q(current.q); + // calculate the phase voltage + voltage.q = PID_current_q(current_sp - current.q); + // d voltage - lag compensation + if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + else voltage.d = 0; + break; + case TorqueControlType::foc_current: + if(!current_sense) return; + // read dq currents + current = current_sense->getFOCCurrents(electrical_angle); + // filter values + current.q = LPF_current_q(current.q); + current.d = LPF_current_d(current.d); + // calculate the phase voltages + voltage.q = PID_current_q(current_sp - current.q); + voltage.d = PID_current_d(-current.d); + // d voltage - lag compensation - TODO verify + // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + break; + default: + // no torque control selected + SIMPLEFOC_DEBUG("MOT: no torque control selected!"); + break; + } + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage.q, voltage.d, electrical_angle); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void HybridStepperMotor::move(float new_target) { + + // set internal target variable + if(_isset(new_target) ) target = new_target; + + // downsampling (optional) + if(motion_cnt++ < motion_downsample) return; + motion_cnt = 0; + + // shaft angle/velocity need the update() to be called first + // get shaft angle + // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float + // For this reason it is NOT precise when the angles become large. + // Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem + // when switching to a 2-component representation. + if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) + shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode + // get angular velocity + shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated + + // if disabled do nothing + if(!enabled) return; + + + // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; + // estimate the motor current if phase reistance available and current_sense not available + if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; + + // upgrade the current based voltage limit + switch (controller) { + case MotionControlType::torque: + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control + if(!_isset(phase_resistance)) voltage.q = target; + else voltage.q = target*phase_resistance + voltage_bemf; + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + }else{ + current_sp = target; // if current/foc_current torque control + } + break; + case MotionControlType::angle: + // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when + // the angles are large. This results in not being able to command small changes at high position values. + // to solve this, the delta-angle has to be calculated in a numerically precise way. + // angle set point + shaft_angle_sp = target; + // calculate velocity set point + shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle ); + shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit); + // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control + // if torque controlled through voltage + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } + break; + case MotionControlType::velocity: + // velocity set point - sensor precision: this calculation is numerically precise. + shaft_velocity_sp = target; + // calculate the torque command + current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control + // if torque controlled through voltage control + if(torque_controller == TorqueControlType::voltage){ + // use voltage if phase-resistance not provided + if(!_isset(phase_resistance)) voltage.q = current_sp; + else voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit); + // set d-component (lag compensation if known inductance) + if(!_isset(phase_inductance)) voltage.d = 0; + else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit); + } + break; + case MotionControlType::velocity_openloop: + // velocity control in open loop - sensor precision: this calculation is numerically precise. + shaft_velocity_sp = target; + voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor + voltage.d = 0; + break; + case MotionControlType::angle_openloop: + // angle control in open loop - + // TODO sensor precision: this calculation NOT numerically precise, and subject + // to the same problems in small set-point changes at high angles + // as the closed loop version. + shaft_angle_sp = target; + voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor + voltage.d = 0; + break; + } +} + +void HybridStepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) +{ + angle_el = _normalizeAngle(angle_el); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + float center; + + switch (foc_modulation) { + case FOCModulationType::Trapezoid_120: + case FOCModulationType::Trapezoid_150: + // not handled + Ua = 0; + Ub = 0; + Uc = 0; + break; + case FOCModulationType::SinePWM: + // C phase is fixed at half-rail to provide bias point for A, B legs + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + center = driver->voltage_limit / 2; + + Ua += center; + Ub += center; + Uc = center; + break; + + case FOCModulationType::SpaceVectorPWM: + // C phase moves in order to increase max bias on coils + Ua = (_ca * Ud) - (_sa * Uq); + Ub = (_sa * Ud) + (_ca * Uq); + + float Umin = fmin(fmin(Ua, Ub), 0); + float Umax = fmax(fmax(Ua, Ub), 0); + float Vo = -(Umin + Umax)/2 + driver->voltage_limit/2; + + Ua = Ua + Vo; + Ub = Ub + Vo; + Uc = Vo; + } + driver->setPwm(Ua, Ub, Uc); + +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +float HybridStepperMotor::velocityOpenloop(float target_velocity) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to achieve target velocity + shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts); + // for display purposes + shaft_velocity = target_velocity; + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +float HybridStepperMotor::angleOpenloop(float target_angle) +{ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6f; + // quick fix for strange cases (micros overflow + timestamp not defined) + if (Ts <= 0 || Ts > 0.5f) + Ts = 1e-3f; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if (abs(target_angle - shaft_angle) > abs(velocity_limit * Ts)) + { + shaft_angle += _sign(target_angle - shaft_angle) * abs(velocity_limit) * Ts; + shaft_velocity = velocity_limit; + } + else + { + shaft_angle = target_angle; + shaft_velocity = 0; + } + + // use voltage limit or current limit + float Uq = voltage_limit; + if (_isset(phase_resistance)) + { + Uq = _constrain(current_limit * phase_resistance + fabs(voltage_bemf), -voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf)) / phase_resistance; + } + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; + + return Uq; +} diff --git a/src/HybridStepperMotor.h b/src/HybridStepperMotor.h new file mode 100644 index 00000000..d75ef8cd --- /dev/null +++ b/src/HybridStepperMotor.h @@ -0,0 +1,113 @@ +/** + * @file HybridStepperMotor.h + * + */ + +#ifndef HybridStepperMotor_h +#define HybridStepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class HybridStepperMotor : public FOCMotor +{ +public: + /** + HybridStepperMotor class constructor + @param pp pole pair number + @param R motor phase resistance - [Ohm] + @param KV motor KV rating (1/K_bemf) - rpm/V + @param L motor phase inductance - [H] + */ + HybridStepperMotor(int pp, float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver handle for hardware peripheral control + */ + void linkDriver(BLDCDriver *driver); + + /** + * BLDCDriver link: + */ + BLDCDriver *driver; + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC() override; + + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the HybridStepperMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua, Ub, Uc; //!< Phase voltages used for inverse Park and Clarke transform + + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + +private: + int alignCurrentSense(); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroSearch(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + float velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + float angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + +#endif diff --git a/src/SimpleFOC.h b/src/SimpleFOC.h index 4e6815e5..b4f48f29 100644 --- a/src/SimpleFOC.h +++ b/src/SimpleFOC.h @@ -98,6 +98,7 @@ void loop() { #include "BLDCMotor.h" #include "StepperMotor.h" +#include "HybridStepperMotor.h" #include "sensors/Encoder.h" #include "sensors/MagneticSensorSPI.h" #include "sensors/MagneticSensorI2C.h" diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 4813dc3b..85ebd2c5 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -59,6 +59,22 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } + if (driver_type == DriverType::Hybrid){ + ABCurrent_s return_ABcurrent; + //ia + ib + ic = 0 + if(current.a == 0){ + return_ABcurrent.alpha = -current.c - current.b; + return_ABcurrent.beta = current.b; + }else if(current.b == 0){ + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = -current.c - current.a; + }else{ + return_ABcurrent.alpha = current.a; + return_ABcurrent.beta = current.b; + } + return return_ABcurrent; + } + // otherwise it's a BLDC motor and // calculate clarke transform float i_alpha, i_beta; @@ -141,10 +157,18 @@ int CurrentSense::driverAlign(float voltage, bool modulation_centered){ if (!initialized) return 0; // check if stepper or BLDC - if(driver_type == DriverType::Stepper) - return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); - else - return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); + switch(driver_type){ + case DriverType::BLDC: + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); + case DriverType::Stepper: + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); + case DriverType::Hybrid: + return alignHybridDriver(voltage, (BLDCDriver*)driver, modulation_centered); + default: + // driver type not supported + SIMPLEFOC_DEBUG("CS: Cannot align driver type!"); + return 0; + } } @@ -473,3 +497,239 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive } + + + +int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ + + _UNUSED(modulation_centered); + + bool phases_switched = 0; + bool phases_inverted = 0; + + // first find the middle phase, which will be set to the C phase + // set phase A active and phases B active, and C down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i), voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + PhaseCurrent_s c = readAverageCurrents(); + // disable the phases + bldc_driver->setPwm(0, 0, 0); + if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f && fabs(c.c) < 0.1f ){ + SIMPLEFOC_DEBUG("CS: Err too low current!"); + return 0; // measurement current too low + } + // find the highest magnitude in c + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cc[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cc[i]) continue; // current not measured + if(cc[i] > max_c){ + max_c = cc[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cc[j]) continue; // current not measured + float ratio = max_c / cc[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 0: // phase A is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c.a, c.c); + phases_switched = true; // signal that pins have been switched + break; + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + // switch phase B and C + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c.b, c.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is positive and invert the gain if so + // current c should be negative + if( _sign(c.c) > 0 ){ + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else{ + // c - middle phase is not measured + SIMPLEFOC_DEBUG("CS: Middle phase not measured!"); + if(_isset(pinC)){ + // switch the missing phase with the phase C + if(!_isset(pinA)){ + SIMPLEFOC_DEBUG("CS: Switch (A)NC-C"); + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c.a, c.c); + phases_switched = true; // signal that pins have been switched + }else if(!_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch (B)NC-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c.b, c.c); + phases_switched = true; // signal that pins have been switched + } + } + } + + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + + // Find the phase A + + // set phase A active and phases B down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(voltage/100.0*((float)i), 0, 0); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + // disable the phases + bldc_driver->setPwm(0, 0, 0); + + // check if currents are to low (lower than 100mA) + if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I on the phase C + + // find the highest magnitude in A + // and make sure it's around the same as the C current (if the phase C is measured) + + float ca[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + if(c.a && c.c){ + // if a and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.a) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-C currents not equal!"); + return 0; + } + }else if(c.b && c.c){ + // if a and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.a) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!"); + return 0; + }else{ + // if the current are equal + // switch phase A and B + SIMPLEFOC_DEBUG("CS: Switch A-B"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c.a, c.b); + phases_switched = true; // signal that pins have been switched + } + }else if(c.a && c.b){ + // check if one is significantly higher than the other + if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-B currents zero!"); + return 0; + }else{ + // if they are not equal take the highest as A + if (fabs(c.a) < fabs(c.b)){ + // switch phase A and B + SIMPLEFOC_DEBUG("CS: Switch A-B"); + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c.a, c.b); + phases_switched = true; // signal that pins have been switched + + } + } + } + // if we get here, phase A is aligned + // check if the current is negative and invert the gain if so + if( _sign(c.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // at this point the driver's phase A is aligned with the ADC pinA + // and the pin B should be the phase B + + // set phase B active and phases A down + // ramp 300ms + for(int i=0; i < 100; i++){ + bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + _delay(3); + } + _delay(500); + c = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + + // check if currents are to low (lower than 100mA) + if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // check the phase B + // find the highest magnitude in c + // and make sure it's around the same as the C current (if the phase C is measured) + float cb[3] = {fabs(c.a), fabs(c.b), fabs(c.c)}; + if(c.b && c.c){ + // if b and mid-phase c measured + // verify that they have almost the same magnitude + if((fabs(c.b) - fabs(c.c)) > 0.1f){ + SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!"); + return 0; + } + }else if(c.a && c.b){ + // check if one is significantly higher than the other + if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){ + SIMPLEFOC_DEBUG("CS: Err A-B currents zero!"); + return 0; + } + } + + // check if b has good polarity + if( _sign(c.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + + // construct the return flag + // if success and nothing changed return 1 + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; + return exit_flag; +} + + diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index f3d15b31..3d9ec1b5 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -135,6 +135,11 @@ class CurrentSense{ * Function used to align the current sense with the Stepper motor driver */ int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); + /** + * Function used to align the current sense with the Hybrid motor driver + */ + int alignHybridDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); + /** * Function used to read the average current values over N samples */ diff --git a/src/common/base_classes/FOCDriver.h b/src/common/base_classes/FOCDriver.h index 15ba1c47..263460b3 100644 --- a/src/common/base_classes/FOCDriver.h +++ b/src/common/base_classes/FOCDriver.h @@ -15,7 +15,8 @@ enum PhaseState : uint8_t { enum DriverType{ UnknownDriver=0, BLDC=1, - Stepper=2 + Stepper=2, + Hybrid=3 }; /** 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 6fc97d69..3a5c661a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -22,10 +22,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif @@ -116,39 +119,19 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); -#endif - return -1; - } - - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + #endif return -1; } } - cs_params->adc_handle = &hadc; return 0; } @@ -165,14 +148,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) { int pins[3] = {pinA, pinB, pinC}; - const char* port_names[3] = {"A", "B", "C"}; for(int i=0; i<3; i++){ if(_isset(pins[i])){ // check if pin is an analog pin if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ #ifdef SIMPLEFOC_STM32_DEBUG SimpleFOCDebug::print("STM32-CS: ERR: Pin "); - SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::print(i == 0 ? "A" : i == 1 ? "B" : "C"); SimpleFOCDebug::println(" does not belong to any ADC!"); #endif return -1; From ccb3b48437ac90158c9b883054099c4c2b159331 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 15:43:00 +0100 Subject: [PATCH 31/53] allow stm32 to have any current sense pin as _NC --- .../stm32/stm32f1/stm32f1_hal.cpp | 30 ++++++------ .../stm32/stm32f4/stm32f4_hal.cpp | 2 +- .../stm32/stm32f7/stm32f7_hal.cpp | 45 ++++++------------ .../stm32/stm32g4/stm32g4_hal.cpp | 46 ++++++------------ .../stm32/stm32l4/stm32l4_hal.cpp | 47 ++++++------------- 5 files changed, 60 insertions(+), 110 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 f8fd6fbf..4eb366e7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -122,24 +122,22 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - // first channel - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - // second channel - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_2; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_3; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + + + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1 + i; + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + #endif + return -1; + } } - cs_params->adc_handle = &hadc; - return 0; } 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 3a5c661a..4c465425 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -127,7 +127,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); #endif return -1; } 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 5d036cab..8f25bf63 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -22,10 +22,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif @@ -125,34 +128,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); -#endif - return -1; - } - - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } 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 79f6fd4f..7fd38921 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -21,10 +21,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif @@ -167,38 +170,19 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); -#endif - return -1; - } - - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } - cs_params->adc_handle = &hadc; return 0; } 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 2d5e5661..bc74f095 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -21,17 +21,19 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive ADC_InjectionConfTypeDef sConfigInjected; // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); - ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); #endif return -1; } - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); @@ -166,38 +168,19 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - // first channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); -#endif - return -1; - } - - // second channel - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); - if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; -#endif - return -1; - } - - // third channel - if exists - if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; -#endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif return -1; } } - cs_params->adc_handle = &hadc; return 0; } From dc1db4842f4769425f868118c31ebcbedc0c7bea Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 15:49:00 +0100 Subject: [PATCH 32/53] typo in f1 --- .../hardware_specific/stm32/stm32f1/stm32f1_hal.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 4eb366e7..f0ef07c4 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -129,10 +129,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive if (!_isset(cs_params->pins[i])) continue; sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1 + i; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC)); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC))); #endif return -1; } From 8268acfa24b3152d7a2dc39259dd3cb2a81f48ea Mon Sep 17 00:00:00 2001 From: Jeremiah Rose Date: Thu, 13 Mar 2025 12:05:41 +1100 Subject: [PATCH 33/53] Fix `Pin is not configured as analog channel` error MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Forum conversation on the issue here: https://community.simplefoc.com/t/pin-is-not-configured-as-analog-channel/6751/5 Current sense align is currently not working on ESP32 boards due to a `__analogChannelConfig(): Pin is not configured as analog channel` error during `current_sense.init()`. The error is coming from [this function](https://github.com/espressif/arduino-esp32/blob/bda7c4811728e538b4ea393a5be148cbb614daed/cores/esp32/esp32-hal-adc.c#L157C38-L172) in the `arduino-esp32` framework. It is checking to see if the pin type is `ESP32_BUS_TYPE_ADC_ONESHOT`. In `arduino-esp32`, it appears that the `ESP32_BUS_TYPE_ADC_ONESHOT` setting is only applied to a pin in the `__analogInit` function, which in turn is only ever invoked during an `analogRead` (or `analogReadMilliVolts`, which SimpleFOC doesn’t use). This is mainly a bug in `arduino-esp32` (since `analogSetPinAttenuation` should be able to work before an `analogRead` is called). But we can (and should) work around this in the SimpleFOC library since it may be a long time until a fix is released upstream. We can ensure the pin is initialised correctly by reversing the order of the `analogSetPinAttenuation` and the `analogRead` calls in the hardware specific driver for ESP32. --- src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index caf29c19..d3eceb83 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -162,8 +162,8 @@ bool IRAM_ATTR adcInit(uint8_t pin){ analogReadResolution(SIMPLEFOC_ADC_RES); } pinMode(pin, ANALOG); - analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); analogRead(pin); + analogSetPinAttenuation(pin, SIMPLEFOC_ADC_ATTEN); #if CONFIG_IDF_TARGET_ESP32 // if esp32 variant __configFastADCs(); From 360121de21adede93e7e1ebc3bf95f27b5a92df9 Mon Sep 17 00:00:00 2001 From: gospar Date: Thu, 13 Mar 2025 11:05:41 +0100 Subject: [PATCH 34/53] typo if a channel not used --- .../hardware_specific/stm32/stm32f1/stm32f1_hal.cpp | 4 ++-- .../hardware_specific/stm32/stm32f4/stm32f4_hal.cpp | 3 ++- .../hardware_specific/stm32/stm32f7/stm32f7_hal.cpp | 4 ++-- .../hardware_specific/stm32/stm32f7/stm32f7_utils.h | 4 ++-- .../hardware_specific/stm32/stm32g4/stm32g4_hal.cpp | 4 ++-- .../hardware_specific/stm32/stm32l4/stm32l4_hal.cpp | 4 ++-- 6 files changed, 12 insertions(+), 11 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 f0ef07c4..fa0445e1 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -123,12 +123,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - + uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1 + i; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC)); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 4c465425..bae57db9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -119,11 +119,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } + uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 8f25bf63..b5a46295 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -127,12 +127,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - + uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 47465e5d..5e60c69b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -16,11 +16,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 +https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc_ex.h#L178 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 +https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc.h#L331 uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance 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 7fd38921..25928992 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -169,12 +169,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - + uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 bc74f095..c4c19033 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -167,12 +167,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - + uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG From a2bd1ea8f95c95a1e778810f1d07aa580f56028b Mon Sep 17 00:00:00 2001 From: gospar Date: Thu, 13 Mar 2025 11:48:23 +0100 Subject: [PATCH 35/53] roll-back --- .../hardware_specific/stm32/stm32f1/stm32f1_hal.cpp | 3 +-- .../hardware_specific/stm32/stm32f4/stm32f4_hal.cpp | 3 +-- .../hardware_specific/stm32/stm32f7/stm32f7_hal.cpp | 3 +-- .../hardware_specific/stm32/stm32g4/stm32g4_hal.cpp | 3 +-- .../hardware_specific/stm32/stm32l4/stm32l4_hal.cpp | 4 ++-- 5 files changed, 6 insertions(+), 10 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 fa0445e1..ac122992 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -123,12 +123,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC)); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 bae57db9..4c465425 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -119,12 +119,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 b5a46295..96b09d00 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -127,12 +127,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 25928992..0ac50c80 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -169,12 +169,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint8_t rank_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 c4c19033..bc74f095 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -167,12 +167,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint8_t rank_no = 0; + for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + rank_no++; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG From 5dffc7bf2d5671291637e9103cb9f1c204f8278d Mon Sep 17 00:00:00 2001 From: gospar Date: Thu, 13 Mar 2025 11:50:52 +0100 Subject: [PATCH 36/53] f7 typo --- .../hardware_specific/stm32/stm32f7/stm32f7_utils.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 5e60c69b..2e125d35 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -16,11 +16,11 @@ uint32_t _getADCChannel(PinName pin); // timer to injected TRGO -https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc_ex.h#L178 +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc_ex.h#L178 uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); // timer to regular TRGO -https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc.h#L331 +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc.h#L331 uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance From b8240262287a6f82dc92b924180b5163d708db06 Mon Sep 17 00:00:00 2001 From: gospar Date: Thu, 13 Mar 2025 12:07:57 +0100 Subject: [PATCH 37/53] inital support for h7 low-side current sensing - compiless --- .../stm32/stm32h7/stm32h7_hal.cpp | 207 +++++++++++++++ .../stm32/stm32h7/stm32h7_hal.h | 13 + .../stm32/stm32h7/stm32h7_mcu.cpp | 116 +++++++++ .../stm32/stm32h7/stm32h7_utils.cpp | 240 ++++++++++++++++++ .../stm32/stm32h7/stm32h7_utils.h | 30 +++ 5 files changed, 606 insertions(+) create mode 100644 src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h create mode 100644 src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp new file mode 100644 index 00000000..d8ed8652 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp @@ -0,0 +1,207 @@ +#include "stm32h7_hal.h" + +#if defined(STM32H7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +ADC_HandleTypeDef hadc; + +/** + * Function initializing the ADC and the injected channels for the low-side current sensing + * + * @param cs_params - current sense parameters + * @param driver_params - driver parameters + * + * @return int - 0 if success + */ +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + // check if all pins belong to the same ADC + ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; + ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; + if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || + ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || + ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) + ){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); +#endif + return -1; + } + + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + + if(hadc.Instance == ADC1) __HAL_RCC_ADC12_CLK_ENABLE(); +#ifdef ADC2 // if defined ADC2 + else if(hadc.Instance == ADC2) __HAL_RCC_ADC12_CLK_ENABLE(); +#endif +#ifdef ADC3 // if defined ADC3 + else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE(); +#endif + else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); +#endif + return -1; // error not a valid ADC instance + } + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); +#endif + + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now +#if defined(ADC_VER_V5_V90) + // only for ADC3 + if(hadc.Instance == ADC3){ + hadc.Init.DataAlign = ADC3_DATAALIGN_RIGHT; + } + // more info here + // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L170C13-L170C27 + hadc.Init.DMAContinuousRequests = DISABLE; + // not sure about this one!!! maybe use: ADC_SAMPLING_MODE_NORMAL + hadc.Init.SamplingMode = ADC_SAMPLING_MODE_TRIGGER_CONTROLED; +#endif + hadc.Init.NbrOfConversion = 1; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + // if ADC1 or ADC2 + if(hadc.Instance == ADC1 || hadc.Instance == ADC2){ + // more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658 + sConfigInjected.InjectedNbrOfConversion = ADC_SAMPLETIME_2CYCLE_5; + }else { + // adc3 + // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L673 + sConfigInjected.InjectedNbrOfConversion = ADC3_SAMPLETIME_2CYCLES_5; + } + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISINGFALLING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + for (size_t i=0; i<6; i++) { + TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->Instance; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // check if TRGO used already - if yes use the next timer + if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync + (timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync + { + continue; + } + + // if the code comes here, it has found the timer available + // timer does have trgo flag for injected channels + sConfigInjected.ExternalTrigInjecConv = trigger_flag; + + // this will be the timer with which the ADC will sync + cs_params->timer_handle = timer_to_check; + // done + break; + } + if( cs_params->timer_handle == NP ){ + // not possible to use these timers for low-side current sense + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); + #endif + return -1; + }else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); +#endif + } + + for(int i=0; i<3; i++){ + // skip if not set + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + #endif + return -1; + } + } + + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif + + cs_params->adc_handle = &hadc; + return 0; +} + + +/** + * Function to initialize the ADC GPIO pins + * + * @param cs_params current sense parameters + * @param pinA pin number for phase A + * @param pinB pin number for phase B + * @param pinC pin number for phase C + * @return int 0 if success, -1 if error + */ +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + int pins[3] = {pinA, pinB, pinC}; + const char* port_names[3] = {"A", "B", "C"}; + for(int i=0; i<3; i++){ + if(_isset(pins[i])){ + // check if pin is an analog pin + if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){ +#ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: ERR: Pin "); + SimpleFOCDebug::print(port_names[i]); + SimpleFOCDebug::println(" does not belong to any ADC!"); +#endif + return -1; + } + pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC); + cs_params->pins[i] = pins[i]; + } + } + return 0; +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h new file mode 100644 index 00000000..623f2b85 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h @@ -0,0 +1,13 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32H7xx) +#include "stm32h7xx_hal.h" +#include "../stm32_mcu.h" +#include "stm32h7_utils.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); + +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp new file mode 100644 index 00000000..f07a8ad8 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp @@ -0,0 +1,116 @@ +#include "../../../hardware_api.h" + +#if defined(STM32H7xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32h7_hal.h" +#include "stm32h7_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + + +// array of values of 4 injected channels per adc instance (3) +uint32_t adc_val[3][4]={0}; +// does adc interrupt need a downsample - per adc (3) +bool needs_downsample[3] = {1}; +// downsampling variable - per adc (3) +uint8_t tim_downsample[3] = {1}; + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + return cs_params; +} + + +void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // if compatible timer has not been found + if (cs_params->timer_handle == NULL) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + + // stop all the timers for the driver + 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->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->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->Instance, LL_TIM_TRGO_UPDATE); + + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + + // restart all the timers of the driver + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } + } + return 0; +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + + // calculate the instance + int adc_index = _adcToIndex(AdcHandle); + + // if the timer han't repetition counter - downsample two times + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { + tim_downsample[adc_index] = 0; + return; + } + + adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp new file mode 100644 index 00000000..41db1d9a --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp @@ -0,0 +1,240 @@ +#include "stm32h7_utils.h" + +#if defined(STM32H7xx) + +/* Exported Functions */ + + +PinName analog_to_pin(uint32_t pin) { + PinName pin_name = analogInputToPinName(pin); + if (pin_name == NC) { + return (PinName) pin; + } + return pin_name; +} + + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc_ex.h#L235 +uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ + + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIGINJEC_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIGINJEC_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIGINJEC_T4_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->Instance == TIM3) + return ADC_EXTERNALTRIGINJEC_T3_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->Instance == TIM6) + return ADC_EXTERNALTRIGINJEC_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIGINJEC_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->Instance == TIM15) + return ADC_EXTERNALTRIGINJEC_T15_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L554 +uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIG_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIG_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->Instance == TIM3) + return ADC_EXTERNALTRIG_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIG_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->Instance == TIM6) + return ADC_EXTERNALTRIG_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIG_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->Instance == TIM15) + return ADC_EXTERNALTRIG_T15_TRGO; +#endif +#ifdef TIM23 // if defined timer 23 + else if(timer->Instance == TIM23) + return ADC_EXTERNALTRIG_T23_TRGO; +#endif +#ifdef TIM24 // if defined timer 24 + else if(timer->Instance == TIM24) + return ADC_EXTERNALTRIG_T24_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h new file mode 100644 index 00000000..22dfbed1 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h @@ -0,0 +1,30 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32H7xx) + +#define _TRGO_NOT_AVAILABLE 12345 + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +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(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(TIM_HandleTypeDef* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif \ No newline at end of file From 412c47ab68032256a4df7d1b12d3bf05bc01ecc5 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 14 Mar 2025 09:31:09 +0100 Subject: [PATCH 38/53] use default dir only if sensor not connected --- src/BLDCMotor.cpp | 11 +++++++---- src/HybridStepperMotor.cpp | 12 ++++++++---- src/StepperMotor.cpp | 11 +++++++---- 3 files changed, 22 insertions(+), 12 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 5ad8a93f..74c3ce2f 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -93,10 +93,13 @@ int BLDCMotor::init() { P_angle.limit = velocity_limit; // if using open loop control, set a CW as the default direction if not already set - if ((controller==MotionControlType::angle_openloop - ||controller==MotionControlType::velocity_openloop) - && (sensor_direction == Direction::UNKNOWN)) { - sensor_direction = Direction::CW; + // only if no sensor is used + if(!sensor){ + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } } _delay(500); diff --git a/src/HybridStepperMotor.cpp b/src/HybridStepperMotor.cpp index 2e70965f..618985d9 100644 --- a/src/HybridStepperMotor.cpp +++ b/src/HybridStepperMotor.cpp @@ -30,6 +30,7 @@ HybridStepperMotor::HybridStepperMotor(int pp, float _R, float _KV, float _induc void HybridStepperMotor::linkDriver(BLDCDriver *_driver) { driver = _driver; + SIMPLEFOC_DEBUG("MOT: BLDCDriver linked, using pin C as the mid-phase"); } // init hardware pins @@ -64,10 +65,13 @@ int HybridStepperMotor::init() P_angle.limit = velocity_limit; // if using open loop control, set a CW as the default direction if not already set - if ((controller==MotionControlType::angle_openloop - ||controller==MotionControlType::velocity_openloop) - && (sensor_direction == Direction::UNKNOWN)) { - sensor_direction = Direction::CW; + // only if no sensor is used + if(!sensor){ + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } } _delay(500); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 1d8f3147..ec805914 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -57,10 +57,13 @@ int StepperMotor::init() { P_angle.limit = velocity_limit; // if using open loop control, set a CW as the default direction if not already set - if ((controller==MotionControlType::angle_openloop - ||controller==MotionControlType::velocity_openloop) - && (sensor_direction == Direction::UNKNOWN)) { - sensor_direction = Direction::CW; + // only if no sensor is used + if(!sensor){ + if ((controller==MotionControlType::angle_openloop + ||controller==MotionControlType::velocity_openloop) + && (sensor_direction == Direction::UNKNOWN)) { + sensor_direction = Direction::CW; + } } _delay(500); From 5f9c23b9db8aa1f40e3bfcd2b50c06d2061017db Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 18 Mar 2025 12:09:45 +0100 Subject: [PATCH 39/53] inital testing for h7 --- .../stm32/stm32h7/stm32h7_hal.cpp | 26 +++++++++++++------ .../stm32/stm32h7/stm32h7_mcu.cpp | 4 +++ 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp index d8ed8652..b572a5aa 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp @@ -35,7 +35,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return -1; } - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); @@ -74,10 +73,15 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L170C13-L170C27 hadc.Init.DMAContinuousRequests = DISABLE; // not sure about this one!!! maybe use: ADC_SAMPLING_MODE_NORMAL - hadc.Init.SamplingMode = ADC_SAMPLING_MODE_TRIGGER_CONTROLED; + hadc.Init.SamplingMode = ADC_SAMPLING_MODE_BULB; #endif hadc.Init.NbrOfConversion = 1; + hadc.Init.NbrOfDiscConversion = 0; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + + hadc.Init.LowPowerAutoWait = DISABLE; + + if ( HAL_ADC_Init(&hadc) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); @@ -91,16 +95,20 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // if ADC1 or ADC2 if(hadc.Instance == ADC1 || hadc.Instance == ADC2){ // more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658 - sConfigInjected.InjectedNbrOfConversion = ADC_SAMPLETIME_2CYCLE_5; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLE_5; }else { // adc3 // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L673 - sConfigInjected.InjectedNbrOfConversion = ADC3_SAMPLETIME_2CYCLES_5; + sConfigInjected.InjectedSamplingTime = ADC3_SAMPLETIME_2CYCLES_5; } sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISINGFALLING; sConfigInjected.AutoInjectedConv = DISABLE; sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; sConfigInjected.InjectedOffset = 0; + sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + sConfigInjected.QueueInjectedContext = DISABLE; + sConfigInjected.InjecOversamplingMode = DISABLE; // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; @@ -139,11 +147,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } + uint32_t ranks[4] = {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; for(int i=0; i<3; i++){ // skip if not set - if (!_isset(cs_params->pins[i])) continue; - - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + if (!_isset(cs_params->pins[i])) continue; + sConfigInjected.InjectedRank = ranks[i]; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG @@ -152,7 +160,9 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return -1; } } - + + + delay(1000); #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT // enable interrupt HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp index f07a8ad8..fe7f16ed 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp @@ -60,6 +60,9 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // set the trigger output event 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_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED); + // start the adc #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); @@ -67,6 +70,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ HAL_ADCEx_InjectedStart(cs_params->adc_handle); #endif + // restart all the timers of the driver stm32_resume(driver_params); From f1823852eea549f7a0850114084cfa9d3458a384 Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 18 Mar 2025 12:10:02 +0100 Subject: [PATCH 40/53] target_h7 to stm32h7 --- .../stm32/stm32_timerutils.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp index 3eef0849..73ddba86 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_STM32H7) +#if defined(_STM32_DEF_) || defined(STM32H7xx) void stm32_pauseTimer(TIM_HandleTypeDef* handle){ @@ -213,7 +213,7 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #endif return -1; } -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) || defined(TARGET_STM32H7) +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) || defined(STM32H7xx) // function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found @@ -252,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) && !defined(TARGET_STM32H7) + #if defined(TIM5) && !defined(STM32H7xx) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; #endif } @@ -268,10 +268,10 @@ 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) && !defined(TARGET_STM32H7) + #if defined(TIM5) && !defined(STM32H7xx) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; #endif - #if defined(TIM5) && defined(TARGET_STM32H7) + #if defined(TIM5) && defined(STM32H7xx) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; #endif } @@ -290,10 +290,10 @@ 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) && !defined(TARGET_STM32H7) + #if defined(TIM5) && !defined(STM32H7xx) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; #endif - #if defined(TIM5) && defined(TARGET_STM32H7) + #if defined(TIM5) && defined(STM32H7xx) else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; #endif } @@ -326,7 +326,7 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #endif } #endif - #if defined(TIM15) && defined(TARGET_STM32H7) + #if defined(TIM15) && defined(STM32H7xx) else if (TIM_master == TIM15){ #if defined(TIM1) if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; @@ -514,7 +514,7 @@ uint32_t stm32_getTimerClockFreq(TIM_HandleTypeDef *handle) { return 0; } -#if defined(STM32H7xx) || defined(TARGET_STM32H7) +#if defined(STM32H7xx) || 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. From b7c663844f756d158a04a54914e44902d3eaf1ed Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 19 Mar 2025 08:41:46 +0100 Subject: [PATCH 41/53] add the compile for portenta and giga --- .github/workflows/arduino_mbed.yml | 47 ++++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 .github/workflows/arduino_mbed.yml diff --git a/.github/workflows/arduino_mbed.yml b/.github/workflows/arduino_mbed.yml new file mode 100644 index 00000000..62de3080 --- /dev/null +++ b/.github/workflows/arduino_mbed.yml @@ -0,0 +1,47 @@ +name: AVR +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:mbed:giga # arudino giga + - arduino:mbed:nanorp2040connect # arduino nano rp2040 connect + - arduino:mbed:portenta # arduino portenta + + include: + - arduino-boards-fqbn: arduino:mbed:nanorp2040connect + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:mbed:portenta + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:mbed:giga + sketch-names: single_full_control_example.ino + + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} From 9e81c2ccb5c148fb93bf63bae08a317636f50144 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 19 Mar 2025 08:52:27 +0100 Subject: [PATCH 42/53] issues with compile CI --- .github/workflows/arduino_mbed.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/arduino_mbed.yml b/.github/workflows/arduino_mbed.yml index 62de3080..abffacb2 100644 --- a/.github/workflows/arduino_mbed.yml +++ b/.github/workflows/arduino_mbed.yml @@ -18,13 +18,17 @@ jobs: arduino-boards-fqbn: - arduino:mbed:giga # arudino giga - arduino:mbed:nanorp2040connect # arduino nano rp2040 connect + - arduino:mbed:nano33ble # arduino nano 33 ble - arduino:mbed:portenta # arduino portenta include: - arduino-boards-fqbn: arduino:mbed:nanorp2040connect sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:mbed:nano33ble + sketch-names: open_loop_position_example.ino - - arduino-boards-fqbn: arduino:mbed:portenta + - arduino-boards-fqbn: arduino:mbed:envie_m7 sketch-names: open_loop_position_example.ino - arduino-boards-fqbn: arduino:mbed:giga From d6b83cbcb61459f0c3ce137b424be572784ba1ee Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 19 Mar 2025 08:56:55 +0100 Subject: [PATCH 43/53] remove giga from CI --- .github/workflows/arduino_mbed.yml | 10 +++++----- README.md | 1 + 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/arduino_mbed.yml b/.github/workflows/arduino_mbed.yml index abffacb2..8d1b47e5 100644 --- a/.github/workflows/arduino_mbed.yml +++ b/.github/workflows/arduino_mbed.yml @@ -1,4 +1,4 @@ -name: AVR +name: MDED on: [push, pull_request] jobs: lint: @@ -16,10 +16,10 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - arduino:mbed:giga # arudino giga + #- arduino:mbed:giga # arudino giga - arduino:mbed:nanorp2040connect # arduino nano rp2040 connect - arduino:mbed:nano33ble # arduino nano 33 ble - - arduino:mbed:portenta # arduino portenta + - arduino:mbed:envie_m7 # arduino portenta include: - arduino-boards-fqbn: arduino:mbed:nanorp2040connect @@ -31,8 +31,8 @@ jobs: - arduino-boards-fqbn: arduino:mbed:envie_m7 sketch-names: open_loop_position_example.ino - - arduino-boards-fqbn: arduino:mbed:giga - sketch-names: single_full_control_example.ino + # - arduino-boards-fqbn: arduino:mbed:giga + # sketch-names: single_full_control_example.ino # Do not cancel all jobs / architectures if one job fails diff --git a/README.md b/README.md index 3dcc7385..19bdd96c 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,7 @@ [![RP2040 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml) [![SAMD build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml) [![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) +[![MBED build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino_mbed.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino_mbed.yml) ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) From 74face78552c1024422d7a1e1b7304a1d68355ff Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 19 Mar 2025 10:49:48 +0100 Subject: [PATCH 44/53] a rewrite for portenta + bugfixes --- .../hardware_specific/stm32/stm32_mcu.cpp | 2 +- .../stm32/stm32_timerutils.cpp | 154 +++++++++++++++--- 2 files changed, 135 insertions(+), 21 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index ed9da6be..1a9c833e 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_STM32H7) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) // if stm32duino or portenta #pragma message("") #pragma message("SimpleFOC: compiling for STM32") diff --git a/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp b/src/drivers/hardware_specific/stm32/stm32_timerutils.cpp index 73ddba86..b4a4fbf2 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(STM32H7xx) +#if defined(_STM32_DEF_) || defined(TARGET_STM32H7) // if stm32duino or portenta void stm32_pauseTimer(TIM_HandleTypeDef* handle){ @@ -213,11 +213,17 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #endif return -1; } -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) || defined(STM32H7xx) +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) || defined(STM32H7xx) || defined(TARGET_STM32H7) // 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 +// currently supports the master timers to be from +// +// fammilies | timers +// --------------| -------------------------------- +// f1,f4,f7 | TIM1 to TIM4 and TIM8 +// l4 | TIM1 to TIM4, TIM8 and TIM15 +// h7 | TIM1 to TIM5, TIM8, TIM15, TIM23 and TIM24 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; @@ -236,6 +242,14 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #if defined(TIM8) else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; #endif + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR0; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR0; + #endif + #endif } #endif #if defined(TIM2) && defined(LL_TIM_TS_ITR1) @@ -252,8 +266,17 @@ 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) && !defined(STM32H7xx) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR1; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif #endif } #endif @@ -268,11 +291,20 @@ 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) && !defined(STM32H7xx) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM5) && defined(STM32H7xx) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR2; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif #endif } #endif @@ -290,17 +322,27 @@ 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) && !defined(STM32H7xx) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; - #endif - #if defined(TIM5) && defined(STM32H7xx) - else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR3; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR3; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR2; + #endif #endif } #endif #if defined(TIM5) else if (TIM_master == TIM5){ - #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + #if defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32F7xx) // f1, f4 adn f7 have tim5 sycned with tim1 and tim3 while others (l4, h7) have tim15 #if defined(TIM1) if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; #endif @@ -311,6 +353,15 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #if defined(TIM8) if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; #endif + + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR4; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR4; + #endif + #endif } #endif #if defined(TIM8) @@ -321,12 +372,25 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #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; + + #if defined(STM32H7xx) || defined(TARGET_STM32H7) + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM23) + else if(TIM_slave == TIM23) return LL_TIM_TS_ITR5; + #endif + #if defined(TIM24) + else if(TIM_slave == TIM24) return LL_TIM_TS_ITR5; + #endif + #else + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR3; + #endif #endif } #endif - #if defined(TIM15) && defined(STM32H7xx) + #if defined(TIM15) && (defined(STM32L4xx) || defined(STM32H7xx) || defined(TARGET_STM32H7) ) else if (TIM_master == TIM15){ #if defined(TIM1) if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; @@ -336,6 +400,56 @@ int stm32_getInternalSourceTrigger(TIM_HandleTypeDef* master, TIM_HandleTypeDef* #endif } #endif + #if defined(TIM23) && (defined(STM32H7xx) || defined(TARGET_STM32H7)) + else if (TIM_master == TIM23){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM3) + if(TIM_slave == TIM3) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM4) + if(TIM_slave == TIM4) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM5) + if(TIM_slave == TIM5) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR12; + #endif + #if defined(TIM24) + if(TIM_slave == TIM24) return LL_TIM_TS_ITR12; + #endif + } + #endif + #if defined(TIM24) && (defined(STM32H7xx) || defined(TARGET_STM32H7)) + else if (TIM_master == TIM24){ + #if defined(TIM1) + if(TIM_slave == TIM1) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM2) + if(TIM_slave == TIM2) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM3) + if(TIM_slave == TIM3) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM4) + if(TIM_slave == TIM4) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM5) + if(TIM_slave == TIM5) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR13; + #endif + #if defined(TIM23) + if(TIM_slave == TIM23) return LL_TIM_TS_ITR13; + #endif + } + #endif return -1; // combination not supported } #else @@ -514,7 +628,7 @@ uint32_t stm32_getTimerClockFreq(TIM_HandleTypeDef *handle) { return 0; } -#if defined(STM32H7xx) || 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. From 36e078c0ae5ae5a95e602127e7d36e13c5df5792 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 10:26:51 +0100 Subject: [PATCH 45/53] inital update of the cs for stm32 --- .../stm32/stm32_adc_utils.cpp | 268 ++++++++++++++++++ .../stm32f4_utils.h => stm32_adc_utils.h} | 25 +- .../hardware_specific/stm32/stm32_mcu.cpp | 1 + .../hardware_specific/stm32/stm32_mcu.h | 1 + .../stm32/stm32f4/stm32f4_hal.h | 2 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 2 +- .../stm32/stm32f4/stm32f4_utils.cpp | 152 +--------- .../stm32/stm32f7/stm32f7_hal.h | 8 +- .../stm32/stm32f7/stm32f7_mcu.cpp | 2 +- .../stm32/stm32f7/stm32f7_utils.cpp | 161 +---------- .../stm32/stm32f7/stm32f7_utils.h | 30 -- .../stm32/stm32g4/stm32g4_hal.cpp | 11 +- .../stm32/stm32g4/stm32g4_hal.h | 2 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 4 +- .../stm32/stm32g4/stm32g4_utils.cpp | 152 +--------- .../stm32/stm32g4/stm32g4_utils.h | 34 --- .../stm32/stm32l4/stm32l4_hal.h | 2 +- .../stm32/stm32l4/stm32l4_mcu.cpp | 2 +- .../stm32/stm32l4/stm32l4_utils.cpp | 151 +--------- .../stm32/stm32l4/stm32l4_utils.h | 34 --- 20 files changed, 311 insertions(+), 733 deletions(-) create mode 100644 src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp rename src/current_sense/hardware_specific/stm32/{stm32f4/stm32f4_utils.h => stm32_adc_utils.h} (51%) delete mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h delete mode 100644 src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h delete mode 100644 src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp new file mode 100644 index 00000000..fac4a377 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -0,0 +1,268 @@ +#include "stm32_adc_utils.h" + +#if defined(_STM32_DEF_) + +// for searching the best ADCs, we need to know the number of ADCs +// it might be better to use some HAL variable for example ADC_COUNT +// here I've assumed the maximum number of ADCs is 5 +#define ADC_COUNT 5 + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + + +ADC_TypeDef* _indexToADC(uint8_t index){ + switch (index) { + case 0: + return ADC1; + break; +#ifdef ADC2 // if ADC2 exists + case 1: + return ADC2; + break; +#endif +#ifdef ADC3 // if ADC3 exists + case 2: + return ADC3; + break; +#endif +#ifdef ADC4 // if ADC4 exists + case 3: + return ADC4; + break; +#endif +#ifdef ADC5 // if ADC5 exists + case 4: + return ADC5; + break; +#endif + } + return nullptr; +} + + +// functions finding the index of the first pin entry in the PinMap_ADC +// returns -1 if not found +int _findIndexOfFirstPinMapADCEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while ((PinMap_ADC[i].pin & ~ALTX_MASK) !=NC) { + if (pinName == (PinMap_ADC[i].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} + +// functions finding the index of the last pin entry in the PinMap_ADC +// returns -1 if not found +int _findIndexOfLastPinMapADCEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + int i = 0; + while (PinMap_ADC[i].pin!=NC) { + if ( pinName == (PinMap_ADC[i].pin & ~ALTX_MASK) + && pinName != (PinMap_ADC[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} + +// find the best ADC combination for the given pins +// returns the index of the best ADC +// each pin can be connected to multiple ADCs +// the function will try to find a single ADC that can be used for all pins +// if not possible it will return nullptr +ADC_TypeDef* _findBestADCForPins(int numPins, int pins[]) { + + // assuning that there is less than 8 ADCs + uint8_t pins_at_adc[ADC_COUNT] = {0}; + + // check how many pins are there and are not set + int no_pins = 0; + for (int i = 0; i < numPins; i++) { + if(_isset(pins[i])) no_pins++; + } + + // loop over all elements and count the pins connected to each ADC + for (int i = 0; i < numPins; i++) { + int pin = pins[i]; + if(!_isset(pin)) continue; + + int index = _findIndexOfFirstPinMapADCEntry(pin); + int last_index = _findIndexOfLastPinMapADCEntry(pin); + if (index == -1) { + return nullptr; + } + for (int j = index; j <= last_index; j++) { + if (PinMap_ADC[j].pin == NC) { + break; + } + int adcIndex = _adcToIndex((ADC_TypeDef*)PinMap_ADC[j].peripheral); + pins_at_adc[adcIndex]++; + } + } + + for (int i = 0; i < ADC_COUNT; i++) { + if(!pins_at_adc[i]) continue; + SimpleFOCDebug::print("STM32-CS: ADC"); + SimpleFOCDebug::print(i+1); + SimpleFOCDebug::print(" pins: "); + SimpleFOCDebug::println(pins_at_adc[i]); + } + + // now take the first ADC that has all pins connected + for (int i = 0; i < ADC_COUNT; i++) { + if (pins_at_adc[i] == no_pins) { + return _indexToADC(i); + } + } + return nullptr; +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h similarity index 51% rename from src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h rename to src/current_sense/hardware_specific/stm32/stm32_adc_utils.h index 05303965..0dd2a2a2 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h @@ -1,13 +1,14 @@ - -#ifndef STM32F4_UTILS_HAL -#define STM32F4_UTILS_HAL +#ifndef STM32_ADC_UTILS_HAL +#define STM32_ADC_UTILS_HAL #include "Arduino.h" -#if defined(STM32F4xx) +#if defined(_STM32_DEF_) #define _TRGO_NOT_AVAILABLE 12345 +#include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" /* Exported Functions */ /** @@ -17,18 +18,22 @@ */ 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 +// timer to injected TRGO - architecure specific 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 +// timer to regular TRGO - architecure specific uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); // function returning index of the ADC instance int _adcToIndex(ADC_HandleTypeDef *AdcHandle); int _adcToIndex(ADC_TypeDef *AdcHandle); -#endif -#endif \ No newline at end of file + + +// functions helping to find the best ADC channel +int _findIndexOfFirstPinMapADCEntry(int pin); +int _findIndexOfLastPinMapADCEntry(int pin); +ADC_TypeDef* _findBestADCForPins(int num_pins, int pins[]); +#endif +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp index 94253d74..efc55733 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp @@ -30,4 +30,5 @@ __attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* c return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } + #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32_mcu.h b/src/current_sense/hardware_specific/stm32/stm32_mcu.h index 5c7b1a28..564598d3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_mcu.h +++ b/src/current_sense/hardware_specific/stm32/stm32_mcu.h @@ -19,5 +19,6 @@ typedef struct Stm32CurrentSenseParams { TIM_HandleTypeDef* timer_handle = NP; } Stm32CurrentSenseParams; + #endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h index ca73f181..f0f9a03d 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -6,7 +6,7 @@ #if defined(STM32F4xx) #include "stm32f4xx_hal.h" #include "../stm32_mcu.h" -#include "stm32f4_utils.h" +#include "../stm32_adc_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); 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 85d142bc..ed58e8cb 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -6,8 +6,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32f4_hal.h" -#include "stm32f4_utils.h" #include "Arduino.h" 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 8a636c84..23d4b0d3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -1,136 +1,7 @@ -#include "stm32f4_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32F4xx) -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin) -{ - uint32_t function = pinmap_function(pin, PinMap_ADC); - uint32_t channel = 0; - switch (STM_PIN_CHANNEL(function)) { -#ifdef ADC_CHANNEL_0 - case 0: - channel = ADC_CHANNEL_0; - break; -#endif - case 1: - channel = ADC_CHANNEL_1; - break; - case 2: - channel = ADC_CHANNEL_2; - break; - case 3: - channel = ADC_CHANNEL_3; - break; - case 4: - channel = ADC_CHANNEL_4; - break; - case 5: - channel = ADC_CHANNEL_5; - break; - case 6: - channel = ADC_CHANNEL_6; - break; - case 7: - channel = ADC_CHANNEL_7; - break; - case 8: - channel = ADC_CHANNEL_8; - break; - case 9: - channel = ADC_CHANNEL_9; - break; - case 10: - channel = ADC_CHANNEL_10; - break; - case 11: - channel = ADC_CHANNEL_11; - break; - case 12: - channel = ADC_CHANNEL_12; - break; - case 13: - channel = ADC_CHANNEL_13; - break; - case 14: - channel = ADC_CHANNEL_14; - break; - case 15: - channel = ADC_CHANNEL_15; - break; -#ifdef ADC_CHANNEL_16 - case 16: - channel = ADC_CHANNEL_16; - break; -#endif - case 17: - channel = ADC_CHANNEL_17; - break; -#ifdef ADC_CHANNEL_18 - case 18: - channel = ADC_CHANNEL_18; - break; -#endif -#ifdef ADC_CHANNEL_19 - case 19: - channel = ADC_CHANNEL_19; - break; -#endif -#ifdef ADC_CHANNEL_20 - case 20: - channel = ADC_CHANNEL_20; - break; - case 21: - channel = ADC_CHANNEL_21; - break; - case 22: - channel = ADC_CHANNEL_22; - break; - case 23: - channel = ADC_CHANNEL_23; - break; -#ifdef ADC_CHANNEL_24 - case 24: - channel = ADC_CHANNEL_24; - break; - case 25: - channel = ADC_CHANNEL_25; - break; - case 26: - channel = ADC_CHANNEL_26; - break; -#ifdef ADC_CHANNEL_27 - case 27: - channel = ADC_CHANNEL_27; - break; - case 28: - channel = ADC_CHANNEL_28; - break; - case 29: - channel = ADC_CHANNEL_29; - break; - case 30: - channel = ADC_CHANNEL_30; - break; - case 31: - channel = ADC_CHANNEL_31; - break; -#endif -#endif -#endif - default: - _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); - break; - } - return channel; -} - - // 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(TIM_HandleTypeDef* timer){ @@ -169,25 +40,4 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ return _TRGO_NOT_AVAILABLE; } - -int _adcToIndex(ADC_TypeDef *AdcHandle){ - if(AdcHandle == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle == ADC3) return 2; -#endif -#ifdef ADC4 // if ADC4 exists - else if(AdcHandle == ADC4) return 3; -#endif -#ifdef ADC5 // if ADC5 exists - else if(AdcHandle == ADC5) return 4; -#endif - return 0; -} -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - return _adcToIndex(AdcHandle->Instance); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h index ebd9e75e..391b3fb5 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -1,13 +1,17 @@ -#pragma once +#ifndef STM32F7_LOWSIDE_HAL +#define STM32F7_LOWSIDE_HAL #include "Arduino.h" #if defined(STM32F7xx) + #include "stm32f7xx_hal.h" #include "../stm32_mcu.h" -#include "stm32f7_utils.h" +#include "../stm32_adc_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); #endif + +#endif \ No newline at end of file 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 868aafd0..7ff5bede 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -6,8 +6,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32f7_hal.h" -#include "stm32f7_utils.h" #include "Arduino.h" 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 2321c5da..fbd20d40 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -1,145 +1,7 @@ -#include "stm32f7_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32F7xx) -/* Exported Functions */ - - -PinName analog_to_pin(uint32_t pin) { - PinName pin_name = analogInputToPinName(pin); - if (pin_name == NC) { - return (PinName) pin; - } - return pin_name; -} - - -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin) -{ - uint32_t function = pinmap_function(pin, PinMap_ADC); - uint32_t channel = 0; - switch (STM_PIN_CHANNEL(function)) { -#ifdef ADC_CHANNEL_0 - case 0: - channel = ADC_CHANNEL_0; - break; -#endif - case 1: - channel = ADC_CHANNEL_1; - break; - case 2: - channel = ADC_CHANNEL_2; - break; - case 3: - channel = ADC_CHANNEL_3; - break; - case 4: - channel = ADC_CHANNEL_4; - break; - case 5: - channel = ADC_CHANNEL_5; - break; - case 6: - channel = ADC_CHANNEL_6; - break; - case 7: - channel = ADC_CHANNEL_7; - break; - case 8: - channel = ADC_CHANNEL_8; - break; - case 9: - channel = ADC_CHANNEL_9; - break; - case 10: - channel = ADC_CHANNEL_10; - break; - case 11: - channel = ADC_CHANNEL_11; - break; - case 12: - channel = ADC_CHANNEL_12; - break; - case 13: - channel = ADC_CHANNEL_13; - break; - case 14: - channel = ADC_CHANNEL_14; - break; - case 15: - channel = ADC_CHANNEL_15; - break; -#ifdef ADC_CHANNEL_16 - case 16: - channel = ADC_CHANNEL_16; - break; -#endif - case 17: - channel = ADC_CHANNEL_17; - break; -#ifdef ADC_CHANNEL_18 - case 18: - channel = ADC_CHANNEL_18; - break; -#endif -#ifdef ADC_CHANNEL_19 - case 19: - channel = ADC_CHANNEL_19; - break; -#endif -#ifdef ADC_CHANNEL_20 - case 20: - channel = ADC_CHANNEL_20; - break; - case 21: - channel = ADC_CHANNEL_21; - break; - case 22: - channel = ADC_CHANNEL_22; - break; - case 23: - channel = ADC_CHANNEL_23; - break; -#ifdef ADC_CHANNEL_24 - case 24: - channel = ADC_CHANNEL_24; - break; - case 25: - channel = ADC_CHANNEL_25; - break; - case 26: - channel = ADC_CHANNEL_26; - break; -#ifdef ADC_CHANNEL_27 - case 27: - channel = ADC_CHANNEL_27; - break; - case 28: - channel = ADC_CHANNEL_28; - break; - case 29: - channel = ADC_CHANNEL_29; - break; - case 30: - channel = ADC_CHANNEL_30; - break; - case 31: - channel = ADC_CHANNEL_31; - break; -#endif -#endif -#endif - default: - _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); - break; - } - return channel; -} /* TIM1 TIM2 @@ -231,25 +93,4 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ return _TRGO_NOT_AVAILABLE; } - -int _adcToIndex(ADC_TypeDef *AdcHandle){ - if(AdcHandle == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle == ADC3) return 2; -#endif -#ifdef ADC4 // if ADC4 exists - else if(AdcHandle == ADC4) return 3; -#endif -#ifdef ADC5 // if ADC5 exists - else if(AdcHandle == ADC5) return 4; -#endif - return 0; -} -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - return _adcToIndex(AdcHandle->Instance); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h deleted file mode 100644 index 2e125d35..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include "Arduino.h" - -#if defined(STM32F7xx) - -#define _TRGO_NOT_AVAILABLE 12345 - - -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin); - -// timer to injected TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc_ex.h#L178 -uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); - -// timer to regular TRGO -// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F7xx_HAL_Driver/Inc/stm32f7xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif \ No newline at end of file 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 0ac50c80..c437ae51 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -19,7 +19,11 @@ ADC_HandleTypeDef hadc; int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) { ADC_InjectionConfTypeDef sConfigInjected; - + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + // hadc.Instance = _findBestADCForPins(3, cs_params->pins); + // check if all pins belong to the same ADC ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; @@ -35,7 +39,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); @@ -92,7 +96,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); + SIMPLEFOC_DEBUG("STM32-CS: ERR: Cannot find a common ADC for the pins!"); #endif return -1; // error not a valid ADC instance } @@ -101,6 +105,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); #endif + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; hadc.Init.Resolution = ADC_RESOLUTION_12B; hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h index 34fb947e..81faf26b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -7,7 +7,7 @@ #include "stm32g4xx_hal.h" #include "../stm32_mcu.h" -#include "stm32g4_utils.h" +#include "../stm32_adc_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); 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 a4d66b60..67585edc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -7,8 +7,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32g4_hal.h" -#include "stm32g4_utils.h" #include "Arduino.h" // #define SIMPLEFOC_STM32_ADC_INTERRUPT @@ -77,7 +77,7 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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); + HAL_ADCEx_Calibration_Start(cs_params->adc_handle, ADC_SINGLE_ENDED); // start the adc if (use_adc_interrupt){ 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 2f9e7e4d..3622c928 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp @@ -1,136 +1,7 @@ -#include "stm32g4_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin) -{ - uint32_t function = pinmap_function(pin, PinMap_ADC); - uint32_t channel = 0; - switch (STM_PIN_CHANNEL(function)) { -#ifdef ADC_CHANNEL_0 - case 0: - channel = ADC_CHANNEL_0; - break; -#endif - case 1: - channel = ADC_CHANNEL_1; - break; - case 2: - channel = ADC_CHANNEL_2; - break; - case 3: - channel = ADC_CHANNEL_3; - break; - case 4: - channel = ADC_CHANNEL_4; - break; - case 5: - channel = ADC_CHANNEL_5; - break; - case 6: - channel = ADC_CHANNEL_6; - break; - case 7: - channel = ADC_CHANNEL_7; - break; - case 8: - channel = ADC_CHANNEL_8; - break; - case 9: - channel = ADC_CHANNEL_9; - break; - case 10: - channel = ADC_CHANNEL_10; - break; - case 11: - channel = ADC_CHANNEL_11; - break; - case 12: - channel = ADC_CHANNEL_12; - break; - case 13: - channel = ADC_CHANNEL_13; - break; - case 14: - channel = ADC_CHANNEL_14; - break; - case 15: - channel = ADC_CHANNEL_15; - break; -#ifdef ADC_CHANNEL_16 - case 16: - channel = ADC_CHANNEL_16; - break; -#endif - case 17: - channel = ADC_CHANNEL_17; - break; -#ifdef ADC_CHANNEL_18 - case 18: - channel = ADC_CHANNEL_18; - break; -#endif -#ifdef ADC_CHANNEL_19 - case 19: - channel = ADC_CHANNEL_19; - break; -#endif -#ifdef ADC_CHANNEL_20 - case 20: - channel = ADC_CHANNEL_20; - break; - case 21: - channel = ADC_CHANNEL_21; - break; - case 22: - channel = ADC_CHANNEL_22; - break; - case 23: - channel = ADC_CHANNEL_23; - break; -#ifdef ADC_CHANNEL_24 - case 24: - channel = ADC_CHANNEL_24; - break; - case 25: - channel = ADC_CHANNEL_25; - break; - case 26: - channel = ADC_CHANNEL_26; - break; -#ifdef ADC_CHANNEL_27 - case 27: - channel = ADC_CHANNEL_27; - break; - case 28: - channel = ADC_CHANNEL_28; - break; - case 29: - channel = ADC_CHANNEL_29; - break; - case 30: - channel = ADC_CHANNEL_30; - break; - case 31: - channel = ADC_CHANNEL_31; - break; -#endif -#endif -#endif - default: - _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); - break; - } - return channel; -} - - // 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(TIM_HandleTypeDef* timer){ @@ -213,25 +84,4 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ return _TRGO_NOT_AVAILABLE; } - -int _adcToIndex(ADC_TypeDef *AdcHandle){ - if(AdcHandle == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle == ADC3) return 2; -#endif -#ifdef ADC4 // if ADC4 exists - else if(AdcHandle == ADC4) return 3; -#endif -#ifdef ADC5 // if ADC5 exists - else if(AdcHandle == ADC5) return 4; -#endif - return 0; -} -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - return _adcToIndex(AdcHandle->Instance); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h deleted file mode 100644 index 5a3aca29..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h +++ /dev/null @@ -1,34 +0,0 @@ - -#ifndef STM32G4_UTILS_HAL -#define STM32G4_UTILS_HAL - -#include "Arduino.h" - -#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) - -#define _TRGO_NOT_AVAILABLE 12345 - - -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -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(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(TIM_HandleTypeDef* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h index 2004ff69..fa49d593 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -7,7 +7,7 @@ #include "stm32l4xx_hal.h" #include "../stm32_mcu.h" -#include "stm32l4_utils.h" +#include "../stm32_adc_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); 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 4ddc5484..b2b9a264 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -7,8 +7,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32l4_hal.h" -#include "stm32l4_utils.h" #include "Arduino.h" 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 f93e63a4..64229b8a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp @@ -1,135 +1,7 @@ -#include "stm32l4_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32L4xx) -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin) -{ - uint32_t function = pinmap_function(pin, PinMap_ADC); - uint32_t channel = 0; - switch (STM_PIN_CHANNEL(function)) { -#ifdef ADC_CHANNEL_0 - case 0: - channel = ADC_CHANNEL_0; - break; -#endif - case 1: - channel = ADC_CHANNEL_1; - break; - case 2: - channel = ADC_CHANNEL_2; - break; - case 3: - channel = ADC_CHANNEL_3; - break; - case 4: - channel = ADC_CHANNEL_4; - break; - case 5: - channel = ADC_CHANNEL_5; - break; - case 6: - channel = ADC_CHANNEL_6; - break; - case 7: - channel = ADC_CHANNEL_7; - break; - case 8: - channel = ADC_CHANNEL_8; - break; - case 9: - channel = ADC_CHANNEL_9; - break; - case 10: - channel = ADC_CHANNEL_10; - break; - case 11: - channel = ADC_CHANNEL_11; - break; - case 12: - channel = ADC_CHANNEL_12; - break; - case 13: - channel = ADC_CHANNEL_13; - break; - case 14: - channel = ADC_CHANNEL_14; - break; - case 15: - channel = ADC_CHANNEL_15; - break; -#ifdef ADC_CHANNEL_16 - case 16: - channel = ADC_CHANNEL_16; - break; -#endif - case 17: - channel = ADC_CHANNEL_17; - break; -#ifdef ADC_CHANNEL_18 - case 18: - channel = ADC_CHANNEL_18; - break; -#endif -#ifdef ADC_CHANNEL_19 - case 19: - channel = ADC_CHANNEL_19; - break; -#endif -#ifdef ADC_CHANNEL_20 - case 20: - channel = ADC_CHANNEL_20; - break; - case 21: - channel = ADC_CHANNEL_21; - break; - case 22: - channel = ADC_CHANNEL_22; - break; - case 23: - channel = ADC_CHANNEL_23; - break; -#ifdef ADC_CHANNEL_24 - case 24: - channel = ADC_CHANNEL_24; - break; - case 25: - channel = ADC_CHANNEL_25; - break; - case 26: - channel = ADC_CHANNEL_26; - break; -#ifdef ADC_CHANNEL_27 - case 27: - channel = ADC_CHANNEL_27; - break; - case 28: - channel = ADC_CHANNEL_28; - break; - case 29: - channel = ADC_CHANNEL_29; - break; - case 30: - channel = ADC_CHANNEL_30; - break; - case 31: - channel = ADC_CHANNEL_31; - break; -#endif -#endif -#endif - default: - _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); - break; - } - return channel; -} - // 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 @@ -197,25 +69,4 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ return _TRGO_NOT_AVAILABLE; } - -int _adcToIndex(ADC_TypeDef *AdcHandle){ - if(AdcHandle == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle == ADC3) return 2; -#endif -#ifdef ADC4 // if ADC4 exists - else if(AdcHandle == ADC4) return 3; -#endif -#ifdef ADC5 // if ADC5 exists - else if(AdcHandle == ADC5) return 4; -#endif - return 0; -} -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - return _adcToIndex(AdcHandle->Instance); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h deleted file mode 100644 index e30bf0ce..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h +++ /dev/null @@ -1,34 +0,0 @@ - -#ifndef STM32L4_UTILS_HAL -#define STM32L4_UTILS_HAL - -#include "Arduino.h" - -#if defined(STM32L4xx) - -#define _TRGO_NOT_AVAILABLE 12345 - - -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -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(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(TIM_HandleTypeDef* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif - -#endif \ No newline at end of file From 3e1870bbf403a9fed3b8f5a3df0fd82a0e609a9b Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 10:43:23 +0100 Subject: [PATCH 46/53] rank calculation problem --- .../hardware_specific/stm32/stm32f1/stm32f1_hal.cpp | 3 ++- .../hardware_specific/stm32/stm32f4/stm32f4_hal.cpp | 5 +++-- .../hardware_specific/stm32/stm32f7/stm32f7_hal.cpp | 3 ++- .../hardware_specific/stm32/stm32g4/stm32g4_hal.cpp | 5 +++-- .../hardware_specific/stm32/stm32l4/stm32l4_hal.cpp | 3 ++- 5 files changed, 12 insertions(+), 7 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 ac122992..d9e88ca0 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -123,11 +123,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } + uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ranks[i]; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC)); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 4c465425..1c85d1fa 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -118,12 +118,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif } - + + uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ranks[i]; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 96b09d00..5f905633 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -127,11 +127,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } + uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ranks[i]; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 0ac50c80..497ce070 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -168,12 +168,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance)); #endif } - + + uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ranks[i]; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG 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 bc74f095..7d8923fd 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -168,11 +168,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } + uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i; + sConfigInjected.InjectedRank = ranks[i]; sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG From 49e34e8e5a68a4c433fde1ec144206c76541be47 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 11:53:03 +0100 Subject: [PATCH 47/53] automatic adc selsction --- .../stm32/stm32_adc_utils.cpp | 320 +++++++++++------- .../hardware_specific/stm32/stm32_adc_utils.h | 7 +- .../stm32/stm32f4/stm32f4_hal.cpp | 32 +- .../stm32/stm32f4/stm32f4_mcu.cpp | 6 +- .../stm32/stm32f7/stm32f7_hal.cpp | 32 +- .../stm32/stm32f7/stm32f7_mcu.cpp | 6 +- .../stm32/stm32g4/stm32g4_hal.cpp | 36 +- .../stm32/stm32g4/stm32g4_mcu.cpp | 7 +- .../stm32/stm32l4/stm32l4_hal.cpp | 26 +- .../stm32/stm32l4/stm32l4_mcu.cpp | 6 +- 10 files changed, 262 insertions(+), 216 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp index fac4a377..95eb3e8f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -7,12 +7,173 @@ // here I've assumed the maximum number of ADCs is 5 #define ADC_COUNT 5 + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + + +ADC_TypeDef* _indexToADC(uint8_t index){ + switch (index) { + case 0: + return ADC1; + break; +#ifdef ADC2 // if ADC2 exists + case 1: + return ADC2; + break; +#endif +#ifdef ADC3 // if ADC3 exists + case 2: + return ADC3; + break; +#endif +#ifdef ADC4 // if ADC4 exists + case 3: + return ADC4; + break; +#endif +#ifdef ADC5 // if ADC5 exists + case 4: + return ADC5; + break; +#endif + } + return nullptr; +} + +int _findIndexOfEntry(PinName pin) { + // remove the ALT if it is there + PinName pinName = (PinName)(pinName & ~ALTX_MASK); + int i = 0; + SIMPLEFOC_DEBUG("STM32-CS: Looking for pin "); + while (PinMap_ADC[i].pin !=NC) { + if (pinName == PinMap_ADC[i].pin ) + return i; + i++; + SIMPLEFOC_DEBUG("STM32-CS: Looking for pin ", i); + } + return -1; +} + +int _findIndexOfLastEntry(PinName pin) { + // remove the ALT if it is there + PinName pinName = (PinName)(pin & ~ALTX_MASK); + + int i = 0; + while (PinMap_ADC[i].pin!=NC) { + if ( pinName == (PinMap_ADC[i].pin & ~ALTX_MASK) + && pinName != (PinMap_ADC[i+1].pin & ~ALTX_MASK)) + return i; + i++; + } + return -1; +} +int _findIndexOfFirstEntry(PinName pin) { + // remove the ALT if it is there + PinName pinName = (PinName)(pin & ~ALTX_MASK); + int i = 0; + while (PinMap_ADC[i].pin !=NC) { + if (pinName == PinMap_ADC[i].pin ) + return i; + i++; + } + return -1; +} + +// functions finding the index of the first pin entry in the PinMap_ADC +// returns -1 if not found +int _findIndexOfFirstPinMapADCEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + // remove the ALT if it is there + return _findIndexOfFirstEntry(pinName); +} + +// functions finding the index of the last pin entry in the PinMap_ADC +// returns -1 if not found +int _findIndexOfLastPinMapADCEntry(int pin) { + PinName pinName = digitalPinToPinName(pin); + // remove the ALT if it is there + return _findIndexOfLastEntry(pinName); +} + +// find the best ADC combination for the given pins +// returns the index of the best ADC +// each pin can be connected to multiple ADCs +// the function will try to find a single ADC that can be used for all pins +// if not possible it will return nullptr +ADC_TypeDef* _findBestADCForPins(int numPins, int pins[]) { + + // assuning that there is less than 8 ADCs + uint8_t pins_at_adc[ADC_COUNT] = {0}; + + // check how many pins are there and are not set + int no_pins = 0; + for (int i = 0; i < numPins; i++) { + if(_isset(pins[i])) no_pins++; + } + + // loop over all elements and count the pins connected to each ADC + for (int i = 0; i < numPins; i++) { + int pin = pins[i]; + if(!_isset(pin)) continue; + + int index = _findIndexOfFirstPinMapADCEntry(pin); + int last_index = _findIndexOfLastPinMapADCEntry(pin); + if (index == -1) { + return nullptr; + } + for (int j = index; j <= last_index; j++) { + if (PinMap_ADC[j].pin == NC) { + break; + } + int adcIndex = _adcToIndex((ADC_TypeDef*)PinMap_ADC[j].peripheral); + pins_at_adc[adcIndex]++; + } + } + + for (int i = 0; i < ADC_COUNT; i++) { + if(!pins_at_adc[i]) continue; + SimpleFOCDebug::print("STM32-CS: ADC"); + SimpleFOCDebug::print(i+1); + SimpleFOCDebug::print(" pins: "); + SimpleFOCDebug::println(pins_at_adc[i]); + } + + // now take the first ADC that has all pins connected + for (int i = 0; i < ADC_COUNT; i++) { + if (pins_at_adc[i] == no_pins) { + return _indexToADC(i); + } + } + return nullptr; +} + + + /** * @brief Return ADC HAL channel linked to a PinName * @param pin: PinName * @retval Valid HAL channel */ -uint32_t _getADCChannel(PinName pin) +uint32_t _getADCChannelFromPinMap(PinName pin) { uint32_t function = pinmap_function(pin, PinMap_ADC); uint32_t channel = 0; @@ -134,135 +295,60 @@ uint32_t _getADCChannel(PinName pin) return channel; } +/** + * @brief Return ADC HAL channel linked to a PinName and the ADC handle + * @param pin: PinName + * @param AdcHandle: ADC_HandleTypeDef a pointer to the ADC handle + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin, ADC_TypeDef *AdcHandle ) +{ + if (AdcHandle == NP) { + return _getADCChannelFromPinMap(pin); + } + // find the PinName that corresponds to the ADC + int first_ind = _findIndexOfFirstEntry(pin); + int last_ind = _findIndexOfLastEntry(pin); + if (first_ind == -1 || last_ind == -1) { + _Error_Handler("ADC: Pin not found in PinMap_ADC", (int)pin); + } + // find the channel + uint32_t channel = 0; + for (int i = first_ind; i <= last_ind; i++) { + if (PinMap_ADC[i].peripheral == AdcHandle) { + channel =_getADCChannelFromPinMap(PinMap_ADC[i].pin); -int _adcToIndex(ADC_TypeDef *AdcHandle){ - if(AdcHandle == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle == ADC3) return 2; -#endif -#ifdef ADC4 // if ADC4 exists - else if(AdcHandle == ADC4) return 3; -#endif -#ifdef ADC5 // if ADC5 exists - else if(AdcHandle == ADC5) return 4; -#endif - return 0; -} -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - return _adcToIndex(AdcHandle->Instance); -} +// #ifdef SIMPLEFOC_STM32_DEBUG + SimpleFOCDebug::print("STM32-CS: Configuring ADC"); + SimpleFOCDebug::print(_adcToIndex(AdcHandle)+1); + SimpleFOCDebug::print(" channel: "); + SimpleFOCDebug::println((int)STM_PIN_CHANNEL(pinmap_function(PinMap_ADC[i].pin, PinMap_ADC))); +// #endif + break; + } + } + return channel; +} -ADC_TypeDef* _indexToADC(uint8_t index){ - switch (index) { +uint32_t _getADCInjectedRank(uint8_t ind){ + switch (ind) { case 0: - return ADC1; + return ADC_INJECTED_RANK_1; break; -#ifdef ADC2 // if ADC2 exists case 1: - return ADC2; + return ADC_INJECTED_RANK_2; break; -#endif -#ifdef ADC3 // if ADC3 exists case 2: - return ADC3; + return ADC_INJECTED_RANK_3; break; -#endif -#ifdef ADC4 // if ADC4 exists case 3: - return ADC4; + return ADC_INJECTED_RANK_4; break; -#endif -#ifdef ADC5 // if ADC5 exists - case 4: - return ADC5; + default: + return 0; break; -#endif - } - return nullptr; -} - - -// functions finding the index of the first pin entry in the PinMap_ADC -// returns -1 if not found -int _findIndexOfFirstPinMapADCEntry(int pin) { - PinName pinName = digitalPinToPinName(pin); - int i = 0; - while ((PinMap_ADC[i].pin & ~ALTX_MASK) !=NC) { - if (pinName == (PinMap_ADC[i].pin & ~ALTX_MASK)) - return i; - i++; } - return -1; -} - -// functions finding the index of the last pin entry in the PinMap_ADC -// returns -1 if not found -int _findIndexOfLastPinMapADCEntry(int pin) { - PinName pinName = digitalPinToPinName(pin); - int i = 0; - while (PinMap_ADC[i].pin!=NC) { - if ( pinName == (PinMap_ADC[i].pin & ~ALTX_MASK) - && pinName != (PinMap_ADC[i+1].pin & ~ALTX_MASK)) - return i; - i++; - } - return -1; -} - -// find the best ADC combination for the given pins -// returns the index of the best ADC -// each pin can be connected to multiple ADCs -// the function will try to find a single ADC that can be used for all pins -// if not possible it will return nullptr -ADC_TypeDef* _findBestADCForPins(int numPins, int pins[]) { - - // assuning that there is less than 8 ADCs - uint8_t pins_at_adc[ADC_COUNT] = {0}; - - // check how many pins are there and are not set - int no_pins = 0; - for (int i = 0; i < numPins; i++) { - if(_isset(pins[i])) no_pins++; - } - - // loop over all elements and count the pins connected to each ADC - for (int i = 0; i < numPins; i++) { - int pin = pins[i]; - if(!_isset(pin)) continue; - - int index = _findIndexOfFirstPinMapADCEntry(pin); - int last_index = _findIndexOfLastPinMapADCEntry(pin); - if (index == -1) { - return nullptr; - } - for (int j = index; j <= last_index; j++) { - if (PinMap_ADC[j].pin == NC) { - break; - } - int adcIndex = _adcToIndex((ADC_TypeDef*)PinMap_ADC[j].peripheral); - pins_at_adc[adcIndex]++; - } - } - - for (int i = 0; i < ADC_COUNT; i++) { - if(!pins_at_adc[i]) continue; - SimpleFOCDebug::print("STM32-CS: ADC"); - SimpleFOCDebug::print(i+1); - SimpleFOCDebug::print(" pins: "); - SimpleFOCDebug::println(pins_at_adc[i]); - } - - // now take the first ADC that has all pins connected - for (int i = 0; i < ADC_COUNT; i++) { - if (pins_at_adc[i] == no_pins) { - return _indexToADC(i); - } - } - return nullptr; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h index 0dd2a2a2..7e74a96f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.h @@ -14,9 +14,11 @@ /** * @brief Return ADC HAL channel linked to a PinName * @param pin: PinName + * @param adc: ADC_TypeDef a pointer to the ADC handle * @retval Valid HAL channel */ -uint32_t _getADCChannel(PinName pin); +uint32_t _getADCChannel(PinName pin, ADC_TypeDef* adc = NP); +uint32_t _getADCInjectedRank(uint8_t ind); // timer to injected TRGO - architecure specific uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer); @@ -28,9 +30,6 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer); int _adcToIndex(ADC_HandleTypeDef *AdcHandle); int _adcToIndex(ADC_TypeDef *AdcHandle); - - - // functions helping to find the best ADC channel int _findIndexOfFirstPinMapADCEntry(int pin); int _findIndexOfLastPinMapADCEntry(int pin); 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 1c85d1fa..e6ee1cbc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -21,24 +21,9 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive { ADC_InjectionConfTypeDef sConfigInjected; - // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || - ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || - ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) - ){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); -#endif - return -1; - } - - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ - hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); #ifdef ADC2 // if defined ADC2 @@ -78,7 +63,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time */ - sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; sConfigInjected.AutoInjectedConv = DISABLE; @@ -119,16 +109,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; + uint8_t channel_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ranks[i]; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); #endif return -1; } 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 ed58e8cb..be49adfe 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -96,16 +96,18 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } 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 5f905633..ec5ccb64 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -21,24 +21,9 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive { ADC_InjectionConfTypeDef sConfigInjected; - // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || - ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || - ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) - ){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); -#endif - return -1; - } - - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ - hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); #ifdef ADC2 // if defined ADC2 @@ -78,7 +63,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time */ - sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING; sConfigInjected.AutoInjectedConv = DISABLE; @@ -127,16 +117,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; + uint8_t channel_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ranks[i]; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); #endif return -1; } 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 7ff5bede..664262dc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -79,16 +79,18 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } 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 a4c72680..df639932 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -22,26 +22,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ - // hadc.Instance = _findBestADCForPins(3, cs_params->pins); - - // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || - ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || - ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) - ){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); -#endif - return -1; - } - - - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) - */ - hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) { #ifdef __HAL_RCC_ADC1_CLK_ENABLE @@ -129,7 +110,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time */ - sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; sConfigInjected.AutoInjectedConv = DISABLE; @@ -174,16 +160,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; + uint8_t channel_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ranks[i]; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); #endif return -1; } 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 67585edc..5db5cea7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -133,16 +133,18 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } @@ -161,6 +163,7 @@ extern "C" { adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + adc_val[adc_index][3]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_4); } } 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 7d8923fd..3d1352b9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -20,23 +20,9 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive { ADC_InjectionConfTypeDef sConfigInjected; - // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || - ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || - ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) - ){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); -#endif - return -1; - } - /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ - hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) { #ifdef __HAL_RCC_ADC1_CLK_ENABLE @@ -91,7 +77,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); + SIMPLEFOC_DEBUG("STM32-CS: ERR: Cannot find a common ADC for the pins!"); #endif return -1; // error not a valid ADC instance } @@ -168,16 +154,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; + uint8_t channel_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ranks[i]; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance)); #endif return -1; } 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 b2b9a264..4a6e529c 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -129,16 +129,18 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } From 9bb6cfb19827895c7bd1d6dc1bf4ba151f9b3e17 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 11:57:31 +0100 Subject: [PATCH 48/53] remove some debugging --- .../hardware_specific/stm32/stm32_adc_utils.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp index 95eb3e8f..19e31cc1 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -317,14 +317,7 @@ uint32_t _getADCChannel(PinName pin, ADC_TypeDef *AdcHandle ) for (int i = first_ind; i <= last_ind; i++) { if (PinMap_ADC[i].peripheral == AdcHandle) { channel =_getADCChannelFromPinMap(PinMap_ADC[i].pin); - -// #ifdef SIMPLEFOC_STM32_DEBUG - SimpleFOCDebug::print("STM32-CS: Configuring ADC"); - SimpleFOCDebug::print(_adcToIndex(AdcHandle)+1); - SimpleFOCDebug::print(" channel: "); - SimpleFOCDebug::println((int)STM_PIN_CHANNEL(pinmap_function(PinMap_ADC[i].pin, PinMap_ADC))); -// #endif - + SIMPLEFOC_DEBUG("STM32-CS: ADC channel: ", (int)STM_PIN_CHANNEL(pinmap_function(PinMap_ADC[i].pin, PinMap_ADC))); break; } } From 2857d3ea67e36323694d801b78490df9984bdc29 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 12:11:41 +0100 Subject: [PATCH 49/53] h7 update for auto-search --- .../stm32/stm32h7/stm32h7_hal.cpp | 37 ++-- .../stm32/stm32h7/stm32h7_hal.h | 2 +- .../stm32/stm32h7/stm32h7_mcu.cpp | 8 +- .../stm32/stm32h7/stm32h7_utils.cpp | 159 +----------------- .../stm32/stm32h7/stm32h7_utils.h | 30 ---- 5 files changed, 21 insertions(+), 215 deletions(-) delete mode 100644 src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp index b572a5aa..33d20c67 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp @@ -5,7 +5,6 @@ //#define SIMPLEFOC_STM32_DEBUG #include "../../../../communication/SimpleFOCDebug.h" -#define _TRGO_NOT_AVAILABLE 12345 ADC_HandleTypeDef hadc; @@ -21,23 +20,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive { ADC_InjectionConfTypeDef sConfigInjected; - // check if all pins belong to the same ADC - ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr; - ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; - if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) || - ((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) || - ((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3)) - ){ -#ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); -#endif - return -1; - } /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ - hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); if(hadc.Instance == ADC1) __HAL_RCC_ADC12_CLK_ENABLE(); #ifdef ADC2 // if defined ADC2 @@ -48,7 +34,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif else{ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); + SIMPLEFOC_DEBUG("STM32-CS: ERR: Cannot find a common ADC for the pins!"); #endif return -1; // error not a valid ADC instance } @@ -91,7 +77,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time */ - sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } // if ADC1 or ADC2 if(hadc.Instance == ADC1 || hadc.Instance == ADC2){ // more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658 @@ -147,21 +138,21 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif } - uint32_t ranks[4] = {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; + uint8_t channel_no = 0; for(int i=0; i<3; i++){ // skip if not set - if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ranks[i]; - sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i])); + if (!_isset(cs_params->pins[i])) continue; + + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); #endif return -1; } } - delay(1000); #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT // enable interrupt diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h index 623f2b85..70a4b762 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h @@ -4,8 +4,8 @@ #if defined(STM32H7xx) #include "stm32h7xx_hal.h" +#include "../stm32_adc_utils.h" #include "../stm32_mcu.h" -#include "stm32h7_utils.h" int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp index fe7f16ed..94869c54 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp @@ -6,8 +6,8 @@ #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" #include "../stm32_mcu.h" +#include "../stm32_adc_utils.h" #include "stm32h7_hal.h" -#include "stm32h7_utils.h" #include "Arduino.h" @@ -83,16 +83,18 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp index 41db1d9a..9b16263f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.cpp @@ -1,146 +1,10 @@ -#include "stm32h7_utils.h" +#include "../stm32_adc_utils.h" #if defined(STM32H7xx) /* Exported Functions */ -PinName analog_to_pin(uint32_t pin) { - PinName pin_name = analogInputToPinName(pin); - if (pin_name == NC) { - return (PinName) pin; - } - return pin_name; -} - - -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -uint32_t _getADCChannel(PinName pin) -{ - uint32_t function = pinmap_function(pin, PinMap_ADC); - uint32_t channel = 0; - switch (STM_PIN_CHANNEL(function)) { -#ifdef ADC_CHANNEL_0 - case 0: - channel = ADC_CHANNEL_0; - break; -#endif - case 1: - channel = ADC_CHANNEL_1; - break; - case 2: - channel = ADC_CHANNEL_2; - break; - case 3: - channel = ADC_CHANNEL_3; - break; - case 4: - channel = ADC_CHANNEL_4; - break; - case 5: - channel = ADC_CHANNEL_5; - break; - case 6: - channel = ADC_CHANNEL_6; - break; - case 7: - channel = ADC_CHANNEL_7; - break; - case 8: - channel = ADC_CHANNEL_8; - break; - case 9: - channel = ADC_CHANNEL_9; - break; - case 10: - channel = ADC_CHANNEL_10; - break; - case 11: - channel = ADC_CHANNEL_11; - break; - case 12: - channel = ADC_CHANNEL_12; - break; - case 13: - channel = ADC_CHANNEL_13; - break; - case 14: - channel = ADC_CHANNEL_14; - break; - case 15: - channel = ADC_CHANNEL_15; - break; -#ifdef ADC_CHANNEL_16 - case 16: - channel = ADC_CHANNEL_16; - break; -#endif - case 17: - channel = ADC_CHANNEL_17; - break; -#ifdef ADC_CHANNEL_18 - case 18: - channel = ADC_CHANNEL_18; - break; -#endif -#ifdef ADC_CHANNEL_19 - case 19: - channel = ADC_CHANNEL_19; - break; -#endif -#ifdef ADC_CHANNEL_20 - case 20: - channel = ADC_CHANNEL_20; - break; - case 21: - channel = ADC_CHANNEL_21; - break; - case 22: - channel = ADC_CHANNEL_22; - break; - case 23: - channel = ADC_CHANNEL_23; - break; -#ifdef ADC_CHANNEL_24 - case 24: - channel = ADC_CHANNEL_24; - break; - case 25: - channel = ADC_CHANNEL_25; - break; - case 26: - channel = ADC_CHANNEL_26; - break; -#ifdef ADC_CHANNEL_27 - case 27: - channel = ADC_CHANNEL_27; - break; - case 28: - channel = ADC_CHANNEL_28; - break; - case 29: - channel = ADC_CHANNEL_29; - break; - case 30: - channel = ADC_CHANNEL_30; - break; - case 31: - channel = ADC_CHANNEL_31; - break; -#endif -#endif -#endif - default: - _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); - break; - } - return channel; -} - // timer to injected TRGO // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc_ex.h#L235 uint32_t _timerToInjectedTRGO(TIM_HandleTypeDef* timer){ @@ -216,25 +80,4 @@ uint32_t _timerToRegularTRGO(TIM_HandleTypeDef* timer){ return _TRGO_NOT_AVAILABLE; } - -int _adcToIndex(ADC_TypeDef *AdcHandle){ - if(AdcHandle == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle == ADC3) return 2; -#endif -#ifdef ADC4 // if ADC4 exists - else if(AdcHandle == ADC4) return 3; -#endif -#ifdef ADC5 // if ADC5 exists - else if(AdcHandle == ADC5) return 4; -#endif - return 0; -} -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - return _adcToIndex(AdcHandle->Instance); -} - #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h deleted file mode 100644 index 22dfbed1..00000000 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_utils.h +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include "Arduino.h" - -#if defined(STM32H7xx) - -#define _TRGO_NOT_AVAILABLE 12345 - - -/* Exported Functions */ -/** - * @brief Return ADC HAL channel linked to a PinName - * @param pin: PinName - * @retval Valid HAL channel - */ -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(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(TIM_HandleTypeDef* timer); - -// function returning index of the ADC instance -int _adcToIndex(ADC_HandleTypeDef *AdcHandle); -int _adcToIndex(ADC_TypeDef *AdcHandle); - -#endif \ No newline at end of file From d8fac13074314e063ec4528a84cba2e0a040571c Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 12:18:27 +0100 Subject: [PATCH 50/53] f1 update --- .../stm32/stm32_adc_utils.cpp | 2 +- .../stm32/stm32f1/stm32f1_hal.cpp | 35 +--------------- .../stm32/stm32f1/stm32f1_utils.cpp | 40 +++++++++++++++++++ 3 files changed, 42 insertions(+), 35 deletions(-) create mode 100644 src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp diff --git a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp index 19e31cc1..33774a55 100644 --- a/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32_adc_utils.cpp @@ -1,6 +1,6 @@ #include "stm32_adc_utils.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) // for searching the best ADCs, we need to know the number of ADCs // it might be better to use some HAL variable for example ADC_COUNT 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 d9e88ca0..5cd94a6a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -3,42 +3,9 @@ #if defined(STM32F1xx) #include "../../../../communication/SimpleFOCDebug.h" +#include "../stm32_adc_utils.h" #define _TRGO_NOT_AVAILABLE 12345 -// 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(TIM_HandleTypeDef* timer){ - if(timer->Instance == TIM1) - return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; -#ifdef TIM2 // if defined timer 2 - else if(timer->Instance == TIM2) - return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; -#endif -#ifdef TIM4 // if defined timer 4 - else if(timer->Instance == TIM4) - return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; -#endif -#ifdef TIM5 // if defined timer 5 - else if(timer->Instance == TIM5) - return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; -#endif - else - return _TRGO_NOT_AVAILABLE; -} - -// 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(TIM_HandleTypeDef* timer){ - if(timer->Instance == TIM3) - return ADC_EXTERNALTRIGCONV_T3_TRGO; -#ifdef TIM8 // if defined timer 8 - else if(timer->Instance == TIM8) - return ADC_EXTERNALTRIGCONV_T8_TRGO; -#endif - else - return _TRGO_NOT_AVAILABLE; -} - ADC_HandleTypeDef hadc; /** diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp new file mode 100644 index 00000000..8cd6eb96 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_utils.cpp @@ -0,0 +1,40 @@ +#include "../stm32_adc_utils.h" + +#if defined(STM32F1xx) + + +// 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(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->Instance == TIM5) + return ADC_EXTERNALTRIGINJECCONV_T5_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// 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(TIM_HandleTypeDef* timer){ + if(timer->Instance == TIM3) + return ADC_EXTERNALTRIGCONV_T3_TRGO; +#ifdef TIM8 // if defined timer 8 + else if(timer->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +#endif \ No newline at end of file From b9b9cf3758fd238c22c9524e0ace3af206505f53 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 21 Mar 2025 12:21:01 +0100 Subject: [PATCH 51/53] restructuring for f1 --- .../stm32/stm32f1/stm32f1_hal.cpp | 18 ++++++++++++------ .../stm32/stm32f1/stm32f1_mcu.cpp | 18 +++++------------- 2 files changed, 17 insertions(+), 19 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 5cd94a6a..5378fb1a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -22,7 +22,8 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) */ - hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + hadc.Instance = _findBestADCForPins(3, cs_params->pins); + if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); #ifdef ADC2 // if defined ADC2 else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE(); @@ -49,7 +50,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time */ - sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedNbrOfConversion = 0; + for(int pin_no=0; pin_no<3; pin_no++){ + if(_isset(cs_params->pins[pin_no])){ + sConfigInjected.InjectedNbrOfConversion++; + } + } sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; sConfigInjected.AutoInjectedConv = DISABLE; sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; @@ -90,16 +96,16 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - uint32_t ranks[4]= {ADC_INJECTED_RANK_1, ADC_INJECTED_RANK_2, ADC_INJECTED_RANK_3, ADC_INJECTED_RANK_4}; + uint8_t channel_no = 0; for(int i=0; i<3; i++){ // skip if not set if (!_isset(cs_params->pins[i])) continue; - sConfigInjected.InjectedRank = ranks[i]; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC)); + sConfigInjected.InjectedRank = _getADCInjectedRank(channel_no++); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]), hadc.Instance); if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[i]), PinMap_ADC))); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i]) , hadc.Instance)); #endif return -1; } 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 b52b48bd..6d69dc89 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -5,6 +5,7 @@ #include "../../../../drivers/hardware_api.h" #include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" #include "../../../hardware_api.h" +#include "../stm32_adc_utils.h" #include "../stm32_mcu.h" #include "stm32f1_hal.h" #include "Arduino.h" @@ -25,17 +26,6 @@ uint8_t use_adc_interrupt = 1; uint8_t use_adc_interrupt = 0; #endif -int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ - if(AdcHandle->Instance == ADC1) return 0; -#ifdef ADC2 // if ADC2 exists - else if(AdcHandle->Instance == ADC2) return 1; -#endif -#ifdef ADC3 // if ADC3 exists - else if(AdcHandle->Instance == ADC3) return 2; -#endif - return 0; -} - void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { @@ -107,16 +97,18 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer if (use_adc_interrupt){ - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 - uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; + uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } } + if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } From 302719c14102e286158472d4de04b98f3efabc97 Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 29 Apr 2025 17:12:23 +0200 Subject: [PATCH 52/53] tested h7 low-side, added repetition counterhandling --- .../stm32/stm32f4/stm32f4_utils.cpp | 9 +-- .../stm32/stm32h7/stm32h7_hal.cpp | 5 +- .../stm32/stm32h7/stm32h7_mcu.cpp | 61 +++++++++++++++---- 3 files changed, 55 insertions(+), 20 deletions(-) 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 23d4b0d3..22a54f85 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -26,18 +26,19 @@ 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(TIM_HandleTypeDef* timer){ +#ifdef TIM2 // if defined timer 2 if(timer->Instance == TIM2) return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif #ifdef TIM3 // if defined timer 3 - else if(timer->Instance == TIM3) + if(timer->Instance == TIM3) return ADC_EXTERNALTRIGCONV_T3_TRGO; #endif #ifdef TIM8 // if defined timer 8 - else if(timer->Instance == TIM8) + if(timer->Instance == TIM8) return ADC_EXTERNALTRIGCONV_T8_TRGO; #endif - else - return _TRGO_NOT_AVAILABLE; + return _TRGO_NOT_AVAILABLE; } #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp index 33d20c67..61ae2de3 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp @@ -86,7 +86,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // if ADC1 or ADC2 if(hadc.Instance == ADC1 || hadc.Instance == ADC2){ // more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658 - sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLE_5; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5; }else { // adc3 // https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L673 @@ -196,13 +196,12 @@ int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + extern "C" { void ADC_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp index 94869c54..f826e69d 100644 --- a/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp @@ -22,6 +22,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {1}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { @@ -56,19 +62,47 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } + // set the trigger output event 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_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED); + if(HAL_ADCEx_Calibration_Start(cs_params->adc_handle, ADC_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot calibrate ADC!"); + #endif + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + if(use_adc_interrupt){ + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + + if(HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot start injected channels in interrupt mode!"); + #endif + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + }else{ + if(HAL_ADCEx_InjectedStart(cs_params->adc_handle) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot start injected channels!"); + #endif + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + } // restart all the timers of the driver @@ -83,23 +117,25 @@ void* _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ + // print all values in the buffer + // SIMPLEFOC_DEBUG("adc_a:", (int)HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, _getADCInjectedRank(0))); + // SIMPLEFOC_DEBUG("adc_b:", (int)HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, _getADCInjectedRank(1))); uint8_t channel_no = 0; for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][channel_no] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = _getADCInjectedRank(channel_no); return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } if(_isset(((Stm32CurrentSenseParams*)cs_params)->pins[i])) channel_no++; } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ @@ -107,8 +143,8 @@ extern "C" { int adc_index = _adcToIndex(AdcHandle); // if the timer han't repetition counter - downsample two times - if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { - tim_downsample[adc_index] = 0; + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 1) { + tim_downsample[adc_index] = 1; return; } @@ -117,6 +153,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file From 5c5e30961163e1252fb0a1b74088fbf85cb0a945 Mon Sep 17 00:00:00 2001 From: jeroen Date: Wed, 7 May 2025 11:05:18 +0200 Subject: [PATCH 53/53] add support for timer 8 --- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index 6afbf345..ecdf8a18 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -93,7 +93,7 @@ void syncSlices() { pwm_set_counter(i, 0); } // enable all slices - pwm_set_mask_enabled(0xFF); + pwm_set_mask_enabled(0xFFF); }