From 40e1d21e5e7221a280d94a75fb3ff6a9187b8a49 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 4 May 2022 21:13:44 +0200 Subject: [PATCH 01/42] change StepDirListener to use RISING interrupts --- src/communication/StepDirListener.cpp | 14 +++++++------- src/communication/StepDirListener.h | 4 +++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp index c8f19b47..ee4f69e9 100644 --- a/src/communication/StepDirListener.cpp +++ b/src/communication/StepDirListener.cpp @@ -13,7 +13,7 @@ void StepDirListener::init(){ } void StepDirListener::enableInterrupt(void (*doA)()){ - attachInterrupt(digitalPinToInterrupt(pin_step), doA, CHANGE); + attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity); } void StepDirListener::attach(float* variable){ @@ -22,15 +22,15 @@ void StepDirListener::attach(float* variable){ void StepDirListener::handle(){ // read step status - bool step = digitalRead(pin_step); + //bool step = digitalRead(pin_step); // update counter only on rising edge - if(step && step != step_active){ - if(digitalRead(pin_dir)) + //if(step && step != step_active){ + if(digitalRead(pin_dir)) count++; - else + else count--; - } - step_active = step; + //} + //step_active = step; // if attached variable update it if(attached_variable) *attached_variable = getValue(); } diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 1c50c700..855a701d 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -47,11 +47,13 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing + PinStatus polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable float counter_to_value; //!< step counter to value - bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + }; #endif \ No newline at end of file From 9270663b2a4ab2337be3098fe7fcd92ece400184 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 25 May 2022 23:44:32 +0200 Subject: [PATCH 02/42] change polarity data type --- src/communication/StepDirListener.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 855a701d..6047cf75 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -47,7 +47,7 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing - PinStatus polarity = RISING; //!< polarity of the step pin + int polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable From 4a8f8de48a942d800dcc8471d48716ed2d4df0d7 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 2 Jul 2022 09:09:36 +0200 Subject: [PATCH 03/42] g4 trials --- .../stm32/stm32f4/stm32f4_hal.cpp | 28 +++++- .../stm32/stm32g4/stm32g4_hal.cpp | 93 +++++++++++++------ .../stm32/stm32g4/stm32g4_mcu.cpp | 42 +++++---- 3 files changed, 113 insertions(+), 50 deletions(-) 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 ac890817..ea094859 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -78,7 +78,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - HAL_ADC_Init(&hadc); + if ( HAL_ADC_Init(&hadc) != HAL_OK){ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); + #endif + return -1; + } /**Configure for the selected ADC regular channel to be converted. */ @@ -122,17 +127,32 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive // first channel sConfigInjected.InjectedRank = 1; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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[0]), PinMap_ADC)) ); + #endif + return -1; + } // second channel sConfigInjected.InjectedRank = 2; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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[1]), PinMap_ADC)) ); + #endif + return -1; + } // third channel - if exists if(_isset(cs_params->pins[2])){ sConfigInjected.InjectedRank = 3; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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[2]), PinMap_ADC)) ); + #endif + return -1; + } } // enable interrupt 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 10587bd1..79747fd9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -5,6 +5,7 @@ #include "../../../../communication/SimpleFOCDebug.h" #define _TRGO_NOT_AVAILABLE 12345 +#define SIMPLEFOC_STM32_DEBUG // 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 @@ -16,7 +17,7 @@ uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ return ADC_EXTERNALTRIGINJEC_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->getHandle()->Instance == TIM3) return ADC_EXTERNALTRIGINJEC_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 @@ -57,7 +58,7 @@ uint32_t _timerToRegularTRGO(HardwareTimer* timer){ return ADC_EXTERNALTRIG_T2_TRGO; #endif #ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM2) + else if(timer->getHandle()->Instance == TIM3) return ADC_EXTERNALTRIG_T3_TRGO; #endif #ifdef TIM4 // if defined timer 4 @@ -96,8 +97,10 @@ 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); - + SIMPLEFOC_DEBUG("here adc_init!"); + if(hadc.Instance == ADC1) { + SIMPLEFOC_DEBUG("adc1"); #ifdef __HAL_RCC_ADC1_CLK_ENABLE __HAL_RCC_ADC1_CLK_ENABLE(); #endif @@ -107,6 +110,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } #ifdef ADC2 else if (hadc.Instance == ADC2) { + SIMPLEFOC_DEBUG("adc2"); #ifdef __HAL_RCC_ADC2_CLK_ENABLE __HAL_RCC_ADC2_CLK_ENABLE(); #endif @@ -117,6 +121,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif #ifdef ADC3 else if (hadc.Instance == ADC3) { + SIMPLEFOC_DEBUG("adc3"); #ifdef __HAL_RCC_ADC3_CLK_ENABLE __HAL_RCC_ADC3_CLK_ENABLE(); #endif @@ -135,18 +140,27 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive return -1; // error not a valid ADC instance } - hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + SIMPLEFOC_DEBUG("here configure start!"); + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; hadc.Init.Resolution = ADC_RESOLUTION_12B; - hadc.Init.ScanConvMode = ENABLE; - hadc.Init.ContinuousConvMode = ENABLE; - hadc.Init.DiscontinuousConvMode = DISABLE; - hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now - hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 1; - hadc.Init.DMAContinuousRequests = DISABLE; - hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - HAL_ADC_Init(&hadc); + // hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; + // hadc.Init.ContinuousConvMode = DISABLE; + // hadc.Init.LowPowerAutoWait = DISABLE; + // hadc.Init.GainCompensation = 0; + // hadc.Init.DiscontinuousConvMode = DISABLE; + // hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + // hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + // hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + // hadc.Init.NbrOfConversion = 1; + // hadc.Init.DMAContinuousRequests = DISABLE; + // hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + // hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } /**Configure for the selected ADC regular channel to be converted. */ @@ -155,10 +169,13 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; - sConfigInjected.AutoInjectedConv = DISABLE; - sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; - sConfigInjected.InjectedOffset = 0; + // sConfigInjected.AutoInjectedConv = DISABLE; + // sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + // sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + // sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + // sConfigInjected.InjectedOffset = 0; + SIMPLEFOC_DEBUG("here timer search start!"); // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; while(driver_params->timers[tim_num] != NP && tim_num < 6){ @@ -176,33 +193,51 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } if( cs_params->timer_handle == NP ){ // not possible to use these timers for low-side current sense - #ifdef SIMPLEFOC_STM32_DEBUG +#ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); - #endif +#endif return -1; } // display which timer is being used - #ifdef SIMPLEFOC_STM32_DEBUG +#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); - #endif +#endif + + SIMPLEFOC_DEBUG("here timer search done!"); + SIMPLEFOC_DEBUG("here injected config done!"); // first channel - sConfigInjected.InjectedRank = 1; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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[0]), PinMap_ADC)) ); +#endif + return -1; + } // second channel - sConfigInjected.InjectedRank = 2; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); - + 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[1]), PinMap_ADC)) ); +#endif + return -1; + } // third channel - if exists if(_isset(cs_params->pins[2])){ - sConfigInjected.InjectedRank = 3; + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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[2]), PinMap_ADC)) ); +#endif + return -1; + } } + SIMPLEFOC_DEBUG("here injected config done!"); // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC1_2_IRQn); @@ -229,7 +264,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } extern "C" { - void ADC_IRQHandler(void) + void ADC1_2_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } 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 254216f3..3ee52316 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -49,31 +49,37 @@ 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; - // stop all the timers for the driver - _stopTimers(driver_params->timers, 6); - - // 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) ){ - // 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; - // remember that this timer has repetition counter - no need to downasmple - needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; - } + // // stop all the timers for the driver + // _stopTimers(driver_params->timers, 6); + + // // 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) ){ + // // 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; + // // 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); // start the adc + // HAL_ADC_Start(cs_params->adc_handle); HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + SIMPLEFOC_DEBUG("here driver_sync!"); // restart all the timers of the driver - _startTimers(driver_params->timers, 6); + // _startTimers(driver_params->timers, 6); + pinMode(PB10,OUTPUT); } @@ -89,6 +95,7 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + digitalWrite(PB10, 0); // calculate the instance int adc_index = _adcToIndex(AdcHandle); @@ -100,7 +107,8 @@ 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][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + digitalWrite(PB10, 1); } } From adbdb6a338e4dc11ed1b246b701b7a67445c8066 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 8 Jul 2022 16:14:53 +0200 Subject: [PATCH 04/42] added full support for stm32g4 boards + restructure of stm32f4 and testing of stm32f1/f4/g4 --- .../stm32/stm32f1/stm32f1_mcu.cpp | 2 +- .../stm32/stm32f4/stm32f4_hal.cpp | 96 +++---- .../stm32/stm32f4/stm32f4_hal.h | 1 + .../stm32/stm32f4/stm32f4_mcu.cpp | 14 +- .../stm32/stm32f4/stm32f4_utils.cpp | 193 ++++++++++++++ .../stm32/stm32f4/stm32f4_utils.h | 34 +++ .../stm32/stm32g4/stm32g4_hal.cpp | 250 +++++++++--------- .../stm32/stm32g4/stm32g4_hal.h | 1 + .../stm32/stm32g4/stm32g4_mcu.cpp | 67 ++--- .../stm32/stm32g4/stm32g4_utils.cpp | 237 +++++++++++++++++ .../stm32/stm32g4/stm32g4_utils.h | 34 +++ src/drivers/hardware_specific/stm32_mcu.cpp | 3 + 12 files changed, 691 insertions(+), 241 deletions(-) create mode 100644 src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h create mode 100644 src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h 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 5015c058..720ff285 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -33,7 +33,7 @@ int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { - .pins={0}, + .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); 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 ea094859..68a9d094 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -2,53 +2,29 @@ #if defined(STM32F4xx) +//#define SIMPLEFOC_STM32_DEBUG + #include "../../../../communication/SimpleFOCDebug.h" #define _TRGO_NOT_AVAILABLE 12345 - -// 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) - return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; -#ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) - return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; -#endif -#ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) - return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; -#endif -#ifdef TIM5 // if defined timer 5 - else if(timer->getHandle()->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/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 -uint32_t _timerToRegularTRGO(HardwareTimer* timer){ - if(timer->getHandle()->Instance == TIM2) - return ADC_EXTERNALTRIGCONV_T2_TRGO; -#ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) - return ADC_EXTERNALTRIGCONV_T3_TRGO; -#endif -#ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) - return ADC_EXTERNALTRIGCONV_T8_TRGO; -#endif - else - return _TRGO_NOT_AVAILABLE; -} - ADC_HandleTypeDef hadc; 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 = (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_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) )){ +#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); @@ -67,6 +43,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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_DIV4; hadc.Init.Resolution = ADC_RESOLUTION_12B; hadc.Init.ScanConvMode = ENABLE; @@ -79,13 +59,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; if ( HAL_ADC_Init(&hadc) != HAL_OK){ - #ifdef SIMPLEFOC_STM32_DEBUG +#ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); - #endif +#endif return -1; } - /**Configure for the selected ADC regular channel to be converted. - */ /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time */ @@ -124,33 +102,35 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); #endif + // first channel - sConfigInjected.InjectedRank = 1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + 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)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)) ); - #endif +#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 = 2; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); + 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)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)) ); - #endif +#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 = 3; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); 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[2]), PinMap_ADC)) ); - #endif +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; +#endif return -1; } } 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 35b5aa67..42d75aa9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -8,6 +8,7 @@ #include "../../../../common/foc_utils.h" #include "../../../../drivers/hardware_specific/stm32_mcu.h" #include "../stm32_mcu.h" +#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); 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 f764f65c..0ab1c8f8 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -7,6 +7,7 @@ #include "../../../hardware_api.h" #include "../stm32_mcu.h" #include "stm32f4_hal.h" +#include "stm32f4_utils.h" #include "Arduino.h" @@ -21,21 +22,10 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; -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 { - .pins={0}, + .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); diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp new file mode 100644 index 00000000..20793d8c --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -0,0 +1,193 @@ +#include "stm32f4_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(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJECCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJECCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJECCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->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/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGCONV_T3_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGCONV_T8_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/stm32f4/stm32f4_utils.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h new file mode 100644 index 00000000..b4549bad --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h @@ -0,0 +1,34 @@ + +#ifndef STM32F4_UTILS_HAL +#define STM32F4_UTILS_HAL + +#include "Arduino.h" + +#if defined(STM32F4xx) + +#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(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); + +// 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/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 79747fd9..87b3ebdc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -3,104 +3,32 @@ #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) #include "../../../../communication/SimpleFOCDebug.h" -#define _TRGO_NOT_AVAILABLE 12345 -#define SIMPLEFOC_STM32_DEBUG - -// 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) - return ADC_EXTERNALTRIGINJEC_T1_TRGO; -#ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) - return ADC_EXTERNALTRIGINJEC_T2_TRGO; -#endif -#ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) - return ADC_EXTERNALTRIGINJEC_T3_TRGO; -#endif -#ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) - return ADC_EXTERNALTRIGINJEC_T4_TRGO; -#endif -#ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) - return ADC_EXTERNALTRIGINJEC_T6_TRGO; -#endif -#ifdef TIM7 // if defined timer 7 - else if(timer->getHandle()->Instance == TIM7) - return ADC_EXTERNALTRIGINJEC_T7_TRGO; -#endif -#ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) - return ADC_EXTERNALTRIGINJEC_T8_TRGO; -#endif -#ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) - return ADC_EXTERNALTRIGINJEC_T15_TRGO; -#endif -#ifdef TIM20 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM20) - return ADC_EXTERNALTRIGINJEC_T20_TRGO; -#endif - else - return _TRGO_NOT_AVAILABLE; -} - -// 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) - return ADC_EXTERNALTRIG_T1_TRGO; -#ifdef TIM2 // if defined timer 2 - else if(timer->getHandle()->Instance == TIM2) - return ADC_EXTERNALTRIG_T2_TRGO; -#endif -#ifdef TIM3 // if defined timer 3 - else if(timer->getHandle()->Instance == TIM3) - return ADC_EXTERNALTRIG_T3_TRGO; -#endif -#ifdef TIM4 // if defined timer 4 - else if(timer->getHandle()->Instance == TIM4) - return ADC_EXTERNALTRIG_T4_TRGO; -#endif -#ifdef TIM6 // if defined timer 6 - else if(timer->getHandle()->Instance == TIM6) - return ADC_EXTERNALTRIG_T6_TRGO; -#endif -#ifdef TIM7 // if defined timer 7 - else if(timer->getHandle()->Instance == TIM7) - return ADC_EXTERNALTRIG_T7_TRGO; -#endif -#ifdef TIM8 // if defined timer 8 - else if(timer->getHandle()->Instance == TIM8) - return ADC_EXTERNALTRIG_T7_TRGO; -#endif -#ifdef TIM15 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM15) - return ADC_EXTERNALTRIG_T15_TRGO; -#endif -#ifdef TIM20 // if defined timer 15 - else if(timer->getHandle()->Instance == TIM20) - return ADC_EXTERNALTRIG_T20_TRGO; -#endif - else - return _TRGO_NOT_AVAILABLE; -} +//#define SIMPLEFOC_STM32_DEBUG 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) + + // 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_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) )){ +#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); - SIMPLEFOC_DEBUG("here adc_init!"); if(hadc.Instance == ADC1) { - SIMPLEFOC_DEBUG("adc1"); #ifdef __HAL_RCC_ADC1_CLK_ENABLE __HAL_RCC_ADC1_CLK_ENABLE(); #endif @@ -110,7 +38,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } #ifdef ADC2 else if (hadc.Instance == ADC2) { - SIMPLEFOC_DEBUG("adc2"); #ifdef __HAL_RCC_ADC2_CLK_ENABLE __HAL_RCC_ADC2_CLK_ENABLE(); #endif @@ -121,7 +48,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif #ifdef ADC3 else if (hadc.Instance == ADC3) { - SIMPLEFOC_DEBUG("adc3"); #ifdef __HAL_RCC_ADC3_CLK_ENABLE __HAL_RCC_ADC3_CLK_ENABLE(); #endif @@ -132,6 +58,26 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive __HAL_RCC_ADC345_CLK_ENABLE(); #endif } +#endif +#ifdef ADC4 + else if (hadc.Instance == ADC4) { +#ifdef __HAL_RCC_ADC4_CLK_ENABLE + __HAL_RCC_ADC4_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC5 + else if (hadc.Instance == ADC5) { +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } #endif else{ #ifdef SIMPLEFOC_STM32_DEBUG @@ -139,43 +85,45 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif return -1; // error not a valid ADC instance } - - SIMPLEFOC_DEBUG("here configure start!"); + +#ifdef SIMPLEFOC_STM32_DEBUG + 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; - // hadc.Init.ContinuousConvMode = DISABLE; - // hadc.Init.LowPowerAutoWait = DISABLE; - // hadc.Init.GainCompensation = 0; - // hadc.Init.DiscontinuousConvMode = DISABLE; - // hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - // hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now - // hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - // hadc.Init.NbrOfConversion = 1; - // hadc.Init.DMAContinuousRequests = DISABLE; - // hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - // hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; + hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.LowPowerAutoWait = DISABLE; + hadc.Init.GainCompensation = 0; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 2; + hadc.Init.DMAContinuousRequests = DISABLE; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; if ( HAL_ADC_Init(&hadc) != HAL_OK){ #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); #endif return -1; } - /**Configure for the selected ADC regular channel to be converted. - */ /**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.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5; sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; - // sConfigInjected.AutoInjectedConv = DISABLE; - // sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; - // sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; - // sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; - // sConfigInjected.InjectedOffset = 0; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + sConfigInjected.InjectedOffset = 0; + sConfigInjected.InjecOversamplingMode = DISABLE; + sConfigInjected.QueueInjectedContext = DISABLE; - SIMPLEFOC_DEBUG("here timer search start!"); // automating TRGO flag finding - hardware specific uint8_t tim_num = 0; while(driver_params->timers[tim_num] != NP && tim_num < 6){ @@ -198,49 +146,75 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif return -1; } - // 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); -#endif - SIMPLEFOC_DEBUG("here timer search done!"); - SIMPLEFOC_DEBUG("here injected config done!"); // first channel sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); + 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)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)) ); + 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 = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); + 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)STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)) ); + 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 = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); 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[2]), PinMap_ADC)) ); + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; #endif return -1; } } - SIMPLEFOC_DEBUG("here injected config done!"); - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + + if(hadc.Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } +#ifdef ADC2 + else if (hadc.Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } +#endif +#ifdef ADC3 + else if (hadc.Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } +#endif +#ifdef ADC4 + else if (hadc.Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } +#endif +#ifdef ADC5 + else if (hadc.Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } +#endif cs_params->adc_handle = &hadc; return 0; @@ -268,6 +242,26 @@ extern "C" { { HAL_ADC_IRQHandler(&hadc); } +#ifdef ADC3 + void ADC3_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC4 + void ADC4_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC5 + void ADC5_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif } #endif \ No newline at end of file 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 c4159c72..8d3b9b59 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -9,6 +9,7 @@ #include "../../../../common/foc_utils.h" #include "../../../../drivers/hardware_specific/stm32_mcu.h" #include "../stm32_mcu.h" +#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); 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 3ee52316..a18905e6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -8,6 +8,7 @@ #include "../../../hardware_api.h" #include "../stm32_mcu.h" #include "stm32g4_hal.h" +#include "stm32g4_utils.h" #include "Arduino.h" @@ -15,28 +16,18 @@ #define _ADC_RESOLUTION_G4 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] = {0}; - -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; -} +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; +// does adc interrupt need a downsample - per adc (5) +bool needs_downsample[5] = {1}; +// downsampling variable - per adc (5) +uint8_t tim_downsample[5] = {0}; + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { - .pins={0}, + .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); @@ -49,37 +40,31 @@ 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; - // // stop all the timers for the driver - // _stopTimers(driver_params->timers, 6); - - // // 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) ){ - // // 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; - // // remember that this timer has repetition counter - no need to downasmple - // needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; - // } + // stop all the timers for the driver + _stopTimers(driver_params->timers, 6); + + // 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) ){ + // 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; + // 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); // start the adc - // HAL_ADC_Start(cs_params->adc_handle); HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - - SIMPLEFOC_DEBUG("here driver_sync!"); // restart all the timers of the driver - // _startTimers(driver_params->timers, 6); - pinMode(PB10,OUTPUT); + _startTimers(driver_params->timers, 6); } @@ -95,7 +80,6 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ - digitalWrite(PB10, 0); // calculate the instance int adc_index = _adcToIndex(AdcHandle); @@ -108,7 +92,6 @@ 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); - digitalWrite(PB10, 1); } } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp new file mode 100644 index 00000000..89a9bc34 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp @@ -0,0 +1,237 @@ +#include "stm32g4_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(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJEC_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJEC_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGINJEC_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJEC_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJEC_T6_TRGO; +#endif +#ifdef TIM7 // if defined timer 7 + else if(timer->getHandle()->Instance == TIM7) + return ADC_EXTERNALTRIGINJEC_T7_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJEC_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIGINJEC_T15_TRGO; +#endif +#ifdef TIM20 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM20) + return ADC_EXTERNALTRIGINJEC_T20_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// 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) + return ADC_EXTERNALTRIG_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIG_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIG_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIG_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIG_T6_TRGO; +#endif +#ifdef TIM7 // if defined timer 7 + else if(timer->getHandle()->Instance == TIM7) + return ADC_EXTERNALTRIG_T7_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIG_T7_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIG_T15_TRGO; +#endif +#ifdef TIM20 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM20) + return ADC_EXTERNALTRIG_T20_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/stm32g4/stm32g4_utils.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h new file mode 100644 index 00000000..fa857bd0 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h @@ -0,0 +1,34 @@ + +#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(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); + +// 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/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index aedc4b3c..40af88ae 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -4,6 +4,9 @@ #if defined(_STM32_DEF_) + +//#define SIMPLEFOC_STM32_DEBUG + #ifdef SIMPLEFOC_STM32_DEBUG void printTimerCombination(int numPins, PinMap* timers[], int score); int getTimerNumber(int timerIndex); From a7a33e5d2ddcea917b48696d10efc291c5c248f2 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 8 Jul 2022 16:18:23 +0200 Subject: [PATCH 05/42] initfoc fails if current sense is not well initialised --- README.md | 50 ++++------------------- src/BLDCMotor.cpp | 10 ++++- src/common/base_classes/CurrentSense.h | 1 + src/current_sense/GenericCurrentSense.cpp | 2 + src/current_sense/InlineCurrentSense.cpp | 2 + src/current_sense/LowsideCurrentSense.cpp | 2 + 6 files changed, 25 insertions(+), 42 deletions(-) diff --git a/README.md b/README.md index 25c728a5..d68fed80 100644 --- a/README.md +++ b/README.md @@ -20,47 +20,15 @@ Therefore this is an attempt to: -
-

NEW RELEASE 📢: SimpleFOClibrary v2.2.2 see release

-
    -
  • GenericCurrentSense bugfix and testing
    • -
  • bugfix leonardo #170
    • -
  • bugfix - no index search after specifying natural direction
    • -
  • Low level API restructuring -
      -
    • Driver API
      • -
    • Current sense API
      • -
    -
    • -
  • New debugging interface - see in docs -
      -
    • Static class SimpleFOCDebug
      • -
    -
    • -
  • CurrentSense API change - added method linkDriver() - see in docs
    • -
  • Low-side current sensing - see in docs -
      -
    • ESP32 generic support for multiple motors
      • -
    • Added low-side current sensing support for stm32 - only one motor -
        -
      • f1 family
        • -
      • f4 family
        • -
      • g4 family
        • -
      -
      • -
    -
    • -
  • New appraoch for current estimation for torque control using voltage - see in docs -
      -
    • Support for motor KV rating - back emf estimation
      • -
    • Using motor phase resistance
      • -
    -
    • -
  • KV rating and phase resistance used for open-loop current limiting as well - see in docs
    • -
-
- -## Arduino *SimpleFOClibrary* v2.2 +> FUTURE RELEASE : SimpleFOClibrary v2.2.3 +> - stm32 low-side current sensing +> - g4 supported +> - thoroughly tested f1/f4/g4 +> - bugfixing +> - leonardo +> - mega2560 + +## Arduino *SimpleFOClibrary* v2.2.2

diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c417e684..21b43faa 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -121,7 +121,15 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // and checks the direction of measuremnt. _delay(500); if(exit_flag){ - if(current_sense) exit_flag *= alignCurrentSense(); + if(current_sense){ + 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."); } diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad1bd9ca..367dbaea 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -29,6 +29,7 @@ class CurrentSense{ bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() BLDCDriver* driver; //!< driver link + bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing /** diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 269e0ba5..d0592ef2 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -13,6 +13,8 @@ int GenericCurrentSense::init(){ if(initCallback != nullptr) initCallback(); // calibrate zero offsets calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success return 1; } diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 86f81a47..b289a983 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -27,6 +27,8 @@ int InlineCurrentSense::init(){ if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success return 1; } diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 8b9fe3ed..1453687b 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -29,6 +29,8 @@ int LowsideCurrentSense::init(){ _driverSyncLowSide(driver->params, params); // calibrate zero offsets calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success return 1; } From 20fe40a9cde9f69cacf495e2b772d37daf1f4aba Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 8 Jul 2022 16:19:56 +0200 Subject: [PATCH 06/42] new example and workflow exemption --- .github/workflows/ccpp.yml | 4 +- .../stm32_current_control_low_side.ino | 167 ++++++++++++++++++ 2 files changed, 169 insertions(+), 2 deletions(-) create mode 100644 examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 5ee6e772..19fa7d49 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,7 +26,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: 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 + sketches-exclude: 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 - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino @@ -57,7 +57,7 @@ jobs: - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino, stm32_current_control_low_side.ino - arduino-boards-fqbn: STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json diff --git a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino new file mode 100644 index 00000000..3e3f04ab --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -0,0 +1,167 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A PA8 +#define INH_B PA9 +#define INH_C PA10 + +#define EN_GATE PB7 +#define M_PWM PB4 +#define M_OC PB3 +#define OC_ADJ PB6 +#define OC_GAIN PB5 + +#define IOUTA PA0 +#define IOUTB PA1 +#define IOUTC PA2 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC); + +// encoder instance +Encoder encoder = Encoder(PB14, PB15, 2048); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM,HIGH); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 19; + driver.pwm_frequency = 15000; // suggested under 18khz + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 0; + + // initialise motor + motor.init(); + + cs.init(); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file From a43ae94366329b8b54319720570ed7c11c9aa948 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 8 Jul 2022 16:22:35 +0200 Subject: [PATCH 07/42] a bit more info in readme + 2.2.3 in the library.properties --- README.md | 4 +++- library.properties | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d68fed80..02413220 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,9 @@ Therefore this is an attempt to: > - bugfixing > - leonardo > - mega2560 - +> - `initFOC` fails if current sense not initialised +> - driver and cs have to be well initialised for `initFOC` to start +> - `cs.init()` and `driver.init()` return `1` if well initialised and `0` if failed ## Arduino *SimpleFOClibrary* v2.2.2

diff --git a/library.properties b/library.properties index d4b639d5..79e6cc4b 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.2.2 +version=2.2.3 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From 4c1c66b8a6b124a18fbb5fb772c5be6df6f1e284 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 8 Jul 2022 16:27:50 +0200 Subject: [PATCH 08/42] remove unused variable + CI update --- .github/workflows/ccpp.yml | 6 +++--- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 1 - 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 19fa7d49..72c1054d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -57,7 +57,7 @@ jobs: - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino, stm32_current_control_low_side.ino + sketch-names: bluepill_position_control.ino, stm32_i2c_dual_bus_example.ino, stm32_spi_alt_example.ino - arduino-boards-fqbn: STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json @@ -68,11 +68,11 @@ jobs: - arduino-boards-fqbn: STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino + sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, sdouble_full_control_example.ino + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, sdouble_full_control_example.ino, stm32_current_control_low_side.ino # Do not cancel all jobs / architectures if one job fails diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 941fb385..c172da48 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -68,7 +68,6 @@ int adc_read_index[2]={0}; // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - uint32_t raw_adc; mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; From 614a7e125e4c6a749e5caef0ded1bd36ad53068d Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 8 Jul 2022 17:28:11 +0200 Subject: [PATCH 09/42] bugfix for #188 --- README.md | 1 + src/common/base_classes/CurrentSense.h | 2 +- src/current_sense/InlineCurrentSense.cpp | 5 ++++- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 02413220..f1ec92a4 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,7 @@ Therefore this is an attempt to: > - bugfixing > - leonardo > - mega2560 +> - inline current sense without driver #188 > - `initFOC` fails if current sense not initialised > - driver and cs have to be well initialised for `initFOC` to start > - `cs.init()` and `driver.init()` return `1` if well initialised and `0` if failed diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 367dbaea..ad9f926d 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -28,7 +28,7 @@ class CurrentSense{ // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - BLDCDriver* driver; //!< driver link + BLDCDriver* 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 diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index b289a983..eff00785 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -21,8 +21,11 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ // Inline sensor init function int InlineCurrentSense::init(){ + // if no linked driver its fine in this case + // at least for init() + void* drv_params = driver ? driver->params : nullptr; // configure ADC variables - params = _configureADCInline(driver->params,pinA,pinB,pinC); + params = _configureADCInline(drv_params,pinA,pinB,pinC); // if init failed return fail if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; // calibrate zero offsets From f201af2265f5f581deb40ef09182e0686c5292d1 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Fri, 8 Jul 2022 17:37:18 +0200 Subject: [PATCH 10/42] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e49ef384..1492bf43 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Arduino Simple Field Oriented Control (FOC) library +# SimpleFOC - A Cross-platform Field Oriented Control (FOC) library based on the Arduino IDE ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) From 6983d12afac4f568ca6912d1026bbaea4ef80b4d Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Fri, 8 Jul 2022 17:51:32 +0200 Subject: [PATCH 11/42] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1492bf43..21be0cea 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ -# SimpleFOC - A Cross-platform Field Oriented Control (FOC) library based on the Arduino IDE - +# SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
+### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) From 05b7f8e461f793fd67afe189a3916f391d6c3657 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 10 Jul 2022 09:32:31 +0200 Subject: [PATCH 12/42] Update README.md --- README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ce1e7d1a..75eadf44 100644 --- a/README.md +++ b/README.md @@ -2,10 +2,16 @@ ### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) -[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) +![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) +![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) +![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) + [![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) + We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones. The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used. Additionally, most of the efforts at this moment are still channeled towards the high-power applications of the BLDC motors and proper low-cost and low-power FOC supporting boards are very hard to find today and even may not exist.
From 1acfda1d22aac484ab231fafd729f480603fd854 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 10 Jul 2022 09:45:57 +0200 Subject: [PATCH 13/42] Update esp32_mcu.cpp --- src/current_sense/hardware_specific/esp32/esp32_mcu.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 941fb385..6bde97dc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -68,8 +68,6 @@ int adc_read_index[2]={0}; // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ - uint32_t raw_adc; - mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit; int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index; float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv; From b300dab062a8f25b77b06a8deda92ef5ec04a61e Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 10 Jul 2022 09:48:22 +0200 Subject: [PATCH 14/42] added esp32s3 to compilation --- .github/workflows/ccpp.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 72c1054d..e44a42b6 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -16,6 +16,7 @@ jobs: - adafruit:samd:adafruit_metro_m4 # samd51 - esp32:esp32:esp32doit-devkit-v1 # esp32 - esp32:esp32:esp32s2 # esp32s2 + - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive @@ -51,6 +52,10 @@ jobs: platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone + - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino From c87318caa2812050f6faafc8788432645cd2a208 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 24 Jul 2022 16:43:01 +0200 Subject: [PATCH 15/42] add debug to start/stop timers --- src/drivers/hardware_specific/stm32_mcu.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index aedc4b3c..ee030395 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -167,6 +167,9 @@ void _startTimers(HardwareTimer **timers_to_start, int timer_num) 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 } } From 581af181ae9083db8d7f8d27ae2736e15c7f007f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 19 Aug 2022 00:33:39 +0200 Subject: [PATCH 16/42] add ability to reverse PWM polarity on RP2040 --- src/drivers/hardware_specific/rp2040_mcu.cpp | 75 ++++++++++++-------- 1 file changed, 44 insertions(+), 31 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 2c127d71..15582b73 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -7,15 +7,28 @@ #define SIMPLEFOC_DEBUG_RP2040 -#ifdef SIMPLEFOC_DEBUG_RP2040 -#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL -#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial -#endif +#include "Arduino.h" +#include "communication/SimpleFOCDebug.h" -#endif -#include "Arduino.h" +// these defines determine the polarity of the PWM output. Normally, the polarity is active-high, +// i.e. a high-level PWM output is expected to switch on the MOSFET. But should your driver design +// require inverted polarity, you can change the defines below, or set them via your build environment +// or board definition files. + +// used for 2-PWM, 3-PWM, and 4-PWM modes +#ifndef SIMPLEFOC_PWM_ACTIVE_HIGH +#define SIMPLEFOC_PWM_ACTIVE_HIGH true +#endif +// used fof 6-PWM mode, high-side +#ifndef SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH true +#endif +// used fof 6-PWM mode, low-side +#ifndef SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true +#endif @@ -48,16 +61,16 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 #ifdef SIMPLEFOC_DEBUG_RP2040 - SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue); + SimpleFOCDebug::print("Configuring pin "); + SimpleFOCDebug::print(pin); + SimpleFOCDebug::print(" slice "); + SimpleFOCDebug::print((int)slice); + SimpleFOCDebug::print(" channel "); + SimpleFOCDebug::print((int)chan); + SimpleFOCDebug::print(" frequency "); + SimpleFOCDebug::print((int)pwm_frequency); + SimpleFOCDebug::print(" top value "); + SimpleFOCDebug::println(wrapvalue); #endif pwm_set_wrap(slice, wrapvalue); wrapvalues[slice] = wrapvalue; @@ -84,8 +97,8 @@ void syncSlices() { void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; - setupPWM(pinA, pwm_frequency, false, params, 0); - setupPWM(pinB, pwm_frequency, false, params, 1); + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); syncSlices(); return params; } @@ -95,9 +108,9 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; - setupPWM(pinA, pwm_frequency, false, params, 0); - setupPWM(pinB, pwm_frequency, false, params, 1); - setupPWM(pinC, pwm_frequency, false, params, 2); + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pinC, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); syncSlices(); return params; } @@ -108,10 +121,10 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; - setupPWM(pin1A, pwm_frequency, false, params, 0); - setupPWM(pin1B, pwm_frequency, false, params, 1); - setupPWM(pin2A, pwm_frequency, false, params, 2); - setupPWM(pin2B, pwm_frequency, false, params, 3); + setupPWM(pin1A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pin1B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pin2A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); + setupPWM(pin2B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 3); syncSlices(); return params; } @@ -122,12 +135,12 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons RP2040DriverParams* params = new RP2040DriverParams(); params->pwm_frequency = pwm_frequency; params->dead_zone = dead_zone; - setupPWM(pinA_h, pwm_frequency, false, params, 0); - setupPWM(pinB_h, pwm_frequency, false, params, 2); - setupPWM(pinC_h, pwm_frequency, false, params, 4); - setupPWM(pinA_l, pwm_frequency, true, params, 1); - setupPWM(pinB_l, pwm_frequency, true, params, 3); - setupPWM(pinC_l, pwm_frequency, true, params, 5); + setupPWM(pinA_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 0); + setupPWM(pinB_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 2); + setupPWM(pinC_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 4); + setupPWM(pinA_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 1); + setupPWM(pinB_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 3); + setupPWM(pinC_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 5); syncSlices(); return params; } From eb5347f4ad6f3f51642ae7fbd4cb1283fee6bafc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:03:55 +0200 Subject: [PATCH 17/42] added default PWM frequency to RP2040 driver --- src/drivers/hardware_specific/rp2040_mcu.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 15582b73..9fcd8e45 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -6,10 +6,7 @@ #define SIMPLEFOC_DEBUG_RP2040 - - -#include "Arduino.h" -#include "communication/SimpleFOCDebug.h" +#include "../hardware_api.h" // these defines determine the polarity of the PWM output. Normally, the polarity is active-high, @@ -31,6 +28,10 @@ #endif +#define _PWM_FREQUENCY 24000 +#define _PWM_FREQUENCY_MAX 66000 +#define _PWM_FREQUENCY_MIN 5000 + typedef struct RP2040DriverParams { int pins[6]; @@ -59,7 +60,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para pwm_set_phase_correct(slice, true); uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 - if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 + if (wrapvalue > 12499) wrapvalue = 12499; // 20kHz, resolution 12500 #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); SimpleFOCDebug::print(pin); @@ -96,6 +97,8 @@ void syncSlices() { void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); @@ -107,6 +110,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); @@ -120,6 +125,8 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; setupPWM(pin1A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); setupPWM(pin1B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); @@ -133,6 +140,8 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const 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) { // non-PIO solution... RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; params->dead_zone = dead_zone; setupPWM(pinA_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 0); From 14c27aab4c30eceba4ac03b9504c48d9acceb399 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:04:13 +0200 Subject: [PATCH 18/42] fix STM32 compile problem --- src/communication/StepDirListener.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 6047cf75..911ccbf4 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -4,6 +4,12 @@ #include "Arduino.h" #include "../common/foc_utils.h" + +#if defined(_STM32_DEF_) +#define PinStatus int +#endif + + /** * Step/Dir listenner class for easier interraction with this communication interface. */ @@ -47,7 +53,7 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing - int polarity = RISING; //!< polarity of the step pin + PinStatus polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable From 17b460d6955da84a3881c3c69843e2be0b33a594 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:37:41 +0200 Subject: [PATCH 19/42] fix pinStatus issue --- src/communication/StepDirListener.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 911ccbf4..da9a6a2d 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if defined(_STM32_DEF_) +#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) #define PinStatus int #endif From c9e14b670f6c1cda7013d542680f03aadcf3b051 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Aug 2022 03:45:17 +0200 Subject: [PATCH 20/42] fix compile error for Due --- src/communication/StepDirListener.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index da9a6a2d..7d7f2494 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -5,7 +5,7 @@ #include "../common/foc_utils.h" -#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) +#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_SAM_DUE) #define PinStatus int #endif From 11ff60808b9e655dc8b0a9bf9ee81ae73b8abe0b Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Wed, 24 Aug 2022 20:41:19 +0200 Subject: [PATCH 21/42] Improve debug logging for B-G431B current sense --- .../hardware_specific/stm32/b_g431/b_g431_hal.cpp | 15 +++++++++------ .../hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 14 ++++++++------ 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp index 806c68e7..ee088ecf 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp @@ -1,10 +1,13 @@ #include "../../../hardware_api.h" #if defined(ARDUINO_B_G431B_ESC1) +#include "communication/SimpleFOCDebug.h" + #include "stm32g4xx_hal.h" #include "stm32g4xx_ll_pwr.h" #include "stm32g4xx_hal_adc.h" #include "b_g431_hal.h" + // From STM32 cube IDE /** ****************************************************************************** @@ -101,7 +104,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) if (HAL_ADC_Init(hadc1) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); } /** Configure the ADC multi-mode @@ -109,7 +112,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) multimode.Mode = ADC_MODE_INDEPENDENT; if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel failed!"); } /** Configure Regular Channel */ @@ -121,7 +124,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /** Configure Regular Channel */ @@ -129,7 +132,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Rank = ADC_REGULAR_RANK_2; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /* USER CODE BEGIN ADC1_Init 2 */ @@ -173,7 +176,7 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) if (HAL_ADC_Init(hadc2) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); } /** Configure Regular Channel */ @@ -185,7 +188,7 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /* USER CODE BEGIN ADC2_Init 2 */ 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 4adfff57..5f2a7c87 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 @@ -6,6 +6,7 @@ #include "Arduino.h" #include "../stm32_mcu.h" #include "../../../../drivers/hardware_specific/stm32_mcu.h" +#include "communication/SimpleFOCDebug.h" #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4096.0f @@ -21,8 +22,8 @@ static OPAMP_HandleTypeDef hopamp3; static DMA_HandleTypeDef hdma_adc1; static DMA_HandleTypeDef hdma_adc2; -uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion -uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion +volatile uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion +volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion // function reading an ADC value and returning the read voltage // As DMA is being used just return the DMA result @@ -52,7 +53,7 @@ void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; if (HAL_OPAMP_Init(hopamp) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!"); } } void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){ @@ -75,7 +76,7 @@ void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Chan HAL_DMA_DeInit(hdma_adc); if (HAL_DMA_Init(hdma_adc) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_DMA_Init failed!"); } __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); } @@ -95,11 +96,11 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("DMA read init failed"); } if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("DMA read init failed"); } HAL_OPAMP_Start(&hopamp1); @@ -111,6 +112,7 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. _delay(5); if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { + SIMPLEFOC_DEBUG("Current sense init failed, no DMA?"); return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; } From 370a2099ac9a510fd921acb9a947726e7c6fac65 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Thu, 25 Aug 2022 14:35:32 +0200 Subject: [PATCH 22/42] Fix low side current sense on B-G431B --- .../hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 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 5f2a7c87..1d74dbf8 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 @@ -106,19 +106,11 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi HAL_OPAMP_Start(&hopamp1); HAL_OPAMP_Start(&hopamp2); HAL_OPAMP_Start(&hopamp3); - - // Check if the ADC DMA is collecting any data. - // If this fails, it likely means timer1 has not started. Verify that your application starts - // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. - _delay(5); - if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { - SIMPLEFOC_DEBUG("Current sense init failed, no DMA?"); - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; - } Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), + .timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this) }; return params; From 6f90ee27a594626f8c9401b8fe2083783796c88c Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 27 Aug 2022 22:32:43 +0200 Subject: [PATCH 23/42] add routines for 1-pwm in driver code --- src/BLDCMotor.cpp | 2 +- src/drivers/hardware_api.h | 22 +++++++++++++++++++ src/drivers/hardware_specific/generic_mcu.cpp | 21 ++++++++++++++++++ 3 files changed, 44 insertions(+), 1 deletion(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 21b43faa..f6b5ef38 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -345,7 +345,7 @@ void BLDCMotor::move(float new_target) { // 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 + // get angular velocity TODO the velocity reading probably also shouldn't happen in open loop modes? shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index cb2385b7..a1ca885a 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -19,6 +19,17 @@ typedef struct GenericDriverParams { float dead_zone; } GenericDriverParams; +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA pwm pin + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful + */ +void* _configure1PWM(long pwm_frequency, const int pinA); /** * Configuring PWM frequency, resolution and alignment @@ -80,6 +91,17 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const */ 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); +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param params the driver parameters + */ +void _writeDutyCycle1PWM(float dc_a, void* params); + /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) * - Stepper driver - 2PWM setting diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index cb3bfd34..fe6e65a4 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -5,6 +5,18 @@ __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif +// function setting the high pwm frequency to the supplied pin +// - Stepper motor - 1PWM setting +// - hardware speciffic +// in generic case dont do anything +__attribute__((weak)) void* _configure1PWM(long pwm_frequency, const int pinA) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -59,6 +71,15 @@ __attribute__((weak)) void* _configure6PWM(long pwm_frequency, float dead_zone, } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 1PWM setting +// - hardware speciffic +__attribute__((weak)) void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic From 60acc40c8e558ce415eb51ab9190b059bbfb1f79 Mon Sep 17 00:00:00 2001 From: Stefan Dessens Date: Mon, 5 Sep 2022 12:38:46 +0200 Subject: [PATCH 24/42] Print a warning message when inline current sense is used on the B-G431b --- .../hardware_specific/stm32/b_g431/b_g431_mcu.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) 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 1d74dbf8..c22de049 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 @@ -83,6 +83,17 @@ void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Chan void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); + _UNUSED(pinA); + _UNUSED(pinB); + _UNUSED(pinC); + + SIMPLEFOC_DEBUG("B-G431B does not implement inline current sense. Use low-side current sense instead."); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; +} + + +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); HAL_Init(); MX_GPIO_Init(); From 2d258e372ee8d0693bf3a6849539c84c2b42647a Mon Sep 17 00:00:00 2001 From: Taylor Alexander Date: Thu, 15 Sep 2022 21:00:50 -0700 Subject: [PATCH 25/42] Fix incomplete PWM initialization for 8th slice. --- src/drivers/hardware_specific/rp2040_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 9fcd8e45..565a3c96 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -91,7 +91,7 @@ void syncSlices() { pwm_set_counter(i, 0); } // enable all slices - pwm_set_mask_enabled(0x7F); + pwm_set_mask_enabled(0xFF); } From c9ddb99b683b1f5853ebbbb8a8a4b56e4cb95103 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Sep 2022 01:24:15 +0200 Subject: [PATCH 26/42] add 1-PWM support to RP2040 PWM driver --- src/drivers/hardware_specific/rp2040_mcu.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 565a3c96..f7b58f80 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -95,6 +95,19 @@ void syncSlices() { } + +void* _configure1PWM(long pwm_frequency, const int pinA) { + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + syncSlices(); + return params; +} + + + void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; @@ -165,6 +178,12 @@ void writeDutyCycle(float val, uint slice, uint chan) { +void _writeDutyCycle1PWM(float dc_a, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); +} + + + void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); From 07090bce055a81d67f4c905563766c6350fa146e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Sep 2022 01:32:33 +0200 Subject: [PATCH 27/42] Add 1-PWM support to esp32 LEDC driver --- .../hardware_specific/esp32_ledc_mcu.cpp | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp index 55348e2b..2d34493e 100644 --- a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -53,6 +53,33 @@ void _setHighFrequency(const long freq, const int pin, const int channel){ + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1 }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + + + + + + void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -123,6 +150,12 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in +void _writeDutyCycle1PWM(float dc_a, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); +} + + + void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); From 2c43b81ed15f5a35bbb2524b50e6c5fdcc060a20 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Sep 2022 01:32:51 +0200 Subject: [PATCH 28/42] add 1-PWM support to esp8266 driver --- src/drivers/hardware_specific/esp8266_mcu.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index 5bdfd5d2..0f8c9397 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -12,6 +12,22 @@ void _setHighFrequency(const long freq, const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -60,6 +76,13 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic From bf0019d55bda4e5942328fe7c257ea192866d930 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Sep 2022 01:39:09 +0200 Subject: [PATCH 29/42] add 1-PWM support to atmega drivers --- .../hardware_specific/atmega2560_mcu.cpp | 23 ++++++++++++++++ .../hardware_specific/atmega328_mcu.cpp | 26 +++++++++++++++++++ .../hardware_specific/atmega32u4_mcu.cpp | 25 ++++++++++++++++++ 3 files changed, 74 insertions(+) diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 8ee1a727..11b7b945 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -25,6 +25,21 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -56,6 +71,14 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 49318c52..39d9bf38 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -17,6 +17,20 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -50,6 +64,18 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 9001fd90..1e2adb81 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -28,6 +28,20 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -59,6 +73,17 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic From e29ee8c07e15fc2f44ab83192d915d212dd6cbdb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 18 Sep 2022 01:39:22 +0200 Subject: [PATCH 30/42] add 1-PWM support to teensy driver --- src/drivers/hardware_specific/teensy_mcu.cpp | 28 ++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 93331252..37865d2d 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -12,6 +12,21 @@ void _setHighFrequency(const long freq, const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -60,6 +75,17 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i return params; } + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params) { + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic @@ -68,6 +94,8 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic From 44e4c4adf91dd59f7ee55318b07acb9bdb4e390e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Mon, 19 Sep 2022 23:03:40 +0200 Subject: [PATCH 31/42] fix for compile on Itsy Bitsy M4 #217 --- src/drivers/hardware_specific/samd51_mcu.cpp | 35 ++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index a9191702..f9cd8467 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -14,6 +14,36 @@ #endif +#ifndef TCC3_CH0 +#define TCC3_CH0 NOT_ON_TIMER +#define TCC3_CH1 NOT_ON_TIMER +#endif + +#ifndef TCC4_CH0 +#define TCC4_CH0 NOT_ON_TIMER +#define TCC4_CH1 NOT_ON_TIMER +#endif + + +#ifndef TC4_CH0 +#define TC4_CH0 NOT_ON_TIMER +#define TC4_CH1 NOT_ON_TIMER +#endif + +#ifndef TC5_CH0 +#define TC5_CH0 NOT_ON_TIMER +#define TC5_CH1 NOT_ON_TIMER +#endif + +#ifndef TC6_CH0 +#define TC6_CH0 NOT_ON_TIMER +#define TC6_CH1 NOT_ON_TIMER +#endif + +#ifndef TC7_CH0 +#define TC7_CH0 NOT_ON_TIMER +#define TC7_CH1 NOT_ON_TIMER +#endif @@ -109,7 +139,12 @@ struct wo_association WO_associations[] = { wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; +#ifndef TCC3_CC_NUM +uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM }; +#else uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM }; +#endif + struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { for (int i=0;i Date: Sat, 8 Oct 2022 19:49:22 +0200 Subject: [PATCH 32/42] public setPhaseVoltage --- src/BLDCMotor.h | 2 +- src/StepperMotor.h | 9 ++++----- src/common/base_classes/FOCMotor.h | 11 +++++++++++ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 4c87be67..a585ebe2 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -78,7 +78,7 @@ class BLDCMotor: public FOCMotor * @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); + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; private: // FOC methods diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 9ee39b79..8d9fcc1b 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -78,10 +78,7 @@ class StepperMotor: public FOCMotor float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - private: - - // FOC methods - /** + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm * @@ -89,8 +86,10 @@ class StepperMotor: public FOCMotor * @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); + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + private: + /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 670c69b2..0b86b89c 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -127,6 +127,16 @@ class FOCMotor */ virtual void move(float target = NOT_SET)=0; + /** + * 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 + */ + virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0; + // State calculation methods /** Shaft angle calculation in radians [rad] */ float shaftAngle(); @@ -137,6 +147,7 @@ class FOCMotor float shaftVelocity(); + /** * Electrical angle calculation */ From d7d1ac0ea8c4ef195b81ddc74675f1bd97449334 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 9 Oct 2022 11:27:31 +0200 Subject: [PATCH 33/42] bemf voltage open loop bug #219 --- src/BLDCMotor.cpp | 15 ++++++++++----- src/StepperMotor.cpp | 14 ++++++++++---- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index f6b5ef38..1909eb67 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -636,9 +636,11 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, 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)); @@ -674,8 +676,11 @@ float BLDCMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, 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 // sensor precision: this calculation is OK due to the normalisation setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index f8b112ef..8c557cfe 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -377,8 +377,11 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, 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)); @@ -412,8 +415,11 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, 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)); From 2cb2c8ae5f4cda312092fc4d870572ac21bdd52e Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 9 Oct 2022 11:35:02 +0200 Subject: [PATCH 34/42] _round macro issue - already defined for esp32 - #207 --- src/common/foc_utils.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 0d72485d..be655fb0 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -5,7 +5,9 @@ // sign function #define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#ifndef _round #define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f)) +#endif #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) From b1a5e4ed1f42e136e04057835b3532b40e5f116f Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 9 Oct 2022 12:02:54 +0200 Subject: [PATCH 35/42] esp32s adc driver -contribution by @Fu-Xiaotian issues #198 and #212 --- .../esp32/esp32_adc_driver.cpp | 2 +- .../esp32/esp32s_adc_driver.cpp | 257 ++++++++++++++++++ 2 files changed, 258 insertions(+), 1 deletion(-) create mode 100644 src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp 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 93aba630..807c387d 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3) #include "freertos/FreeRTOS.h" #include "freertos/task.h" diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp new file mode 100644 index 00000000..2e956770 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -0,0 +1,257 @@ +#include "esp32_adc_driver.h" + +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)) + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "rom/ets_sys.h" +#include "esp_attr.h" +#include "esp_intr.h" +#include "soc/rtc_io_reg.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/sens_reg.h" + +static uint8_t __analogAttenuation = 3;//11db +static uint8_t __analogWidth = 3;//12 bits +static uint8_t __analogCycles = 8; +static uint8_t __analogSamples = 0;//1 sample +static uint8_t __analogClockDiv = 1; + +// Width of returned answer () +static uint8_t __analogReturnedWidth = 12; + +void __analogSetWidth(uint8_t bits){ + if(bits < 9){ + bits = 9; + } else if(bits > 12){ + bits = 12; + } + __analogReturnedWidth = bits; + __analogWidth = bits - 9; + // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); + // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); + + // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); + // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); +} + +void __analogSetCycles(uint8_t cycles){ + __analogCycles = cycles; + // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); + // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); +} + +void __analogSetSamples(uint8_t samples){ + if(!samples){ + return; + } + __analogSamples = samples - 1; + SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); + SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); +} + +void __analogSetClockDiv(uint8_t clockDiv){ + if(!clockDiv){ + return; + } + __analogClockDiv = clockDiv; + SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); + SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); +} + +void __analogSetAttenuation(uint8_t attenuation) +{ + __analogAttenuation = attenuation & 3; + uint32_t att_data = 0; + int i = 10; + while(i--){ + att_data |= __analogAttenuation << (i * 2); + } + WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels + WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); +} + +void IRAM_ATTR __analogInit(){ + static bool initialized = false; + if(initialized){ + return; + } + + __analogSetAttenuation(__analogAttenuation); + __analogSetCycles(__analogCycles); + __analogSetSamples(__analogSamples + 1);//in samples + __analogSetClockDiv(__analogClockDiv); + __analogSetWidth(__analogWidth + 9);//in bits + + SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV); + SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV); + + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + + CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM + SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + + CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM + SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); + SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); + SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); + while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm== + + initialized = true; +} + +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) +{ + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0 || attenuation > 3){ + return ; + } + __analogInit(); + if(channel > 7){ + SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); + } else { + SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); + } +} + +bool IRAM_ATTR __adcAttachPin(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + int8_t pad = digitalPinToTouchChannel(pin); + if(pad >= 0){ + uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_CONF_REG); + if(touch & (1 << pad)){ + touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S))); + WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_REG, touch); + } + } else if(pin == 25){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 + } else if(pin == 26){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 + } + + pinMode(pin, ANALOG); + + __analogInit(); + return true; +} + +bool IRAM_ATTR __adcStart(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + } + return true; +} + +bool IRAM_ATTR __adcBusy(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 7){ + return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); + } + return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); +} + +uint16_t IRAM_ATTR __adcEnd(uint8_t pin) +{ + + uint16_t value = 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return 0;//not adc pin + } + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); +} + +void __analogReadResolution(uint8_t bits) +{ + if(!bits || bits > 16){ + return; + } + __analogSetWidth(bits); // hadware from 9 to 12 + __analogReturnedWidth = bits; // software from 1 to 16 +} + +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + __analogInit(); + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M); + } + + uint16_t value = 0; + + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); +} + + +#endif \ No newline at end of file From b90c4c69ada0f5dfba1432cc56f6fc9d92ce7cc7 Mon Sep 17 00:00:00 2001 From: Igitigit2 Date: Mon, 10 Oct 2022 18:24:43 +0200 Subject: [PATCH 36/42] Addded changes tio use additional ADC channels --- .../stm32/b_g431/b_g431_hal.cpp | 43 ++++++++++++++++++- .../stm32/b_g431/b_g431_mcu.cpp | 10 ++++- 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp index ee088ecf..dc505d6f 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp @@ -95,7 +95,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc1->Init.LowPowerAutoWait = DISABLE; hadc1->Init.ContinuousConvMode = DISABLE; - hadc1->Init.NbrOfConversion = 2; + hadc1->Init.NbrOfConversion = 5; hadc1->Init.DiscontinuousConvMode = DISABLE; hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; @@ -134,6 +134,47 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) { SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } + + //****************************************************************** + // Temp, Poti .... + /* Configure Regular Channel (PB12, Potentiometer) + */ + sConfig.Channel = ADC_CHANNEL_11; + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PB14, Temperature) + */ + sConfig.Channel = ADC_CHANNEL_5; + sConfig.Rank = ADC_REGULAR_RANK_4; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PB14, Temperature) + */ + sConfig.Channel = ADC_CHANNEL_1; + sConfig.Rank = ADC_REGULAR_RANK_5; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } /* USER CODE BEGIN ADC1_Init 2 */ /* USER CODE END ADC1_Init 2 */ 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 c22de049..31cf3b38 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 @@ -10,7 +10,7 @@ #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4096.0f -#define ADC_BUF_LEN_1 2 +#define ADC_BUF_LEN_1 5 #define ADC_BUF_LEN_2 1 static ADC_HandleTypeDef hadc1; @@ -37,6 +37,14 @@ float _readADCVoltageInline(const int pin, const void* cs_params){ else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[0]; #endif + + else if (pin == A_POTENTIOMETER) + raw_adc = adcBuffer1[2]; + else if (pin == A_TEMPERATURE) + raw_adc = adcBuffer1[3]; + else if (pin == A_VBUS) + raw_adc = adcBuffer1[4]; + return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } From f9f1d3ca041eb2a499456dbaae74a15cae44bcb5 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Oct 2022 08:17:34 +0200 Subject: [PATCH 37/42] hall sensor fix #192 --- src/sensors/HallSensor.cpp | 5 ++++- src/sensors/HallSensor.h | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 3d2ba42e..937169cb 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -116,7 +116,10 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + float vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 + if(vel < -velocity_max || vel > velocity_max) vel = 0f; //if velocity is out of range then make it zero + return vel; } } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 9e80ed2b..89de756a 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -76,6 +76,9 @@ class HallSensor: public Sensor{ // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) volatile long total_interrupts; + // variable used to filter outliers - rad/s + float velocity_max = 1000.0f + private: Direction decodeDirection(int oldState, int newState); From c0e6727cba86360ca2aa126af60920ec0546f6c9 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Oct 2022 08:25:07 +0200 Subject: [PATCH 38/42] Hall typo --- src/sensors/HallSensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 89de756a..d6b4493a 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -77,7 +77,7 @@ class HallSensor: public Sensor{ volatile long total_interrupts; // variable used to filter outliers - rad/s - float velocity_max = 1000.0f + float velocity_max = 1000.0f; private: From 6d93dcadfbad20a45ea3727d60e236310d71ff10 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Oct 2022 08:26:59 +0200 Subject: [PATCH 39/42] Hall typo in cpp --- src/sensors/HallSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 937169cb..6c2367ff 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -118,7 +118,7 @@ float HallSensor::getVelocity(){ } else { float vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 - if(vel < -velocity_max || vel > velocity_max) vel = 0f; //if velocity is out of range then make it zero + if(vel < -velocity_max || vel > velocity_max) vel = 0.0f; //if velocity is out of range then make it zero return vel; } From f4687c6e3f2f72ff461d9f50d2a506e01c553389 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Oct 2022 07:34:37 +0200 Subject: [PATCH 40/42] spacing issues + stepper sensor align update --- src/BLDCMotor.cpp | 4 ++-- src/StepperMotor.cpp | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 1909eb67..6f4eb12d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -183,7 +183,7 @@ int BLDCMotor::alignSensor() { for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); - sensor->update(); + sensor->update(); _delay(2); } // take and angle in the middle @@ -193,7 +193,7 @@ int BLDCMotor::alignSensor() { for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); - sensor->update(); + sensor->update(); _delay(2); } sensor->update(); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 8c557cfe..9dccd1ea 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -141,6 +141,7 @@ int StepperMotor::alignSensor() { 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 @@ -150,6 +151,7 @@ int StepperMotor::alignSensor() { 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(); From 091a98d76570f18a950cce81ba4723719f6fb6af Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Oct 2022 07:35:39 +0200 Subject: [PATCH 41/42] readme prepare for v2.2.3 --- README.md | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 75eadf44..3a1b3571 100644 --- a/README.md +++ b/README.md @@ -24,25 +24,31 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -

NEWS 📢: SimpleFOClibrary has been published in the Journal of Open Source Software

-

- SimpleFOC: A Field Oriented Control (FOC) Library for Controlling Brushless Direct Current (BLDC) and Stepper Motors.
- A. Skuric, HS. Bank, R. Unger, O. Williams, D. González-Reyes
-Journal of Open Source Software, 7(74), 4232, https://doi.org/10.21105/joss.04232 -

- -> FUTURE RELEASE : SimpleFOClibrary v2.2.3 +> NEW RELEASE 📢 : SimpleFOClibrary v2.2.3 > - stm32 low-side current sensing > - g4 supported -> - thoroughly tested f1/f4/g4 +> - thoroughly tested f1/f4/g4 - [#187](https://github.com/simplefoc/Arduino-FOC/issues/187) +> - bg431b: added support for VBAT and TEMPERATURE readings [#222](https://github.com/simplefoc/Arduino-FOC/pull/222) > - bugfixing > - leonardo -> - mega2560 -> - inline current sense without driver #188 -> - `initFOC` fails if current sense not initialised -> - driver and cs have to be well initialised for `initFOC` to start -> - `cs.init()` and `driver.init()` return `1` if well initialised and `0` if failed -## Arduino *SimpleFOClibrary* v2.2.2 +> - mega2560 [#190](https://github.com/simplefoc/Arduino-FOC/issues/190) +> - inline current sense without driver [#188](https://github.com/simplefoc/Arduino-FOC/issues/188) +> - bg431b support current sense fix [#210](https://github.com/simplefoc/Arduino-FOC/pull/210) +> - StepperDriver4PWM wrong init [#182](https://github.com/simplefoc/Arduino-FOC/issues/182) +> - open loop back-emf vlotage issue [#219](https://github.com/simplefoc/Arduino-FOC/issues/219) +> - SAMD51 compile issue [#217](https://github.com/simplefoc/Arduino-FOC/issues/217) +> - ESP32-S3 compile issue [#198](https://github.com/simplefoc/Arduino-FOC/issues/198) +> - ESP32 compile issue [#208](https://github.com/simplefoc/Arduino-FOC/issues/208), [#207](https://github.com/simplefoc/Arduino-FOC/issues/207) +> - magnetic sensor direction finding more robust [#173](https://github.com/simplefoc/Arduino-FOC/issues/173), [#164](https://github.com/simplefoc/Arduino-FOC/pull/164) +> - `StepDirListener` improved timing [#169](https://github.com/simplefoc/Arduino-FOC/issues/169), [#209](https://github.com/simplefoc/Arduino-FOC/pull/209) +> - API changes +> - `setPhaseVoltage` is now public function +> - `getVelocity` can now be called as many times as necessary (it recalculates the velocity if the time between calls is longer then `minDeltaT` - default 0.1ms) +> - BG431 board can be used only with `LowsideCurrentSense` class! +> - `initFOC` fails if current sense not initialised +> - driver and curent sense have to be well initialised for `initFOC` to start +> - `cs.init()` and `driver.init()` return `1` if well initialised and `0` if failed +## Arduino *SimpleFOClibrary* v2.2.3

From 8c65a5f7e37f59cce64865784a0d0e09d007cf71 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 15 Oct 2022 11:02:08 +0200 Subject: [PATCH 42/42] minDeltaT to min_elapsed_time --- src/common/base_classes/Sensor.cpp | 2 +- src/common/base_classes/Sensor.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index f3b19053..f806cddf 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -18,7 +18,7 @@ void Sensor::update() { float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; - if (Ts