From 87279080e2b2d5aa25149e90df65775244eb5204 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 24 Sep 2023 17:59:32 +0200 Subject: [PATCH 01/65] refactoring of FOC --- src/common/base_classes/CurrentSense.cpp | 67 +++++++++++------------- src/common/base_classes/CurrentSense.h | 13 ++++- src/common/foc_utils.h | 6 +++ 3 files changed, 47 insertions(+), 39 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 609162c2..bbe5edc7 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -7,33 +7,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); - // currnet sign - if motor angle not provided the magnitude is always positive - float sign = 1; - + // calculate clarke transform - float i_alpha, i_beta; - if(!current.c){ - // if only two measured currents - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ - // if only two measured currents - float a = -current.c - current.b; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ - // if only two measured currents - float b = -current.a - current.c; - i_alpha = current.a; - i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b; - }else{ - // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed. - float mid = (1.f/3) * (current.a + current.b + current.c); - float a = current.a - mid; - float b = current.b - mid; - i_alpha = a; - i_beta = _1_SQRT3 * a + _2_SQRT3 * b; - } + ABCurrent_s ABcurrent = getABCurrents(current); + + // current sign - if motor angle not provided the magnitude is always positive + float sign = 1; // if motor angle provided function returns signed value of the current // determine the sign of the current @@ -42,20 +21,38 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ float ct; float st; _sincos(motor_electrical_angle, &st, &ct); - sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1; + sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1; } // return current magnitude - return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); + return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta); } // function used with the foc algorihtm // calculating DQ currents from phase currents // - function calculating park and clarke transform of the phase currents -// - using getPhaseCurrents internally +// - using getPhaseCurrents and getABCurrents internally DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ // read current phase currents PhaseCurrent_s current = getPhaseCurrents(); + // calculate clarke transform + ABCurrent_s ABcurrent = getABCurrents(current); + + // calculate park transform + float ct; + float st; + _sincos(angle_el, &st, &ct); + DQCurrent_s return_current; + return_current.d = ABcurrent.alpha * ct + ABcurrent.beta * st; + return_current.q = ABcurrent.beta * ct - ABcurrent.alpha * st; + return return_current; +} + +// function used with the foc algorihtm +// calculating Alpha Beta currents from phase currents +// - function calculating Clarke transform of the phase currents +ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ + // calculate clarke transform float i_alpha, i_beta; if(!current.c){ @@ -81,14 +78,10 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } - // calculate park transform - float ct; - float st; - _sincos(angle_el, &st, &ct); - DQCurrent_s return_current; - return_current.d = i_alpha * ct + i_beta * st; - return_current.q = i_beta * ct - i_alpha * st; - return return_current; + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = i_alpha; + return_ABcurrent.beta = i_beta; + return return_ABcurrent; } /** diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad9f926d..5292988b 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -53,7 +53,7 @@ class CurrentSense{ virtual PhaseCurrent_s getPhaseCurrents() = 0; /** * Function reading the magnitude of the current set to the motor - * It returns the abosolute or signed magnitude if possible + * It returns the absolute or signed magnitude if possible * It can receive the motor electrical angle to help with calculation * This function is used with the current control (not foc) * @@ -62,13 +62,22 @@ class CurrentSense{ virtual float getDCCurrent(float angle_el = 0); /** - * Function used for FOC contorl, it reads the DQ currents of the motor + * Function used for FOC control, it reads the DQ currents of the motor * It uses the function getPhaseCurrents internally * * @param angle_el - motor electrical angle */ DQCurrent_s getFOCCurrents(float angle_el); + /** + * Function used for Clarke transform in FOC control + * It reads the phase currents of the motor + * It returns the alpha and beta currents + * + * @param current - phase current + */ + ABCurrent_s getABCurrents(PhaseCurrent_s current); + }; diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 0efe3b59..ac1ae458 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -56,6 +56,12 @@ struct DQVoltage_s float d; float q; }; +// alpha beta current structure +struct ABCurrent_s +{ + float alpha; + float beta; +}; /** From 7374f7a4f3297f51a2d67123cefd10d9bf7a2b39 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 24 Sep 2023 23:35:48 +0200 Subject: [PATCH 02/65] Add getDQCurrents function for Park transform --- src/common/base_classes/CurrentSense.cpp | 22 ++++++++++++++++------ src/common/base_classes/CurrentSense.h | 9 +++++++++ 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index bbe5edc7..c9394ad0 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -39,12 +39,8 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ ABCurrent_s ABcurrent = getABCurrents(current); // calculate park transform - float ct; - float st; - _sincos(angle_el, &st, &ct); - DQCurrent_s return_current; - return_current.d = ABcurrent.alpha * ct + ABcurrent.beta * st; - return_current.q = ABcurrent.beta * ct - ABcurrent.alpha * st; + DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el); + return return_current; } @@ -84,6 +80,20 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ return return_ABcurrent; } +// function used with the foc algorihtm +// calculating D and Q currents from Alpha Beta currents and electrical angle +// - function calculating Clarke transform of the phase currents +DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ + // calculate park transform + float ct; + float st; + _sincos(angle_el, &st, &ct); + DQCurrent_s return_current; + return_current.d = current.alpha * ct + current.beta * st; + return_current.q = current.beta * ct - current.alpha * st; + return return_current; +} + /** Driver linking to the current sense */ diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 5292988b..d9cd8c56 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -78,6 +78,15 @@ class CurrentSense{ */ ABCurrent_s getABCurrents(PhaseCurrent_s current); + /** + * Function used for Park transform in FOC control + * It reads the Alpha Beta currents and electircal angle of the motor + * It returns the D and Q currents + * + * @param current - phase current + */ + DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el); + }; From 655c099a0bfd679ab8249ffb684fc86093c7183d Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sat, 21 Oct 2023 15:58:00 +0200 Subject: [PATCH 03/65] Move Ualpha and Ubeta to FOCMotor --- src/BLDCMotor.h | 3 +-- src/common/base_classes/FOCMotor.h | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index a1f196ab..a7155c1f 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -69,8 +69,7 @@ class BLDCMotor: public FOCMotor void move(float target = NOT_SET) override; float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor - float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 318be99a..d4aa152d 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -161,6 +161,8 @@ class FOCMotor DQVoltage_s voltage;//!< current d and q voltage set to the motor DQCurrent_s current;//!< current d and q current measured float voltage_bemf; //!< estimated backemf voltage (if provided KV constant) + float Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + // motor configuration parameters float voltage_sensor_align;//!< sensor and motor align voltage parameter From bfa492c3e0c27912e6fab1222b5f850a836bee41 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Mon, 30 Oct 2023 21:57:14 +0100 Subject: [PATCH 04/65] Initialize Ualpha and Ubeta --- src/StepperMotor.h | 2 -- src/common/base_classes/FOCMotor.cpp | 4 ++++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7eda3167..45d20c63 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -73,8 +73,6 @@ class StepperMotor: public FOCMotor */ void move(float target = NOT_SET) override; - float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1427bcf..c78db959 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -32,6 +32,10 @@ FOCMotor::FOCMotor() // voltage bemf voltage_bemf = 0; + + // Initialize phase voltages U alpha and U beta used for inverse Park and Clarke transform + Ualpha = 0; + Ubeta = 0; //monitor_port monitor_port = nullptr; From 716ad1dbddf13bff928a535cf861e1becdd027ec Mon Sep 17 00:00:00 2001 From: Candas1 Date: Thu, 16 Nov 2023 09:13:25 +0000 Subject: [PATCH 05/65] ADC Calibration for g4 and l4 --- .../hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp | 4 ++++ .../hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp | 4 ++++ 2 files changed, 8 insertions(+) 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 ae96bad7..35689d8f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -61,6 +61,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + // start the adc HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); // restart all the timers of the driver diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index edac6414..ed229606 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -61,6 +61,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + + // Start the adc calibration + HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); + // start the adc HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); // restart all the timers of the driver From 5c724d9aedc555b4291fb49107f603b6c8795280 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Thu, 16 Nov 2023 10:09:35 +0000 Subject: [PATCH 06/65] build flag for use adc interrupt on f1/f4/g4/l4 --- .../stm32/stm32f1/stm32f1_hal.cpp | 4 ++++ .../stm32/stm32f1/stm32f1_mcu.cpp | 18 ++++++++++++++---- .../stm32/stm32f4/stm32f4_hal.cpp | 4 ++++ .../stm32/stm32f4/stm32f4_mcu.cpp | 19 +++++++++++++++---- .../stm32/stm32g4/stm32g4_hal.cpp | 5 ++++- .../stm32/stm32g4/stm32g4_mcu.cpp | 17 ++++++++++++++--- .../stm32/stm32l4/stm32l4_hal.cpp | 5 ++++- .../stm32/stm32l4/stm32l4_mcu.cpp | 19 +++++++++++++++---- 8 files changed, 74 insertions(+), 17 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 27e11958..ec75ef4f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -124,9 +124,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); } + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + #endif cs_params->adc_handle = &hadc; @@ -151,6 +153,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -158,5 +161,6 @@ extern "C" { } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 51b494f4..3a97dc54 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -69,9 +69,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); - + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -81,13 +85,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -104,5 +113,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index 68a9d094..dcf32137 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -135,9 +135,11 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT // enable interrupt HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif cs_params->adc_handle = &hadc; return 0; @@ -160,11 +162,13 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index 60344781..3a7b131c 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -58,8 +58,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); - // start the adc + + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -69,13 +74,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -92,5 +102,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 91322112..bf89463b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -181,7 +181,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT if(hadc.Instance == ADC1) { // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); @@ -214,6 +214,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC5_IRQn); } +#endif #endif cs_params->adc_handle = &hadc; @@ -237,6 +238,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -263,5 +265,6 @@ extern "C" { } #endif } +#endif #endif \ No newline at end of file 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 35689d8f..4e1aa9ac 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -66,7 +66,12 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + // restart all the timers of the driver _startTimers(driver_params->timers, 6); } @@ -75,13 +80,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -98,5 +108,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index bfec2175..688ba796 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -180,7 +180,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT if(hadc.Instance == ADC1) { // enable interrupt HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); @@ -213,6 +213,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); HAL_NVIC_EnableIRQ(ADC5_IRQn); } +#endif #endif cs_params->adc_handle = &hadc; @@ -236,6 +237,7 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -262,5 +264,6 @@ extern "C" { } #endif } +#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index ed229606..d1ca2814 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -65,8 +65,13 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); - // start the adc + // start the adc + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + // restart all the timers of the driver _startTimers(driver_params->timers, 6); } @@ -75,13 +80,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ - if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer - return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } } return 0; } - +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -98,5 +108,6 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } +#endif #endif \ No newline at end of file From 72cdd21f02d77a02e8d9e3198fc5a0ee41291f75 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 19 Nov 2023 19:03:42 +0100 Subject: [PATCH 07/65] Update CurrentSense.cpp Fix Clarke transform for dual shunt --- src/common/base_classes/CurrentSense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index c9394ad0..c57bdfe6 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -55,12 +55,12 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ + }else if(!current.a){ // if only two measured currents float a = -current.c - current.b; i_alpha = a; i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b; - }if(!current.b){ + }else if(!current.b){ // if only two measured currents float b = -current.a - current.c; i_alpha = current.a; @@ -99,4 +99,4 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ */ void CurrentSense::linkDriver(BLDCDriver* _driver) { driver = _driver; -} \ No newline at end of file +} From d4e1c2cd04ed19552e9f488c3a3e3ccbfef89c21 Mon Sep 17 00:00:00 2001 From: nmscode <105442557+nmscode@users.noreply.github.com> Date: Mon, 27 Nov 2023 16:48:28 -0500 Subject: [PATCH 08/65] Remove sqrt 2 multiplication and correct bemf calculation --- src/BLDCMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8dc2f5cd..16534199 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -46,7 +46,7 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance) // 1/sqrt(2) - rms value KV_rating = NOT_SET; if (_isset(_KV)) - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -396,7 +396,7 @@ void BLDCMotor::move(float new_target) { if(_isset(new_target)) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; From afdbd9dba8f4e31af71afa5e4c07b35a19f9b185 Mon Sep 17 00:00:00 2001 From: nmscode <105442557+nmscode@users.noreply.github.com> Date: Mon, 27 Nov 2023 16:52:13 -0500 Subject: [PATCH 09/65] apply fix to stepper motor logic as well --- src/StepperMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 7831f0bf..6cbe417a 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -16,7 +16,7 @@ StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance) phase_resistance = _R; // save back emf constant KV = 1/K_bemf // usually used rms - KV_rating = _KV*_SQRT2; + KV_rating = _KV; // save phase inductance phase_inductance = _inductance; @@ -292,7 +292,7 @@ void StepperMotor::move(float new_target) { if(_isset(new_target) ) target = new_target; // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV) - if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS; + if (_isset(KV_rating)) voltage_bemf = shaft_velocity/(KV_rating*_SQRT3)/_RPM_TO_RADS; // estimate the motor current if phase reistance available and current_sense not available if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance; From 8ea87abfe350c09e02344c46cfd9f3fc9de432f8 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 2 Dec 2023 21:42:31 +0100 Subject: [PATCH 10/65] timeout configurable in MagneticSensorPWM #348 --- src/sensors/MagneticSensorPWM.cpp | 2 +- src/sensors/MagneticSensorPWM.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index cf211644..04b7cc75 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -85,7 +85,7 @@ float MagneticSensorPWM::getSensorAngle(){ int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse - pulse_length_us = pulseIn(pinPWM, HIGH, 1200); // 1200us timeout, should this be configurable? + pulse_length_us = pulseIn(pinPWM, HIGH, timeout_us); // 1200us timeout, should this be configurable? } return pulse_length_us; } diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 48492c84..a5fd7e6e 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -44,6 +44,8 @@ class MagneticSensorPWM: public Sensor{ void enableInterrupt(void (*doPWM)()); unsigned long pulse_length_us; + unsigned int timeout_us = 1200; + private: // raw count (typically in range of 0-1023) int raw_count; From ef580d79ebc5ae7fb913584b2bb923e9df2b9775 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 17 Dec 2023 11:12:28 +0100 Subject: [PATCH 11/65] Prevent double constants --- src/common/base_classes/Sensor.cpp | 2 +- src/common/foc_utils.cpp | 2 +- src/current_sense/InlineCurrentSense.cpp | 12 ++++++------ src/current_sense/LowsideCurrentSense.cpp | 12 ++++++------ 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index b0a8f4db..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,7 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time - float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; vel_full_rotations = full_rotations; diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 233bd246..0809b137 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256 /_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 492b3ac9..c3db74ef 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -156,9 +156,9 @@ int InlineCurrentSense::driverAlign(float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -203,9 +203,9 @@ int InlineCurrentSense::driverAlign(float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index a706beeb..aeb8dea0 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -157,9 +157,9 @@ int LowsideCurrentSense::driverAlign(float voltage){ // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); @@ -204,9 +204,9 @@ int LowsideCurrentSense::driverAlign(float voltage){ // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4f*c1.a; - c.b = c.b*0.6 + 0.4f*c1.b; - c.c = c.c*0.6 + 0.4f*c1.c; + c.a = c.a*0.6f + 0.4f*c1.a; + c.b = c.b*0.6f + 0.4f*c1.b; + c.c = c.c*0.6f + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); From b0d7daafbfb8589ed0d92795dd82d6c3f9ff3a85 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 11:13:34 +0100 Subject: [PATCH 12/65] fix double constants to be floats --- src/common/base_classes/Sensor.cpp | 2 +- src/common/defaults.h | 2 +- src/common/foc_utils.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index b0a8f4db..db17e92e 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -19,7 +19,7 @@ void Sensor::update() { /** get current angular velocity (rad/s) */ float Sensor::getVelocity() { // calculate sample time - float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; + float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; vel_full_rotations = full_rotations; diff --git a/src/common/defaults.h b/src/common/defaults.h index c0a46182..ac57daa1 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -43,7 +43,7 @@ // align voltage #define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant +#define DEF_VEL_FILTER_Tf 0.005f //!< default velocity filter time constant // current sense default parameters #define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 233bd246..fa937d15 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { From 5f259f7944cc55fe389fffb72b62c3b58e69ec0f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 11:13:58 +0100 Subject: [PATCH 13/65] prepare files for next release version --- README.md | 13 ++----------- library.properties | 2 +- 2 files changed, 3 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 75475bec..7d8150d0 100644 --- a/README.md +++ b/README.md @@ -25,17 +25,8 @@ 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) -> NEW RELEASE 📢 : SimpleFOClibrary v2.3.2 -> - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1) -> - Bugfix for stepper motor initialization -> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning -> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321) -> - Added teensy3/4 compile to the github CI using platformio -> - Fix compile issues with recent versions of ESP32 framework -> - Add ADC calibration on STM32 MCUs -> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346) -> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322) -> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1) +> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 +> - And more bugfixes - see the complete list of 2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) ## Arduino *SimpleFOClibrary* v2.3.2 diff --git a/library.properties b/library.properties index 7242d57c..0a2606b2 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.3.2 +version=2.3.3 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors From b7300fe356e854b58a67f2a25efe9c1a4230b35e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 17 Dec 2023 22:39:11 +0100 Subject: [PATCH 14/65] Add library.json to set libArchive #357 --- library.json | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 library.json diff --git a/library.json b/library.json new file mode 100644 index 00000000..25e2a353 --- /dev/null +++ b/library.json @@ -0,0 +1,5 @@ +{ + "build": { + "libArchive": false + } +} From 4934aae508dff2c2fea1ce129cbc3a58ec5cd124 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 7 Jan 2024 21:22:49 +0100 Subject: [PATCH 15/65] add currentsense.enable/disable --- src/BLDCMotor.cpp | 9 +++++++++ src/common/base_classes/CurrentSense.cpp | 9 +++++++++ src/common/base_classes/CurrentSense.h | 10 ++++++++++ 3 files changed, 28 insertions(+) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 16534199..8fd54ba6 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -111,6 +111,8 @@ void BLDCMotor::init() { // disable motor driver void BLDCMotor::disable() { + // disable the current sense + if(current_sense) current_sense->disable(); // set zero to PWM driver->setPwm(0, 0, 0); // disable the driver @@ -125,6 +127,13 @@ void BLDCMotor::enable() driver->enable(); // set zero to PWM driver->setPwm(0, 0, 0); + // enable the current sense + if(current_sense) current_sense->enable(); + // reset the pids + PID_velocity.reset(); + P_angle.reset(); + PID_current_q.reset(); + PID_current_d.reset(); // motor status update enabled = 1; } diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index c57bdfe6..b732c48e 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -100,3 +100,12 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ void CurrentSense::linkDriver(BLDCDriver* _driver) { driver = _driver; } + + +void enable(){ + // nothing is done here, but you can override this function +}; + +void disable(){ + // nothing is done here, but you can override this function +}; \ No newline at end of file diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index d9cd8c56..1c839053 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -87,7 +87,17 @@ class CurrentSense{ */ DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el); + /** + * enable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void enable(); + /** + * disable the current sense. default implementation does nothing, but you can + * override it to do something useful. + */ + virtual void disable(); }; #endif \ No newline at end of file From 9db53cdd17e6365bf6ab0e484659a636d9446578 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 9 Jan 2024 12:11:58 +0100 Subject: [PATCH 16/65] add methods to class correctly --- src/common/base_classes/CurrentSense.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index b732c48e..03ea19ea 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -102,10 +102,10 @@ void CurrentSense::linkDriver(BLDCDriver* _driver) { } -void enable(){ +void CurrentSense::enable(){ // nothing is done here, but you can override this function }; -void disable(){ +void CurrentSense::disable(){ // nothing is done here, but you can override this function }; \ No newline at end of file From 71ce4b1b95891eb5d9103a2c5f397be786176bdc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 9 Jan 2024 12:16:14 +0100 Subject: [PATCH 17/65] solve AVR compiler warnings --- .../hardware_specific/atmega/atmega2560_mcu.cpp | 12 ++++++++---- .../hardware_specific/atmega/atmega328_mcu.cpp | 12 ++++++++---- .../hardware_specific/atmega/atmega32u4_mcu.cpp | 12 ++++++++---- 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp index 8a7fbbec..532b3ce3 100644 --- a/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp @@ -57,7 +57,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -75,7 +76,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -94,7 +96,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; // _syncAllTimers(); return params; @@ -141,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp index 53fb1086..9df9b8a7 100644 --- a/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp @@ -53,7 +53,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -70,7 +71,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -89,7 +91,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC, pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; _syncAllTimers(); return params; @@ -141,7 +144,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B,pwm_frequency); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } diff --git a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp index b4ad3101..4cf454ae 100644 --- a/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp @@ -41,7 +41,8 @@ void* _configure1PWM(long pwm_frequency,const int pinA) { _pinHighFrequency(pinA); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -56,7 +57,8 @@ void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { _pinHighFrequency(pinB); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -72,7 +74,8 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in _pinHighFrequency(pinC); GenericDriverParams* params = new GenericDriverParams { .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } @@ -119,7 +122,8 @@ void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const _pinHighFrequency(pin2B); GenericDriverParams* params = new GenericDriverParams { .pins = { pin1A, pin1B, pin2A, pin2B }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .dead_zone = 0.0f }; return params; } From 3bcb9af9fc4564d3cb998d3a83ee8a381ee69e23 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 20 Jan 2024 23:32:25 +0100 Subject: [PATCH 18/65] expose result of PP check --- src/BLDCMotor.cpp | 3 ++- src/StepperMotor.cpp | 3 ++- src/common/base_classes/FOCMotor.h | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 8fd54ba6..e0f5415d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -259,7 +259,8 @@ int BLDCMotor::alignSensor() { sensor_direction = Direction::CW; } // check pole pair number - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 6cbe417a..697ed277 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -175,7 +175,8 @@ int StepperMotor::alignSensor() { } // check pole pair number float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! + pp_check_result = !(fabs(moved*pole_pairs - _2PI) > 0.5f); // 0.5f is arbitrary number it can be lower or higher! + if( pp_check_result==false ) { SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved); } else { SIMPLEFOC_DEBUG("MOT: PP check: OK!"); diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 318be99a..b5b0a557 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -206,6 +206,7 @@ class FOCMotor float sensor_offset; //!< user defined sensor zero offset float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration + bool pp_check_result = false; //!< the result of the PP check, if run during loopFOC /** * Function providing BLDCMotor class with the From b2da2b96822f466221b04715964add00d5457efb Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 21 Jan 2024 00:45:00 +0100 Subject: [PATCH 19/65] respect pin nrs on boards which remap them --- src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp index eee5797b..f187fbef 100644 --- a/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp @@ -2,6 +2,10 @@ /** * Support for the RP2040 MCU, as found on the Raspberry Pi Pico. */ + +#include "./rp2040_mcu.h" + + #if defined(TARGET_RP2040) @@ -13,9 +17,9 @@ #define SIMPLEFOC_DEBUG_RP2040 #include "../../hardware_api.h" -#include "./rp2040_mcu.h" #include "hardware/pwm.h" #include "hardware/clocks.h" +#include #define _PWM_FREQUENCY 24000 #define _PWM_FREQUENCY_MAX 66000 @@ -30,7 +34,8 @@ uint16_t wrapvalues[NUM_PWM_SLICES]; // TODO add checks which channels are already used... -void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { +void setupPWM(int pin_nr, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) { + uint pin = (uint)digitalPinToPinName(pin_nr); // we could check for -DBOARD_HAS_PIN_REMAP ? gpio_set_function(pin, GPIO_FUNC_PWM); uint slice = pwm_gpio_to_slice_num(pin); uint chan = pwm_gpio_to_channel(pin); @@ -45,7 +50,7 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1; #ifdef SIMPLEFOC_DEBUG_RP2040 SimpleFOCDebug::print("Configuring pin "); - SimpleFOCDebug::print(pin); + SimpleFOCDebug::print((int)pin); SimpleFOCDebug::print(" slice "); SimpleFOCDebug::print((int)slice); SimpleFOCDebug::print(" channel "); From 50fc6de70c17bcc4fba00a0347df0596bebf2ffc Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Sun, 21 Jan 2024 11:21:28 +0100 Subject: [PATCH 20/65] initFOC: remove unnecessary delay --- src/BLDCMotor.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index e0f5415d..9dbc0a48 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -151,7 +151,6 @@ int BLDCMotor::initFOC() { // alignment necessary for encoders! // sensor and motor alignment - can be skipped // by setting motor.sensor_direction and motor.zero_electric_angle - _delay(500); if(sensor){ exit_flag *= alignSensor(); // added the shaft_angle update @@ -165,7 +164,6 @@ int BLDCMotor::initFOC() { // aligning the current sensor - can be skipped // checks if driver phases are the same as current sense phases // and checks the direction of measuremnt. - _delay(500); if(exit_flag){ if(current_sense){ if (!current_sense->initialized) { From 813917a3ed68485f0ee5646869403b306363416a Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 26 Jan 2024 18:08:19 +0100 Subject: [PATCH 21/65] Force the use of the ADC_INJECTED_RANK_x instead of the index --- .../hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp | 6 ++++-- .../hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp | 4 +++- .../hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp | 5 ++++- .../hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp | 4 +++- 4 files changed, 14 insertions(+), 5 deletions(-) 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 3a97dc54..69fea017 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -88,8 +88,10 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } 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 3a7b131c..a3ee5407 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -78,7 +78,9 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } 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 4e1aa9ac..1a954105 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -11,6 +11,7 @@ #include "stm32g4_utils.h" #include "Arduino.h" +// #define SIMPLEFOC_STM32_ADC_INTERRUPT #define _ADC_VOLTAGE_G4 3.3f #define _ADC_RESOLUTION_G4 4096.0f @@ -84,7 +85,9 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index d1ca2814..9dea0933 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -84,7 +84,9 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #else - return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,i+1) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; #endif } } From 36e3da807c0d1edb17b80474524af37267a3cb35 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 26 Jan 2024 21:55:52 +0100 Subject: [PATCH 22/65] fix floating point constant incorrectly converted to int --- src/common/foc_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index fa937d15..c938d363 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -9,7 +9,7 @@ __attribute__((weak)) float _sin(float a){ // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; - unsigned int i = (unsigned int)(a * (64*4*256/_2PI)); + unsigned int i = (unsigned int)(a * (64*4*256.0f/_2PI)); int t1, t2, frac = i & 0xff; i = (i >> 8) & 0xff; if (i < 64) { From a49da451503356a2bb3443303b7e2751dcc979c1 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 28 Jan 2024 14:23:56 +0100 Subject: [PATCH 23/65] initial version of comprehensive timer synchornisation for truly center aligned PWM for stm32 (when using more than one timer) --- .../hardware_specific/stm32/stm32_mcu.cpp | 118 +++++++++++++++--- 1 file changed, 99 insertions(+), 19 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 65dad9f4..db7429ac 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -4,6 +4,7 @@ #if defined(_STM32_DEF_) +#define SIMPLEFOC_STM32_DEBUG #pragma message("") #pragma message("SimpleFOC: compiling for STM32") #pragma message("") @@ -204,25 +205,43 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) } } -// align the timers to end the init -void _startTimers(HardwareTimer **timers_to_start, int timer_num) -{ - // TODO - sart each timer only once - // sart timers - for (int i=0; i < timer_num; i++) { - if(timers_to_start[i] == NP) return; - timers_to_start[i]->resume(); - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); - #endif - } + +// function finds the appropriate timer source trigger for the master timer index provided +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM8 +int _getTriggerSourceRegister(HardwareTimer* timer) { + #if defined(TIM1) && TIM_TS_ITR0 + if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + #endif + #if defined(TIM2) && TIM_TS_ITR1 + if (timer->getHandle()->Instance == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + #endif + #if defined(TIM3) && TIM_TS_ITR2 + if (timer->getHandle()->Instance == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + #endif + #if defined(TIM4) && TIM_TS_ITR3 + if (timer->getHandle()->Instance == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + #endif + #if defined(TIM5) && TIM_TS_ITR4 + if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + #endif + #if defined(TIM6) && TIM_TS_ITR5 + if (timer->getHandle()->Instance == TIM6) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + #endif + #if defined(TIM7) && TIM_TS_ITR6 + if (timer->getHandle()->Instance == TIM7) return LL_TIM_TS_ITR6;//return TIM_TS_ITR6; + #endif + #if defined(TIM8) && TIM_TS_ITR7 + if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR7;// return TIM_TS_ITR7; + #endif + return -1; } void _alignTimersNew() { int numTimers = 0; HardwareTimer *timers[numTimerPinsUsed]; - // reset timer counters + // find the timers used for (int i=0; iperipheral); HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); @@ -237,6 +256,55 @@ void _alignTimersNew() { timers[numTimers++] = timer; } + // see if there is more then 1 timers used for the pwm + // if yes, try to align timers + if(numTimers > 1){ + // find the master timer + uint8_t masterTimerIndex = 0; + int triggerEvent = -1; + for (int i=0; igetHandle()->Instance)) { + // check if timer already configured in TRGO update mode (used for ADC triggering) + // in that case we should not change its TRGO configuration + if(timers[i]->getHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; + // check if it has the supported internal trigger + triggerEvent = _getTriggerSourceRegister(timers[i]); + if(triggerEvent == -1) continue; // not supported keep searching + masterTimerIndex = i; // found the master timer + break; + } + } + + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: aligning!"); + #endif + + // if no master timer found do not perform alignment + if (triggerEvent == -1) { + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: ERR: No master timer found, cannot align timers!"); + #endif + }else{ + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Aligning PWM to master timer: ", getTimerNumber(get_timer_index(timers[masterTimerIndex]->getHandle()->Instance))); + #endif + // make the master timer generate ITRGx event + // if it was already configured in slave mode + LL_TIM_SetSlaveMode(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); + // Configure the master timer to send a trigger signal on enable + LL_TIM_SetTriggerOutput(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); + // configure other timers to get the input trigger from the master timer + for (int i=0; igetHandle()->Instance, triggerEvent); + LL_TIM_SetSlaveMode(timers[i]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); + } + } + } + // enable timer clock for (int i=0; ipause(); @@ -254,6 +322,20 @@ void _alignTimersNew() { +// align the timers to end the init +void _startTimers(HardwareTimer **timers_to_start, int timer_num) +{ + // // TODO - start each timer only once + // // start timers + // for (int i=0; i < timer_num; i++) { + // if(timers_to_start[i] == NP) return; + // timers_to_start[i]->resume(); + // #ifdef SIMPLEFOC_STM32_DEBUG + // SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + // #endif + // } + _alignTimersNew(); +} // configure hardware 6pwm for a complementary pair of channels @@ -540,7 +622,7 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED; HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\ - // allign the timers + // align the timers _alignTimersNew(); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); @@ -598,6 +680,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { +TIM_MasterConfigTypeDef sMasterConfig; +TIM_SlaveConfigTypeDef sSlaveConfig; // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -620,7 +704,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); - + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -945,8 +1029,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif - - - - #endif From ce7a0327aba428fd570f016c2f311aa025f42ddb Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 28 Jan 2024 14:44:43 +0100 Subject: [PATCH 24/65] removed alignement message --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index db7429ac..a9d2b9ce 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -256,6 +256,10 @@ void _alignTimersNew() { timers[numTimers++] = timer; } + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers!\nTimer no. ", numTimers); + #endif + // see if there is more then 1 timers used for the pwm // if yes, try to align timers if(numTimers > 1){ @@ -276,9 +280,6 @@ void _alignTimersNew() { } } - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: aligning!"); - #endif // if no master timer found do not perform alignment if (triggerEvent == -1) { From adbc55a2a6b64f4dd7dce01c6012ee2f80c09b8e Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 29 Jan 2024 20:04:47 +0100 Subject: [PATCH 25/65] bugfix forgotten define check for TRGO events --- .../hardware_specific/stm32/stm32_mcu.cpp | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index a9d2b9ce..f9a2e847 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -10,8 +10,6 @@ #pragma message("") -//#define SIMPLEFOC_STM32_DEBUG - #ifdef SIMPLEFOC_STM32_DEBUG void printTimerCombination(int numPins, PinMap* timers[], int score); int getTimerNumber(int timerIndex); @@ -210,28 +208,28 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) // returns -1 if no trigger source is found // currently supports the master timers to be from TIM1 to TIM8 int _getTriggerSourceRegister(HardwareTimer* timer) { - #if defined(TIM1) && TIM_TS_ITR0 + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; #endif - #if defined(TIM2) && TIM_TS_ITR1 + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) if (timer->getHandle()->Instance == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; #endif - #if defined(TIM3) && TIM_TS_ITR2 + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) if (timer->getHandle()->Instance == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; #endif - #if defined(TIM4) && TIM_TS_ITR3 + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) if (timer->getHandle()->Instance == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; #endif - #if defined(TIM5) && TIM_TS_ITR4 + #if defined(TIM5) && defined(LL_TIM_TS_ITR4) if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; #endif - #if defined(TIM6) && TIM_TS_ITR5 + #if defined(TIM6) && defined(LL_TIM_TS_ITR5) if (timer->getHandle()->Instance == TIM6) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; #endif - #if defined(TIM7) && TIM_TS_ITR6 + #if defined(TIM7) && defined(LL_TIM_TS_ITR6) if (timer->getHandle()->Instance == TIM7) return LL_TIM_TS_ITR6;//return TIM_TS_ITR6; #endif - #if defined(TIM8) && TIM_TS_ITR7 + #if defined(TIM8) && defined(LL_TIM_TS_ITR7) if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR7;// return TIM_TS_ITR7; #endif return -1; @@ -256,9 +254,9 @@ void _alignTimersNew() { timers[numTimers++] = timer; } - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers!\nTimer no. ", numTimers); - #endif + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Syncronising timers! Timer no. ", numTimers); + #endif // see if there is more then 1 timers used for the pwm // if yes, try to align timers From 2d09173d03e4b498ad42afe540ff97c52932d5c5 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 1 Feb 2024 08:32:34 +0100 Subject: [PATCH 26/65] remove timers 6 and 7 --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index f9a2e847..035e0dbd 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -206,7 +206,7 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) // function finds the appropriate timer source trigger for the master timer index provided // returns -1 if no trigger source is found -// currently supports the master timers to be from TIM1 to TIM8 +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 int _getTriggerSourceRegister(HardwareTimer* timer) { #if defined(TIM1) && defined(LL_TIM_TS_ITR0) if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; @@ -223,14 +223,8 @@ int _getTriggerSourceRegister(HardwareTimer* timer) { #if defined(TIM5) && defined(LL_TIM_TS_ITR4) if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; #endif - #if defined(TIM6) && defined(LL_TIM_TS_ITR5) - if (timer->getHandle()->Instance == TIM6) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; - #endif - #if defined(TIM7) && defined(LL_TIM_TS_ITR6) - if (timer->getHandle()->Instance == TIM7) return LL_TIM_TS_ITR6;//return TIM_TS_ITR6; - #endif - #if defined(TIM8) && defined(LL_TIM_TS_ITR7) - if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR7;// return TIM_TS_ITR7; + #if defined(TIM8) && defined(LL_TIM_TS_ITR5) + if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; #endif return -1; } From 6aed7803de0bf0ef4a5c3ceff061bca224a629d6 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 1 Feb 2024 20:12:12 +0100 Subject: [PATCH 27/65] undate on the alignement logic --- .../hardware_specific/stm32/stm32_mcu.cpp | 123 +++++++++++++++--- 1 file changed, 102 insertions(+), 21 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 035e0dbd..e4305f41 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -204,30 +204,105 @@ void _stopTimers(HardwareTimer **timers_to_stop, int timer_num) } -// function finds the appropriate timer source trigger for the master timer index provided +#if defined(STM32G4xx) +// function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found // currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getTriggerSourceRegister(HardwareTimer* timer) { +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; #if defined(TIM1) && defined(LL_TIM_TS_ITR0) - if (timer->getHandle()->Instance == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; + if (TIM_master == TIM1) return LL_TIM_TS_ITR0;// return TIM_TS_ITR0; #endif #if defined(TIM2) && defined(LL_TIM_TS_ITR1) - if (timer->getHandle()->Instance == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; + else if (TIM_master == TIM2) return LL_TIM_TS_ITR1;//return TIM_TS_ITR1; #endif #if defined(TIM3) && defined(LL_TIM_TS_ITR2) - if (timer->getHandle()->Instance == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; + else if (TIM_master == TIM3) return LL_TIM_TS_ITR2;//return TIM_TS_ITR2; #endif #if defined(TIM4) && defined(LL_TIM_TS_ITR3) - if (timer->getHandle()->Instance == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; + else if (TIM_master == TIM4) return LL_TIM_TS_ITR3;//return TIM_TS_ITR3; #endif #if defined(TIM5) && defined(LL_TIM_TS_ITR4) - if (timer->getHandle()->Instance == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; + else if (TIM_master == TIM5) return LL_TIM_TS_ITR4;//return TIM_TS_ITR4; #endif #if defined(TIM8) && defined(LL_TIM_TS_ITR5) - if (timer->getHandle()->Instance == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; + else if (TIM_master == TIM8) return LL_TIM_TS_ITR5;//return TIM_TS_ITR5; #endif return -1; } +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) + +// function finds the appropriate timer source trigger for the master/slave timer combination +// returns -1 if no trigger source is found +// currently supports the master timers to be from TIM1 to TIM4 and TIM8 +int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { + // put master and slave in temp variables to avoid arrows + TIM_TypeDef *TIM_master = master->getHandle()->Instance; + TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; + #if defined(TIM1) && defined(LL_TIM_TS_ITR0) + if (TIM_master == TIM1){ + if(TIM_slave == TIM2 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR0; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM2) && defined(LL_TIM_TS_ITR1) + else if (TIM_master == TIM2){ + if(TIM_slave == TIM1 || TIM_slave == TIM3 || TIM_slave == TIM4) return LL_TIM_TS_ITR1; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR1; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR0; + #endif + } + #endif + #if defined(TIM3) && defined(LL_TIM_TS_ITR2) + else if (TIM_master == TIM3){ + if(TIM_slave== TIM1 || TIM_slave == TIM2 || TIM_slave == TIM4) return LL_TIM_TS_ITR2; + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM4) && defined(LL_TIM_TS_ITR3) + else if (TIM_master == TIM4){ + if(TIM_slave == TIM1 || TIM_slave == TIM2 || TIM_slave == TIM3) return LL_TIM_TS_ITR3; + #if defined(TIM8) + else if(TIM_slave == TIM8) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM5) + else if(TIM_slave == TIM5) return LL_TIM_TS_ITR1; + #endif + } + #endif + #if defined(TIM5) + else if (TIM_master == TIM5){ + #if !defined(STM32L4xx) // only difference between F4,F1 and L4 + if(TIM_slave == TIM1) return LL_TIM_TS_ITR0; + else if(TIM_slave == TIM3) return LL_TIM_TS_ITR2; + #endif + #if defined(TIM8) + if(TIM_slave == TIM8) return LL_TIM_TS_ITR3; + #endif + } + #endif + #if defined(TIM8) + else if (TIM_master == TIM8){ + if(TIM_slave==TIM2) return LL_TIM_TS_ITR1; + else if(TIM_slave ==TIM4 || TIM_slave ==TIM5) return LL_TIM_TS_ITR3; + } + #endif + return -1; // combination not supported +} +#else +// Alignment not supported for this architecture +int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { + return -1; +} +#endif + void _alignTimersNew() { int numTimers = 0; @@ -256,7 +331,7 @@ void _alignTimersNew() { // if yes, try to align timers if(numTimers > 1){ // find the master timer - uint8_t masterTimerIndex = 0; + int16_t master_index = -1; int triggerEvent = -1; for (int i=0; igetHandle()->Instance->CR2 & LL_TIM_TRGO_UPDATE) continue; - // check if it has the supported internal trigger - triggerEvent = _getTriggerSourceRegister(timers[i]); - if(triggerEvent == -1) continue; // not supported keep searching - masterTimerIndex = i; // found the master timer + // check if the timer has the supported internal trigger for other timers + for (int slave_i=0; slave_igetHandle()->Instance))); + SIMPLEFOC_DEBUG("STM32-DRV: Aligning PWM to master timer: ", getTimerNumber(get_timer_index(timers[master_index]->getHandle()->Instance))); #endif // make the master timer generate ITRGx event // if it was already configured in slave mode - LL_TIM_SetSlaveMode(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); + LL_TIM_SetSlaveMode(timers[master_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_DISABLED ); // Configure the master timer to send a trigger signal on enable - LL_TIM_SetTriggerOutput(timers[masterTimerIndex]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); + LL_TIM_SetTriggerOutput(timers[master_index]->getHandle()->Instance, LL_TIM_TRGO_ENABLE); // configure other timers to get the input trigger from the master timer - for (int i=0; igetHandle()->Instance, triggerEvent); - LL_TIM_SetSlaveMode(timers[i]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); + LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getTriggerSourceRegister(timers[master_index], timers[slave_index])); + LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); } } } From 3ad11eebafb13e3603b6e32dfbea8903cfd775c0 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 1 Feb 2024 20:17:49 +0100 Subject: [PATCH 28/65] typo function name --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index e4305f41..bfb25789 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -235,7 +235,7 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // // function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found // currently supports the master timers to be from TIM1 to TIM4 and TIM8 -int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // put master and slave in temp variables to avoid arrows TIM_TypeDef *TIM_master = master->getHandle()->Instance; TIM_TypeDef *TIM_slave = slave->getHandle()->Instance; @@ -298,7 +298,7 @@ int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { } #else // Alignment not supported for this architecture -int _getTriggerSourceRegister(HardwareTimer* master, HardwareTimer* slave) { +int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { return -1; } #endif @@ -343,7 +343,7 @@ void _alignTimersNew() { for (int slave_i=0; slave_igetHandle()->Instance, _getTriggerSourceRegister(timers[master_index], timers[slave_index])); + LL_TIM_SetTriggerInput(timers[slave_index]->getHandle()->Instance, _getInternalSourceTrigger(timers[master_index], timers[slave_index])); LL_TIM_SetSlaveMode(timers[slave_index]->getHandle()->Instance, LL_TIM_SLAVEMODE_TRIGGER); } } From b34bd626526acc0c66b8698ca46e15d17dead021 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 3 Feb 2024 16:41:25 +0100 Subject: [PATCH 29/65] initial support for teensy4 low-side current sensing (only for 6pwm at the motment) - in progress --- .../hardware_specific/teensy/teensy4_mcu.cpp | 191 ++++++++++++++++++ .../hardware_specific/teensy/teensy4_mcu.h | 77 +++++++ .../{ => teensy}/teensy_mcu.cpp | 2 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 14 +- 4 files changed, 273 insertions(+), 11 deletions(-) create mode 100644 src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp create mode 100644 src/current_sense/hardware_specific/teensy/teensy4_mcu.h rename src/current_sense/hardware_specific/{ => teensy}/teensy_mcu.cpp (94%) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp new file mode 100644 index 00000000..8f2b441c --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,191 @@ +#include "teensy4_mcu.h" +#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 +int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +volatile uint32_t val0, val1; + +void read_currents(uint32_t *a, uint32_t*b){ + *a = val0; + *b = val1; +} + +// interrupt service routine for the ADC_ETC0 +// reading the ADC values and clearing the interrupt +void adcetc0_isr() { + digitalWrite(30,HIGH); + ADC_ETC_DONE0_1_IRQ |= 1; // clear + val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; + val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; + asm("dsb"); + digitalWrite(30,LOW); +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc1_init() { + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + + //Calibration of ADC1 + ADC1_GC |= ADC_GC_CAL; // begin cal ADC1 + while (ADC1_GC & ADC_GC_CAL) ; + + ADC1_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC1_HC1 = 16; +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc2_init(){ + + // configuring ADC2 + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + //Calibration of ADC2 + ADC2_GC |= ADC_GC_CAL; // begin cal ADC2 + while (ADC2_GC & ADC_GC_CAL) ; + + ADC2_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC2_HC1 = 16; +} + +void adc_etc_init(int pin1, int pin2) { + ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST + ADC_ETC_CTRL = 0x40000001; // start with trigger 0 + ADC_ETC_TRIG0_CTRL = 0x100; // chainlength -1 + + // ADC1 7 8, chain channel, HWTS, IE, B2B + // pg 3516, section 66.5.1.8 + ADC_ETC_TRIG0_CHAIN_1_0 = + ADC_ETC_TRIG_CHAIN_IE1(0) | // no interrupt on first or set 2 if interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS1(1) | + ADC_ETC_TRIG_CHAIN_CSEL1(pin_to_channel[pin1]) | // ADC channel 8 + ADC_ETC_TRIG_CHAIN_IE0(1) | // interrupt when Done0 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin2]); // ADC channel 7 + + attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); + // use the second interrupt if necessary (for more than 2 channels) + // attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + // NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); +} + + +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + + GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; + float adc_voltage_conv = params->adc_voltage_conv; + if (pinA == params->pins[0]) { + return val0 * adc_voltage_conv; + } else if (pinA == params->pins[1]) { + return val1 * adc_voltage_conv; + } + return 0.0; +} + +// Configure low side for generic mcu +// cannot do much but +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + + pinMode(30,OUTPUT); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + adc1_init(); + adc_etc_init(pinA, pinB); + xbar_init(); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + return params; +} + +// sync driver and the adc +void _driverSyncLowSide(void* driver_params, void* cs_params){ + Teensy4DriverParams* par = (Teensy4DriverParams*) driver_params; + IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; + int submodule = par->submodules[0]; + + // do xbar connect here + + int xbar_trig_pwm = flextim__submodule_to_trig(flexpwm, submodule); + if(xbar_trig_pwm<0) return; + + xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + + // setup the ADC_ETC trigger + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // setup this val4 for interrupt on val5 match for ADC sync + // reading two ARC takes about 5us. So put the interrupt 2.5us befor the center + flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.h b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h new file mode 100644 index 00000000..2cf77dfb --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h @@ -0,0 +1,77 @@ + +#ifndef TEENSY4_CURRENTSENSE_MCU_DEF +#define TEENSY4_CURRENTSENSE_MCU_DEF + +#include "../../hardware_api.h" +#include "../../../common/foc_utils.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4026.0f + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct Teensy4CurrentSenseParams { + int pins[3] = {(int)NOT_SET}; + float adc_voltage_conv; +} Teensy4CurrentSenseParams; + + + +const uint8_t pin_to_channel[] = { // pg 482 + 7, // 0/A0 AD_B1_02 + 8, // 1/A1 AD_B1_03 + 12, // 2/A2 AD_B1_07 + 11, // 3/A3 AD_B1_06 + 6, // 4/A4 AD_B1_01 + 5, // 5/A5 AD_B1_00 + 15, // 6/A6 AD_B1_10 + 0, // 7/A7 AD_B1_11 + 13, // 8/A8 AD_B1_08 + 14, // 9/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 + 7, // 14/A0 AD_B1_02 + 8, // 15/A1 AD_B1_03 + 12, // 16/A2 AD_B1_07 + 11, // 17/A3 AD_B1_06 + 6, // 18/A4 AD_B1_01 + 5, // 19/A5 AD_B1_00 + 15, // 20/A6 AD_B1_10 + 0, // 21/A7 AD_B1_11 + 13, // 22/A8 AD_B1_08 + 14, // 23/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 +#ifdef ARDUINO_TEENSY41 + 255, // 28 + 255, // 29 + 255, // 30 + 255, // 31 + 255, // 32 + 255, // 33 + 255, // 34 + 255, // 35 + 255, // 36 + 255, // 37 + 128+1, // 38/A14 AD_B1_12 - only on ADC2, 1 + 128+2, // 39/A15 AD_B1_13 - only on ADC2, 2 + 9, // 40/A16 AD_B1_04 + 10, // 41/A17 AD_B1_05 +#endif +}; + + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp similarity index 94% rename from src/current_sense/hardware_specific/teensy_mcu.cpp rename to src/current_sense/hardware_specific/teensy/teensy_mcu.cpp index 7ab370a4..7669edc8 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 322d5a34..d3738aed 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -177,18 +177,15 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match. flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0 ; + flexpwm->SM[submodule].VAL0 = 0; flexpwm->SM[submodule].VAL1 = half_cycle ; flexpwm->SM[submodule].VAL2 = -mid_pwm ; flexpwm->SM[submodule].VAL3 = +mid_pwm ; - // flexpwm->SM[submodule].VAL4 = -mid_pwm ; - // flexpwm->SM[submodule].VAL5 = +mid_pwm ; - + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running } @@ -207,14 +204,11 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; flexpwm->SM[submodule].VAL2 = count_pwm; // A on flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off - // flexpwm->SM[submodule].VAL4 = - count_pwm ; // B off (assuming B inverted) - // flexpwm->SM[submodule].VAL5 = + count_pwm ; // B on flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1< Date: Tue, 6 Feb 2024 10:27:35 +0100 Subject: [PATCH 30/65] Use interrupt if no repetition counter --- .../stm32/stm32f1/stm32f1_hal.cpp | 9 ----- .../stm32/stm32f1/stm32f1_mcu.cpp | 36 +++++++++++++------ 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index ec75ef4f..3dd9b8de 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -124,12 +124,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); } - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - // enable interrupt - HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); - #endif - cs_params->adc_handle = &hadc; return 0; @@ -153,14 +147,11 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } - } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 69fea017..9d83cc4f 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -19,6 +19,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ if(AdcHandle->Instance == ADC1) return 0; #ifdef ADC2 // if ADC2 exists @@ -70,12 +76,24 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); + if( !use_adc_interrupt && !IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance)){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } + // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + if(use_adc_interrupt){ + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } + // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -86,19 +104,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -115,6 +132,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file From 24be38019e17b84a6673d07644f89c24a9f43daa Mon Sep 17 00:00:00 2001 From: Candas1 Date: Wed, 7 Feb 2024 16:13:12 +0100 Subject: [PATCH 31/65] Update stm32_mcu.cpp Comment the refresh of the timer in the new alignment code, it seems to mess up with the low side current sampling --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index bfb25789..33b5b664 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -382,7 +382,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + //timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif From 83826036937ee429cff17fb762355b9cc8758d18 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Wed, 7 Feb 2024 21:00:36 +0100 Subject: [PATCH 32/65] Same for f4,g4 and l4 --- .../stm32/stm32f1/stm32f1_mcu.cpp | 16 ++--- .../stm32/stm32f4/stm32f4_hal.cpp | 8 --- .../stm32/stm32f4/stm32f4_mcu.cpp | 36 +++++++--- .../stm32/stm32g4/stm32g4_hal.cpp | 39 ----------- .../stm32/stm32g4/stm32g4_mcu.cpp | 68 ++++++++++++++++--- .../stm32/stm32l4/stm32l4_hal.cpp | 39 ----------- .../stm32/stm32l4/stm32l4_mcu.cpp | 60 +++++++++++++--- .../hardware_specific/stm32/stm32_mcu.cpp | 2 +- 8 files changed, 144 insertions(+), 124 deletions(-) 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 9d83cc4f..5f090c20 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -69,20 +69,20 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle); - - if( !use_adc_interrupt && !IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance)){ - // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing - use_adc_interrupt = 1; - #ifdef SIMPLEFOC_STM32_DEBUG - SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); - #endif - } // start the adc if(use_adc_interrupt){ 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 dcf32137..bd0df4b6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -135,12 +135,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - // enable interrupt - HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC_IRQn); - #endif - cs_params->adc_handle = &hadc; return 0; } @@ -162,13 +156,11 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC_IRQHandler(void) { HAL_ADC_IRQHandler(&hadc); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index a3ee5407..6388e8e0 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -22,6 +22,12 @@ bool needs_downsample[3] = {1}; // downsampling variable - per adc (3) uint8_t tim_downsample[3] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif + void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { @@ -55,16 +61,28 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + if (use_adc_interrupt){ + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -75,19 +93,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -104,6 +121,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index bf89463b..e9210d54 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -180,43 +180,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - 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 -#endif - cs_params->adc_handle = &hadc; return 0; } @@ -238,7 +201,6 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -265,6 +227,5 @@ extern "C" { } #endif } -#endif #endif \ No newline at end of file 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 1a954105..fb32d36b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -24,6 +24,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -58,6 +63,14 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event @@ -66,12 +79,47 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // Start the adc calibration HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); - // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else - HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + // start the adc + if (use_adc_interrupt){ + // enable interrupt + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif + + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + }else{ + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -82,19 +130,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -111,6 +158,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index 688ba796..d728e076 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -179,43 +179,6 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } } - -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT - 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 -#endif - cs_params->adc_handle = &hadc; return 0; } @@ -237,7 +200,6 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void ADC1_2_IRQHandler(void) { @@ -264,6 +226,5 @@ extern "C" { } #endif } -#endif #endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp index 9dea0933..1fb0ab6b 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -23,6 +23,11 @@ bool needs_downsample[5] = {1}; // downsampling variable - per adc (5) uint8_t tim_downsample[5] = {0}; +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +uint8_t use_adc_interrupt = 1; +#else +uint8_t use_adc_interrupt = 0; +#endif void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ @@ -57,6 +62,14 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ 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; + }else{ + if(!use_adc_interrupt){ + // If the timer has no repetition counter, it needs to use the interrupt to downsample for low side sensing + use_adc_interrupt = 1; + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt has to be used"); + #endif + } } // set the trigger output event @@ -66,11 +79,44 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ HAL_ADCEx_Calibration_Start(cs_params->adc_handle,ADC_SINGLE_ENDED); // start the adc - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ + if(cs_params->adc_handle->Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #ifdef ADC2 + else if (cs_params->adc_handle->Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } + #endif + #ifdef ADC3 + else if (cs_params->adc_handle->Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } + #endif + #ifdef ADC4 + else if (cs_params->adc_handle->Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } + #endif + #ifdef ADC5 + else if (cs_params->adc_handle->Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } + #endif HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); - #else + }else{ HAL_ADCEx_InjectedStart(cs_params->adc_handle); - #endif + } // restart all the timers of the driver _startTimers(driver_params->timers, 6); @@ -81,19 +127,18 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ float _readADCVoltageLowSide(const int pin, const void* cs_params){ for(int i=0; i < 3; i++){ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer - #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + if (use_adc_interrupt){ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #else + }else{ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle,channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; - #endif + } } } return 0; } -#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT extern "C" { void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ // calculate the instance @@ -110,6 +155,5 @@ extern "C" { adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } -#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index bfb25789..33b5b664 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -382,7 +382,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + //timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif From 81db3f5a428efe8b85d712ab54b7beef62ac05f8 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 11 Feb 2024 19:47:10 +0100 Subject: [PATCH 33/65] added support for more than 2/3 adc pins --- .../hardware_specific/teensy/teensy4_mcu.cpp | 89 ++++++++++++++----- 1 file changed, 67 insertions(+), 22 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 8f2b441c..eca862a1 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,5 +1,6 @@ #include "teensy4_mcu.h" #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +#include "../../../common/foc_utils.h" // if defined // - Teensy 4.0 @@ -12,7 +13,7 @@ // // the flags are defined in the imxrt.h file // https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 -int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ if(submodule <0 && submodule > 3) return -1; if(flexpwm == &IMXRT_FLEXPWM1){ return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; @@ -26,32 +27,45 @@ int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ return -1; } -volatile uint32_t val0, val1; +volatile uint32_t val0, val1, val2; -void read_currents(uint32_t *a, uint32_t*b){ +void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ *a = val0; *b = val1; + *c = val2; } // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { digitalWrite(30,HIGH); - ADC_ETC_DONE0_1_IRQ |= 1; // clear + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; asm("dsb"); digitalWrite(30,LOW); } + +void adcetc1_isr() { + digitalWrite(30,HIGH); + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit + val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; + // val2 = (ADC_ETC_TRIG0_RESULT_3_2 >> 16) & 4095; + asm("dsb"); + digitalWrite(30,LOW); +} + // function initializing the ADC2 // and the ADC_ETC trigger for the low side current sensing -void adc1_init() { +void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { //Tried many configurations, but this seems to be best: ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -63,7 +77,9 @@ void adc1_init() { ADC1_HC0 = 16; // ADC_ETC channel // use the second interrupt if necessary (for more than 2 channels) - // ADC1_HC1 = 16; + if(_isset(pin3)) { + ADC1_HC1 = 16; + } } // function initializing the ADC2 @@ -75,7 +91,7 @@ void adc2_init(){ ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -89,10 +105,13 @@ void adc2_init(){ // ADC2_HC1 = 16; } -void adc_etc_init(int pin1, int pin2) { +// function initializing the ADC_ETC trigger for the low side current sensing +// it uses only the ADC1 +// if the pin3 is not set it uses only 2 channels +void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST ADC_ETC_CTRL = 0x40000001; // start with trigger 0 - ADC_ETC_TRIG0_CTRL = 0x100; // chainlength -1 + ADC_ETC_TRIG0_CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN( _isset(pin3) ? 2 : 1) ; // 2 if 3 channels, 1 if 2 channels // ADC1 7 8, chain channel, HWTS, IE, B2B // pg 3516, section 66.5.1.8 @@ -109,11 +128,18 @@ void adc_etc_init(int pin1, int pin2) { attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); // use the second interrupt if necessary (for more than 2 channels) - // attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); - // NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + if(_isset(pin3)) { + ADC_ETC_TRIG0_CHAIN_3_2 = + ADC_ETC_TRIG_CHAIN_IE0(2) | // interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B0 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin3]); + + attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + } } - void xbar_connect(unsigned int input, unsigned int output) { if (input >= 88) return; @@ -135,12 +161,15 @@ void xbar_init() { // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + if(!_isset(pinA)) return 0.0; // if the pin is not set return 0 GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; float adc_voltage_conv = params->adc_voltage_conv; if (pinA == params->pins[0]) { return val0 * adc_voltage_conv; } else if (pinA == params->pins[1]) { return val1 * adc_voltage_conv; + }else if (pinA == params->pins[2]) { + return val2 * adc_voltage_conv; } return 0.0; } @@ -149,6 +178,7 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); + // _UNUSED(pinC); pinMode(30,OUTPUT); @@ -156,11 +186,25 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - adc1_init(); - adc_etc_init(pinA, pinB); + // check if either of the pins are not set + // and dont use it if it isn't + int pin_count = 0; + int pins[3] = {NOT_SET, NOT_SET, NOT_SET}; + if(_isset(pinA)) pins[pin_count++] = pinA; + if(_isset(pinB)) pins[pin_count++] = pinB; + if(_isset(pinC)) pins[pin_count++] = pinC; + + + adc1_init(pins[0], pins[1], pins[2]); + SIMPLEFOC_DEBUG("pins: ",pins[0]); + SIMPLEFOC_DEBUG("pins: ",pins[1]); + SIMPLEFOC_DEBUG("pins: ",pins[2]); + adc_etc_init(pins[0], pins[1], pins[2]); + xbar_init(); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, + .pins = {pins[0], pins[1], pins[2] }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; return params; @@ -172,17 +216,18 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; - // do xbar connect here - - int xbar_trig_pwm = flextim__submodule_to_trig(flexpwm, submodule); + // find the xbar trigger for the flexpwm + int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); if(xbar_trig_pwm<0) return; + // allow theFlexPWM to trigger the ADC_ETC xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc - // setup the ADC_ETC trigger + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); - // setup this val4 for interrupt on val5 match for ADC sync - // reading two ARC takes about 5us. So put the interrupt 2.5us befor the center + // setup this val4 for interrupt on match for ADC sync + // this code assumes that the val4 is not used for anything else! + // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center } From 1ecfb22ef35422e6d0d1ae8eb59821bd96b0c7eb Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 18 Feb 2024 20:28:55 +0100 Subject: [PATCH 34/65] NbrOfConversion = 1 --- .../hardware_specific/stm32/stm32f1/stm32f1_hal.cpp | 2 +- .../hardware_specific/stm32/stm32g4/stm32g4_hal.cpp | 2 +- .../hardware_specific/stm32/stm32l4/stm32l4_hal.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp index 27e11958..245f27f7 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp @@ -67,7 +67,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.DiscontinuousConvMode = DISABLE; hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc.Init.NbrOfConversion = 0; + hadc.Init.NbrOfConversion = 1; HAL_ADC_Init(&hadc); /**Configure for the selected ADC regular channel to be converted. */ 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 91322112..c62ba07a 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -100,7 +100,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp index bfec2175..76336154 100644 --- a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -99,7 +99,7 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; From 0588d3845188237acf070fc0a5cab50fa6f286d4 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 26 Feb 2024 17:41:35 +0100 Subject: [PATCH 35/65] added teensy 4 6pwm tiemer sync and better support for low-side current sensing --- .../hardware_specific/teensy/teensy4_mcu.cpp | 94 +++++----- .../hardware_specific/teensy/teensy4_mcu.cpp | 164 ++++++++++++++++-- .../hardware_specific/teensy/teensy4_mcu.h | 11 ++ 3 files changed, 207 insertions(+), 62 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index eca862a1..b76624bc 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,5 +1,6 @@ #include "teensy4_mcu.h" #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +// #include "../../../common/lowpass_filter.h" #include "../../../common/foc_utils.h" // if defined @@ -7,28 +8,16 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) -// function finding the TRIG event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 -int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; - } - return -1; -} +// #define TEENSY4_ADC_INTERRUPT_DEBUG + +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense volatile uint32_t val0, val1, val2; +// LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); + void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ *a = val0; *b = val1; @@ -38,24 +27,32 @@ void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); +#endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit - val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; + // val0 = lp1(ADC_ETC_TRIG0_RESULT_1_0 & 4095); + val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); + // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; - asm("dsb"); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); +#endif } void adcetc1_isr() { - digitalWrite(30,HIGH); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; - // val2 = (ADC_ETC_TRIG0_RESULT_3_2 >> 16) & 4095; - asm("dsb"); - digitalWrite(30,LOW); +// val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif } // function initializing the ADC2 @@ -65,7 +62,7 @@ void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) (1 is faster and maybe with some filtering could provide better results but 2 for now) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -91,7 +88,7 @@ void adc2_init(){ ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -140,22 +137,6 @@ void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { } } -void xbar_connect(unsigned int input, unsigned int output) -{ - if (input >= 88) return; - if (output >= 132) return; - volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); - uint16_t val = *xbar; - if (!(output & 1)) { - val = (val & 0xFF00) | input; - } else { - val = (val & 0x00FF) | (input << 8); - } - *xbar = val; -} -void xbar_init() { - CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 -} // function reading an ADC value and returning the read voltage @@ -178,9 +159,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); - // _UNUSED(pinC); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); +#endif if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); @@ -196,9 +178,6 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p adc1_init(pins[0], pins[1], pins[2]); - SIMPLEFOC_DEBUG("pins: ",pins[0]); - SIMPLEFOC_DEBUG("pins: ",pins[1]); - SIMPLEFOC_DEBUG("pins: ",pins[2]); adc_etc_init(pins[0], pins[1], pins[2]); xbar_init(); @@ -223,12 +202,29 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // allow theFlexPWM to trigger the ADC_ETC xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 1 (val1) + //This val1 interrupt on match is in the center of the PWM + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<1); + + + // if needed the interrupt can be moved to some other point in the PWM cycle by using an addional val register example: VAL4 // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) - flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); // setup this val4 for interrupt on match for ADC sync // this code assumes that the val4 is not used for anything else! // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center - flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center + // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center + + +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + // pin 4 observes out trigger line for 'scope + xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; + IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 + // Select alt 3 for EMC_06 (XBAR), rather than original 5 (GPIO) + CORE_PIN4_CONFIG = 3 ; // shorthand for IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_06 = 3 ; + // turn up drive & speed as very short pulse + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; +#endif } diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index d3738aed..bf81e246 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -13,10 +13,135 @@ #pragma message("") -// half_cycle of the PWM variable -int half_cycle = 0; + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + // function which finds the flexpwm instance for a pair of pins // if they do not belong to the same timer it returns a nullpointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ @@ -145,7 +270,7 @@ return ch2; // can configure sync, prescale and B inversion. // sets the desired frequency of the PWM // sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long frequency, float dead_zone ) +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) { int submodule_mask = 1 << submodule ; flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running @@ -167,17 +292,21 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque } // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); + int half_cycle = int(newdiv/2.0f); int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% int mid_pwm = int((half_cycle)/2.0f); + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + // setup the timer // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE @@ -204,17 +333,15 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int half_cycle = int(flexpwm->SM[submodule].VAL1); int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - flexpwm->SM[submodule].VAL2 = count_pwm; // A on - flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); } + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index 5e384623..a7d8c97a 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -7,6 +7,7 @@ // teensy 4 driver configuration parameters typedef struct Teensy4DriverParams { + int pins[6] = {(int)NOT_SET}; IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; long pwm_frequency; @@ -105,6 +106,16 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; + +// find the trigger TRG0 for the given timer and submodule +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// find the external trigger for the given timer and submodule +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// function to connecting the triggers +void xbar_connect(unsigned int input, unsigned int output); +// function to initialize the xbar +void xbar_init(); + #endif #endif \ No newline at end of file From 94f4fb5675873fa6f520e380988953d832ba0488 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 29 Feb 2024 12:38:40 +0100 Subject: [PATCH 36/65] added support for center aligned 3pwm at least for one motor + low-side current sensing --- .../hardware_specific/teensy/teensy4_mcu.cpp | 16 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 277 ++++++++- .../hardware_specific/teensy/teensy4_mcu.h | 10 +- .../teensy/teensy4_mcu1.cpp.new | 543 ++++++++++++++++++ .../hardware_specific/teensy/teensy_mcu.cpp | 98 ++-- .../hardware_specific/teensy/teensy_mcu.h | 13 + 6 files changed, 896 insertions(+), 61 deletions(-) create mode 100644 src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index b76624bc..e5934920 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -2,6 +2,7 @@ #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" // #include "../../../common/lowpass_filter.h" #include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" // if defined // - Teensy 4.0 @@ -158,7 +159,13 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // Configure low side for generic mcu // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + if(par == nullptr){ + SIMPLEFOC_DEBUG("TEENSY-CS: Low side current sense failed, driver not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); #ifdef TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); @@ -191,10 +198,15 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p // sync driver and the adc void _driverSyncLowSide(void* driver_params, void* cs_params){ - Teensy4DriverParams* par = (Teensy4DriverParams*) driver_params; + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; + SIMPLEFOC_DEBUG("TEENSY-CS: Syncing low side current sense!"); + char buff[50]; + sprintf(buff, "TEENSY-CS: Syncing to FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwm), submodule); + SIMPLEFOC_DEBUG(buff); + // find the xbar trigger for the flexpwm int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); if(xbar_trig_pwm<0) return; diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index bf81e246..6815c10f 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,4 +1,3 @@ -#include "teensy_mcu.h" #include "teensy4_mcu.h" #include "../../../communication/SimpleFOCDebug.h" @@ -7,11 +6,11 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - #pragma message("") #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") +// #define AVOID_TEENSY4_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -41,7 +40,7 @@ int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ // the flags are defined in the imxrt.h file // https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; + if(submodule < 0 && submodule > 3) return -1; if(flexpwm == &IMXRT_FLEXPWM1){ return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; }else if(flexpwm == &IMXRT_FLEXPWM2){ @@ -54,6 +53,15 @@ int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ return -1; } +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm){ + if(flexpwm == &IMXRT_FLEXPWM1) return 1; + if(flexpwm == &IMXRT_FLEXPWM2) return 2; + if(flexpwm == &IMXRT_FLEXPWM3) return 3; + if(flexpwm == &IMXRT_FLEXPWM4) return 4; + return -1; +} + // The i.MXRT1062 uses one config register per two XBAR outputs, so a helper // function to make code more readable. void xbar_connect(unsigned int input, unsigned int output) @@ -223,11 +231,34 @@ int get_submodule(uint8_t pin, uint8_t pin1){ } +// function which finds the channel for flexpwm timer pin +// 0 - X +// 1 - A +// 2 - B +int get_channel(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + info = pwm_pin_info + pin; +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); + SIMPLEFOC_DEBUG(s); +#endif + return info->channel; +} + // function which finds the timer submodule for a pair of pins // if they do not belong to the same submodule it returns a -1 int get_inverted_channel(uint8_t pin, uint8_t pin1){ - const struct pwm_pin_info_struct *info; if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; @@ -237,10 +268,8 @@ int get_inverted_channel(uint8_t pin, uint8_t pin1){ return -1; } - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; + int ch1 = get_channel(pin); + int ch2 = get_channel(pin1); if (ch1 != 1) { #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -320,6 +349,70 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, c } +// Helper to set up a FlexPWM submodule. +// can configure sync, prescale +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_timer_submodule (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + int half_cycle = int(newdiv/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = 0 ; + flexpwm->SM[submodule].DTCNT1 = 0 ; + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, int channel) +{ + int submodule_mask = 1 << submodule ; + + if(channel==1) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + else if(channel==2) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + // staring the PWM on A and B channels of the submodule void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) { @@ -352,6 +445,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; int submoduleA,submoduleB,submoduleC; int inverted_channelA,inverted_channelB,inverted_channelC; + int channelA,channelB,channelC; flexpwmA = get_flexpwm(pinA_h,pinA_l); submoduleA = get_submodule(pinA_h,pinA_l); inverted_channelA = get_inverted_channel(pinA_h,pinA_l); @@ -361,6 +455,9 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons flexpwmC = get_flexpwm(pinC_h,pinC_l); submoduleC = get_submodule(pinC_h,pinC_l); inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + channelA = get_channel(pinA_h); + channelB = get_channel(pinB_h); + channelC = get_channel(pinC_h); if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -381,21 +478,18 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons return SIMPLEFOC_DRIVER_INIT_FAILED; } - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone - }; - + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-DRV: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-DRV: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif // Configure FlexPWM units, each driving A/B pair, B inverted. - // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz - setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // others externally synced setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced - setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; // this is the master, internally synced delayMicroseconds (100) ; // turn on XBAR1 clock for all but stop mode @@ -409,7 +503,6 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons startup_pwm_pair (flexpwmB, submoduleB) ; startup_pwm_pair (flexpwmC, submoduleC) ; - delayMicroseconds(50) ; // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; @@ -419,6 +512,17 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {1,2, 1, 2, 1, 2}, + .dead_zone = dead_zone + } + }; return params; } @@ -428,11 +532,136 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // - Stepper motor - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); + write_pwm_pair (p->flextimers[0], p->submodules[0], dc_a); + write_pwm_pair (p->flextimers[1], p->submodules[1], dc_b); + write_pwm_pair (p->flextimers[2], p->submodules[2], dc_c); +} + +void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float duty) +{ + uint16_t mask = 1 << submodule; + uint32_t half_cycle = p->SM[submodule].VAL1; + int mid_pwm = int((half_cycle)/2.0f); + int cval = int(mid_pwm*(duty*2-1)) + mid_pwm; + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = half_cycle - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL2 = -cval; + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL4 = -cval; + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } +#ifndef AVOID_TEENSY4_CENTER_ALIGNED_3PWM + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configureCenterAligned3PMW(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + int channelA = get_channel(pinA); + int channelB = get_channel(pinB); + int channelC = get_channel(pinC); + + + // if pins belong to the flextimers and they only use submodules A and B + // we can configure the center-aligned pwm + if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B submodules - Configuring center-aligned pwm!"); + #endif + + // Configure FlexPWM units + setup_pwm_timer_submodule (flexpwmA, submoduleA, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmB, submoduleB, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmC, submoduleC, false, pwm_frequency) ; // this is the master, internally synced + delayMicroseconds (100) ; + + + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-CS: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-CS: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif + + // // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {channelA, channelB, channelC}, + } + }; + + startup_pwm_pair (flexpwmA, submoduleA, channelA) ; + startup_pwm_pair (flexpwmB, submoduleB, channelB) ; + startup_pwm_pair (flexpwmC, submoduleC, channelC) ; + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; + }else{ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B submodules - cannot configure center-aligned pwm!"); + #endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; + write_pwm_on_pin (p->flextimers[0], p->submodules[0], p->channels[0], dc_a); + write_pwm_on_pin (p->flextimers[1], p->submodules[1], p->channels[1], dc_b); + write_pwm_on_pin (p->flextimers[2], p->submodules[2], p->channels[2], dc_c); +} + +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index a7d8c97a..aed64826 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY4_MCU_DRIVER_H +#define TEENSY4_MCU_DRIVER_H + #include "teensy_mcu.h" // if defined @@ -7,10 +10,9 @@ // teensy 4 driver configuration parameters typedef struct Teensy4DriverParams { - int pins[6] = {(int)NOT_SET}; IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; - long pwm_frequency; + int channels[6]; float dead_zone; } Teensy4DriverParams; @@ -106,7 +108,8 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; - +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm); // find the trigger TRG0 for the given timer and submodule int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); // find the external trigger for the given timer and submodule @@ -118,4 +121,5 @@ void xbar_init(); #endif +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new new file mode 100644 index 00000000..5dcac90d --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new @@ -0,0 +1,543 @@ +#include "teensy_mcu.h" +#include "teensy4_mcu.h" +#include "../../../communication/SimpleFOCDebug.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + + + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// half_cycle of the PWM variable +int half_cycle = 0; + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} + + +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + +// function which finds the flexpwm instance for a pair of pins +// if they do not belong to the same timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; + switch ((info->module >> 4) & 3) { + case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; + default: flexpwm1 = &IMXRT_FLEXPWM4; + } + + info = pwm_pin_info + pin1; + switch ((info->module >> 4) & 3) { + case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; + default: flexpwm2 = &IMXRT_FLEXPWM4; + } + if(flexpwm1 == flexpwm2){ + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); + return flexpwm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_submodule(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + info = pwm_pin_info + pin1; + int sm2 = info->module&0x3; + + if (sm1 == sm2) { + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); + SIMPLEFOC_DEBUG(s); + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_inverted_channel(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int ch1 = info->channel; + info = pwm_pin_info + pin1; + int ch2 = info->channel; + + if (ch1 != 1) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else if (ch2 != 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); + SIMPLEFOC_DEBUG(s); +#endif +return ch2; + } +} + +// Helper to set up A/B pair on a FlexPWM submodule. +// can configure sync, prescale and B inversion. +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + half_cycle = int(newdiv/2.0f); + int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% + int mid_pwm = int((half_cycle)/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle ; + flexpwm->SM[submodule].VAL2 = -mid_pwm ; + flexpwm->SM[submodule].VAL3 = +mid_pwm ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) +{ + int submodule_mask = 1 << submodule ; + + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + +// PWM setting on the high and low pair of the PWM channels +void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int mid_pwm = int((half_cycle)/2.0f); + int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; + + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + int count_pwm = int(count*duty); + + SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); + SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); + SIMPLEFOC_DEBUG("count: ",count_pwm); + + // flexpwm->SM[submodule].VAL1 = 0; // A on + flexpwm->SM[submodule].VAL2 = count_pwm ; // A off + flexpwm->SM[submodule].VAL3 = count_pwm; // A on + flexpwm->SM[submodule].VAL4 = count_pwm ; // A off + flexpwm->SM[submodule].VAL5 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; + if (cval > modulo) cval = modulo; // TODO: is this check correct? + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = modulo - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 6PWM setting +// - hardware specific +void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA_h,pinA_l); + submoduleA = get_submodule(pinA_h,pinA_l); + inverted_channelA = get_inverted_channel(pinA_h,pinA_l); + flexpwmB = get_flexpwm(pinB_h,pinB_l); + submoduleB = get_submodule(pinB_h,pinB_l); + inverted_channelB = get_inverted_channel(pinB_h,pinB_l); + flexpwmC = get_flexpwm(pinC_h,pinC_l); + submoduleC = get_submodule(pinC_h,pinC_l); + inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + + if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + + // Configure FlexPWM units, each driving A/B pair, B inverted. + // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + delayMicroseconds (100) ; + + // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + + delayMicroseconds(50) ; + // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; + *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; + *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; + *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; + *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; + *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + return params; +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA, pinB, pinC }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + }; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + // analogWriteFrequency(pinA, pwm_frequency); + // analogWriteFrequency(pinB, pwm_frequency); + // analogWriteFrequency(pinC, pwm_frequency); + // analogWrite(pinA, 0); + // analogWrite(pinB, 0); + // analogWrite(pinC, 0); + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index dcfa3e15..2ad5f57c 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -2,6 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) +#include "../../../communication/SimpleFOCDebug.h" + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); @@ -11,14 +13,15 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific 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 { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -26,38 +29,53 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm +__attribute__((weak)) void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - GenericDriverParams* params = new GenericDriverParams { - .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency - }; + + // try configuring center aligned pwm + void* p = _configureCenterAligned3PMW(pwm_frequency, pinA, pinB, pinC); + if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ + return p; // if center aligned pwm is available return the params + }else{ // if center aligned pwm is not available use fast pwm + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; return params; + } } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -65,9 +83,10 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB, pinC, pinD }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -75,42 +94,57 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific 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); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_b); +} + +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm implementation of this function +__attribute__((weak)) void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(params); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); + + TeensyDriverParams* p = (TeensyDriverParams*)params; + if(p->additional_params != nullptr){ + _writeCenterAligned3PMW(dc_a, dc_b, dc_c, p); + }else{ + // transform duty cycle from [0,1] to [0,255] + analogWrite(p->pins[0], 255.0f*dc_a); + analogWrite(p->pins[1], 255.0f*dc_b); + analogWrite(p->pins[2], 255.0f*dc_c); + } } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); - analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((TeensyDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((TeensyDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.h b/src/drivers/hardware_specific/teensy/teensy_mcu.h index 7956ea90..266f4b69 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY_MCU_DRIVER_H +#define TEENSY_MCU_DRIVER_H + #include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +11,17 @@ // debugging output #define SIMPLEFOC_TEENSY_DEBUG +typedef struct TeensyDriverParams { + int pins[6] = {(int)NOT_SET}; + long pwm_frequency; + void* additional_params; +} TeensyDriverParams; + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin); +void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params); + +#endif #endif \ No newline at end of file From 238d26b63fb88fe77042055bb410a387f18f9a49 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 29 Feb 2024 12:54:32 +0100 Subject: [PATCH 37/65] better error messages --- .../hardware_specific/teensy/teensy4_mcu.cpp | 15 +++++++-------- .../hardware_specific/teensy/teensy_mcu.cpp | 1 + 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 6815c10f..0e7953ff 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -87,7 +87,8 @@ void xbar_init() { IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL) { + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2) { #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); @@ -95,7 +96,6 @@ IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ #endif return nullptr; } - info = pwm_pin_info + pin; // FlexPWM pin IMXRT_FLEXPWM_t *flexpwm; switch ((info->module >> 4) & 3) { @@ -236,17 +236,16 @@ int get_submodule(uint8_t pin, uint8_t pin1){ // 1 - A // 2 - B int get_channel(uint8_t pin){ - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL){ + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2){ #ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; + char s[90]; sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); SIMPLEFOC_DEBUG(s); #endif return -1; } - info = pwm_pin_info + pin; #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); @@ -598,7 +597,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe // we can configure the center-aligned pwm if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ #ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B submodules - Configuring center-aligned pwm!"); + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B channels - Configuring center-aligned pwm!"); #endif // Configure FlexPWM units @@ -645,7 +644,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe return params; }else{ #ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B submodules - cannot configure center-aligned pwm!"); + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B channels - cannot configure center-aligned pwm!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index 2ad5f57c..196f07fd 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -61,6 +61,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ return p; // if center aligned pwm is available return the params }else{ // if center aligned pwm is not available use fast pwm + SIMPLEFOC_DEBUG("TEENSY-DRV: Configuring 3PWM with fast pwm. Please consider using center aligned pwm for better performance!"); _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); From dec7abf63c460340f34bf9b48d2ef1f5530b6d21 Mon Sep 17 00:00:00 2001 From: copper280z Date: Sun, 10 Mar 2024 20:30:45 -0400 Subject: [PATCH 38/65] added code to set timer prescalers and overflow based on actual timer clock frequency --- .../hardware_specific/stm32/stm32_mcu.cpp | 47 +++++++++++++++++-- .../hardware_specific/stm32/stm32_mcu.h | 3 ++ 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 33b5b664..9a349670 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -26,7 +26,15 @@ PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED]; +bool _getPwmState(void* params) { + // assume timers are synchronized and that there's at least one timer + HardwareTimer* pHT = ((STM32DriverParams*)params)->timers[0]; + TIM_HandleTypeDef* htim = pHT->getHandle(); + + bool dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(htim); + return dir; +} // setting pwm to hardware pin - instead analogWrite() @@ -230,7 +238,7 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { // #endif return -1; } -#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) +#elif defined(STM32F4xx) || defined(STM32F1xx) || defined(STM32L4xx) || defined(STM32F7xx) // function finds the appropriate timer source trigger for the master/slave timer combination // returns -1 if no trigger source is found @@ -303,6 +311,30 @@ int _getInternalSourceTrigger(HardwareTimer* master, HardwareTimer* slave) { } #endif +void syncTimerFrequency(long pwm_frequency, HardwareTimer *timers[], uint8_t num_timers) { + uint32_t max_frequency = 0; + uint32_t min_frequency = UINT32_MAX; + for (size_t i=0; igetTimerClkFreq(); + if (freq > max_frequency) { + max_frequency = freq; + } else if (freq < min_frequency) { + min_frequency = freq; + } + } + if (max_frequency==min_frequency) return; + uint32_t overflow_value = min_frequency/pwm_frequency; + for (size_t i=0; igetTimerClkFreq()/min_frequency; + #ifdef SIMPLEFOC_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Setting prescale to ", (float)prescale_factor); + SIMPLEFOC_DEBUG("STM32-DRV: Setting Overflow to ", (float)overflow_value); + #endif + timers[i]->setPrescaleFactor(prescale_factor); + timers[i]->setOverflow(overflow_value,TICK_FORMAT); + timers[i]->refresh(); + } +} void _alignTimersNew() { int numTimers = 0; @@ -382,7 +414,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - //timers[i]->refresh(); + timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif @@ -735,6 +767,8 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); + HardwareTimer *timers[2] = {HT1, HT2}; + syncTimerFrequency(pwm_frequency, timers, 2); // allign the timers _alignPWMTimers(HT1, HT2, HT2); @@ -779,6 +813,9 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); + HardwareTimer *timers[3] = {HT1, HT2, HT3}; + syncTimerFrequency(pwm_frequency, timers, 3); + uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -821,6 +858,8 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]); HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]); + HardwareTimer *timers[4] = {HT1, HT2, HT3, HT4}; + syncTimerFrequency(pwm_frequency, timers, 4); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); @@ -918,6 +957,8 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]); HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]); HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]); + HardwareTimer *timers[6] = {HT1, HT2, HT3, HT4, HT5, HT6}; + syncTimerFrequency(pwm_frequency, timers, 6); uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function); uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function); uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function); @@ -1103,4 +1144,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif -#endif +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.h b/src/drivers/hardware_specific/stm32/stm32_mcu.h index fa6280e9..411c43b2 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.h +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.h @@ -28,5 +28,8 @@ typedef struct STM32DriverParams { void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6); void _startTimers(HardwareTimer **timers_to_start, int timer_num=6); +// timer query functions +bool _getPwmState(void* params); + #endif #endif \ No newline at end of file From f2b7b22deae44254a2c2284ea0665cddcc8d0f8f Mon Sep 17 00:00:00 2001 From: copper280z Date: Sun, 10 Mar 2024 20:45:04 -0400 Subject: [PATCH 39/65] comment out HT->refresh() call --- src/drivers/hardware_specific/stm32/stm32_mcu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp index 9a349670..0fdec2f0 100644 --- a/src/drivers/hardware_specific/stm32/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32/stm32_mcu.cpp @@ -414,7 +414,7 @@ void _alignTimersNew() { // enable timer clock for (int i=0; ipause(); - timers[i]->refresh(); + // timers[i]->refresh(); #ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance))); #endif @@ -1144,4 +1144,4 @@ void printTimerCombination(int numPins, PinMap* timers[], int score) { #endif -#endif \ No newline at end of file +#endif From 49da7b0630b6af9ed254bc570f8d3795825ae402 Mon Sep 17 00:00:00 2001 From: copper280z Date: Sun, 17 Mar 2024 16:20:38 -0400 Subject: [PATCH 40/65] add f7 current sense files --- .../stm32/stm32f7/stm32f7_hal.cpp | 185 +++++++++++++ .../stm32/stm32f7/stm32f7_hal.h | 15 ++ .../stm32/stm32f7/stm32f7_mcu.cpp | 111 ++++++++ .../stm32/stm32f7/stm32f7_utils.cpp | 255 ++++++++++++++++++ .../stm32/stm32f7/stm32f7_utils.h | 30 +++ 5 files changed, 596 insertions(+) create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp create mode 100644 src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp new file mode 100644 index 00000000..d4cffec6 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp @@ -0,0 +1,185 @@ +#include "stm32f7_hal.h" + +#if defined(STM32F7xx) + +//#define SIMPLEFOC_STM32_DEBUG + +#include "../../../../communication/SimpleFOCDebug.h" +#define _TRGO_NOT_AVAILABLE 12345 + +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); + + if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE(); +#ifdef ADC2 // if defined ADC2 + else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef ADC3 // if defined ADC3 + else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE(); +#endif + else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); +#endif + return -1; // error not a valid ADC instance + } + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); +#endif + + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 1; + hadc.Init.DMAContinuousRequests = DISABLE; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + for (size_t i=0; i<6; i++) { + HardwareTimer *timer_to_check = driver_params->timers[tim_num++]; + TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance; + + // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE; + // if(TRGO_already_configured) continue; + + uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // if the code comes here, it has found the timer available + // timer does have trgo flag for injected channels + sConfigInjected.ExternalTrigInjecConv = trigger_flag; + + // this will be the timer with which the ADC will sync + cs_params->timer_handle = timer_to_check; + if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) { + // workaround for errata 2.2.1 in ES0290 Rev 7 + // https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf + __HAL_RCC_DAC_CLK_ENABLE(); + } + // done + break; + } + if( cs_params->timer_handle == NP ){ + // not possible to use these timers for low-side current sense + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); + #endif + return -1; + } + // 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 + + + // first channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); +#endif + return -1; + } + + // second channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; +#endif + return -1; + } + + // third channel - if exists + if(_isset(cs_params->pins[2])){ + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; +#endif + return -1; + } + } + + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + // enable interrupt + HAL_NVIC_SetPriority(ADC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + #endif + + cs_params->adc_handle = &hadc; + return 0; +} + +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + uint8_t cnt = 0; + if(_isset(pinA)){ + pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); + cs_params->pins[cnt++] = pinA; + } + if(_isset(pinB)){ + pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); + cs_params->pins[cnt++] = pinB; + } + if(_isset(pinC)){ + pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); + cs_params->pins[cnt] = pinC; + } +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void ADC_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h new file mode 100644 index 00000000..0a3614b5 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h @@ -0,0 +1,15 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) +#include "stm32f7xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32f7_utils.h" + +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); + +#endif diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp new file mode 100644 index 00000000..3040ea46 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp @@ -0,0 +1,111 @@ +#include "../../../hardware_api.h" + +#if defined(STM32F7xx) +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32f7_hal.h" +#include "stm32f7_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4096.0f + + +// array of values of 4 injected channels per adc instance (3) +uint32_t adc_val[3][4]={0}; +// does adc interrupt need a downsample - per adc (3) +bool needs_downsample[3] = {1}; +// downsampling variable - per adc (3) +uint8_t tim_downsample[3] = {1}; + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + }; + _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + return cs_params; +} + + +void _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // if compatible timer has not been found + if (cs_params->timer_handle == NULL) return; + + // 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 + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + #else + HAL_ADCEx_InjectedStart(cs_params->adc_handle); + #endif + + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer + #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #else + // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3 + uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3; + return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + #endif + } + } + return 0; +} + +#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + + // calculate the instance + int adc_index = _adcToIndex(AdcHandle); + + // if the timer han't repetition counter - downsample two times + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { + tim_downsample[adc_index] = 0; + return; + } + + adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + } +} +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp new file mode 100644 index 00000000..d5f8c6b2 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp @@ -0,0 +1,255 @@ +#include "stm32f7_utils.h" + +#if defined(STM32F7xx) + +/* Exported Functions */ + + +PinName analog_to_pin(uint32_t pin) { + PinName pin_name = analogInputToPinName(pin); + if (pin_name == NC) { + return (PinName) pin; + } + return pin_name; +} + + +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} +/* +TIM1 +TIM2 +TIM3 +TIM4 +TIM5 +TIM6 +TIM7 +TIM12 +TIM13 +TIM14 + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO +ADC_EXTERNALTRIGINJECCONV_T2_TRGO +ADC_EXTERNALTRIGINJECCONV_T4_TRGO + +ADC_EXTERNALTRIGINJECCONV_T1_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T8_TRGO2 +ADC_EXTERNALTRIGINJECCONV_T5_TRGO +ADC_EXTERNALTRIGINJECCONV_T6_TRGO +*/ +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + + 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 +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJECCONV_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJECCONV_T8_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} +/* + +ADC_EXTERNALTRIGCONV_T5_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO +ADC_EXTERNALTRIGCONV_T8_TRGO2 +ADC_EXTERNALTRIGCONV_T1_TRGO +ADC_EXTERNALTRIGCONV_T1_TRGO2 +ADC_EXTERNALTRIGCONV_T2_TRGO +ADC_EXTERNALTRIGCONV_T4_TRGO +ADC_EXTERNALTRIGCONV_T6_TRGO +*/ + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGCONV_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGCONV_T2_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGCONV_T4_TRGO; +#endif +#ifdef TIM5 // if defined timer 5 + else if(timer->getHandle()->Instance == TIM5) + return ADC_EXTERNALTRIGCONV_T5_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGCONV_T6_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/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h new file mode 100644 index 00000000..017ff464 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h @@ -0,0 +1,30 @@ +#pragma once + +#include "Arduino.h" + +#if defined(STM32F7xx) + +#define _TRGO_NOT_AVAILABLE 12345 + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin); + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/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 \ No newline at end of file From 92cb9af1da548c9f978577327961eed6f7686a7f Mon Sep 17 00:00:00 2001 From: Yannik Stradmann <49203055+ystradmann@users.noreply.github.com> Date: Wed, 20 Mar 2024 00:18:19 +0100 Subject: [PATCH 41/65] Add missing implementation for MagneticSensorI2C::AS5600 --- src/sensors/MagneticSensorI2C.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index ac2b273b..7b4ea77e 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -58,6 +58,10 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ wire = &Wire; } +MagneticSensorI2C MagneticSensorI2C::AS5600() { + return {AS5600_I2C}; +} + void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; From d48f4d795591b688470c791509f7117bb745dd0a Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:15:21 +0200 Subject: [PATCH 42/65] added support for debugging with char --- src/communication/SimpleFOCDebug.cpp | 6 ++++++ src/communication/SimpleFOCDebug.h | 1 + 2 files changed, 7 insertions(+) diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index e969d8a2..3bb62bce 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -58,6 +58,12 @@ void SimpleFOCDebug::println(const char* str, int val) { _debugPrint->println(val); } } +void SimpleFOCDebug::println(const char* str, char val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 614e6371..4fcfd538 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -45,6 +45,7 @@ class SimpleFOCDebug { static void println(const char* msg, float val); static void println(const __FlashStringHelper* msg, int val); static void println(const char* msg, int val); + static void println(const char* msg, char val); static void println(); static void println(int val); static void println(float val); From c171a019114277546f8fcb57240f67bf32d51bb3 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:27:32 +0200 Subject: [PATCH 43/65] renamed the flag to force_center_aligend_3pwm --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 0e7953ff..060ff4e2 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -10,7 +10,7 @@ #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") -// #define AVOID_TEENSY4_CENTER_ALIGNED_3PWM +// #define TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -83,7 +83,7 @@ void xbar_init() { } // function which finds the flexpwm instance for a pin -// if it does not belong to the flexpwm timer it returns a nullpointer +// if it does not belong to the flexpwm timer it returns a null-pointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ const struct pwm_pin_info_struct *info; @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifndef AVOID_TEENSY4_CENTER_ALIGNED_3PWM +#ifnef TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting From 82da85726a59d141a96ecebd06699a3f050aa0f3 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:28:52 +0200 Subject: [PATCH 44/65] small restructure of commented code --- src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index e5934920..564f4f46 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -12,9 +12,9 @@ // #define TEENSY4_ADC_INTERRUPT_DEBUG -// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense volatile uint32_t val0, val1, val2; +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense // LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); // LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); // LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); From d6a0b26d72158ba944a3089b511c960e9afe875f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:30:51 +0200 Subject: [PATCH 45/65] a small typo --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 060ff4e2..7c4d0dbb 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifnef TEENSY4_FORCE_CENTER_ALIGNED_3PWM +#ifdef TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting From c2c4365618d2de1b7443bf9528f88184b7ca4bc0 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 09:06:26 +0200 Subject: [PATCH 46/65] enable disabling dowsanmpling #377 --- src/common/base_classes/FOCMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index d1427bcf..6ea26624 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -91,7 +91,7 @@ void FOCMotor::useMonitoring(Print &print){ // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { - if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return; + if( monitor_cnt++ < monitor_downsample ) return; monitor_cnt = 0; if(!monitor_port) return; bool printed = 0; From f796c9d0165520366925b70c231794e35f561d40 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 09:20:36 +0200 Subject: [PATCH 47/65] added the sqrt(3) factor #372 --- .../find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino | 2 +- .../hall_sensor/find_kv_rating/find_kv_rating.ino | 2 +- .../magnetic_sensor/find_kv_rating/find_kv_rating.ino | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino index 76fb46b0..c304d195 100644 --- a/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/encoder/find_kv_rating/find_kv_rating.ino @@ -36,7 +36,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino index 3abef467..575968f4 100644 --- a/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/hall_sensor/find_kv_rating/find_kv_rating.ino @@ -33,7 +33,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } diff --git a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino index f3dd74a1..a9d29d86 100644 --- a/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino +++ b/examples/utils/calibration/find_kv_rating/magnetic_sensor/find_kv_rating/find_kv_rating.ino @@ -31,7 +31,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } void calcKV(char* cmd) { // calculate the KV - Serial.println(motor.shaft_velocity/motor.target*30.0f/_PI); + Serial.println(motor.shaft_velocity/motor.target/_SQRT3*30.0f/_PI); } From 96b08040bd904b79378868306df98f3e0c33e8c6 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 09:27:11 +0200 Subject: [PATCH 48/65] another way of enabling downsample disable #377 --- src/common/base_classes/FOCMotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 6ea26624..e0a00887 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -91,7 +91,7 @@ void FOCMotor::useMonitoring(Print &print){ // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { - if( monitor_cnt++ < monitor_downsample ) return; + if( !monitor_downsample || monitor_cnt++ < (monitor_downsample-1) ) return; monitor_cnt = 0; if(!monitor_port) return; bool printed = 0; From 977b1412c6a2e5d5d3061edc44941c8db6abe043 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 19:37:31 +0200 Subject: [PATCH 49/65] added support for openloop in the initFOC #382 --- src/BLDCMotor.cpp | 50 ++++++++++++++++++++++++++------------------ src/StepperMotor.cpp | 20 ++++++++++++++---- 2 files changed, 46 insertions(+), 24 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9dbc0a48..fe14feff 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -156,25 +156,31 @@ int BLDCMotor::initFOC() { // added the shaft_angle update sensor->update(); shaft_angle = shaftAngle(); - }else { - exit_flag = 0; // no FOC without sensor - SIMPLEFOC_DEBUG("MOT: No sensor."); - } - // aligning the current sensor - can be skipped - // checks if driver phases are the same as current sense phases - // and checks the direction of measuremnt. - if(exit_flag){ - if(current_sense){ - 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(); + // aligning the current sensor - can be skipped + // checks if driver phases are the same as current sense phases + // and checks the direction of measuremnt. + if(exit_flag){ + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } } + else { SIMPLEFOC_DEBUG("MOT: No current sense."); } + } + + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor } - else { SIMPLEFOC_DEBUG("MOT: No current sense."); } } if(exit_flag){ @@ -219,6 +225,10 @@ int BLDCMotor::alignSensor() { // stop init if not found index if(!exit_flag) return exit_flag; + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction if(sensor_direction==Direction::UNKNOWN){ @@ -226,7 +236,7 @@ int BLDCMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -236,13 +246,13 @@ int BLDCMotor::alignSensor() { // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } sensor->update(); float end_angle = sensor->getAngle(); - setPhaseVoltage(0, 0, 0); + // setPhaseVoltage(0, 0, 0); _delay(200); // determine the direction the sensor moved float moved = fabs(mid_angle - end_angle); @@ -270,7 +280,7 @@ int BLDCMotor::alignSensor() { if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 697ed277..5d519f29 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -114,7 +114,15 @@ int StepperMotor::initFOC() { // added the shaft_angle update sensor->update(); shaft_angle = sensor->getAngle(); - } else { SIMPLEFOC_DEBUG("MOT: No sensor."); } + } else { + SIMPLEFOC_DEBUG("MOT: No sensor."); + if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ + exit_flag = 1; + SIMPLEFOC_DEBUG("MOT: Openloop only!"); + }else{ + exit_flag = 0; // no FOC without sensor + } + } if(exit_flag){ SIMPLEFOC_DEBUG("MOT: Ready."); @@ -133,6 +141,10 @@ int StepperMotor::alignSensor() { int exit_flag = 1; //success SIMPLEFOC_DEBUG("MOT: Align sensor."); + // v2.3.3 fix for R_AVR_7_PCREL against symbol" bug for AVR boards + // TODO figure out why this works + float voltage_align = voltage_sensor_align; + // if unknown natural direction if(sensor_direction == Direction::UNKNOWN){ // check if sensor needs zero search @@ -144,7 +156,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -154,7 +166,7 @@ int StepperMotor::alignSensor() { // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; - setPhaseVoltage(voltage_sensor_align, 0, angle); + setPhaseVoltage(voltage_align, 0, angle); sensor->update(); _delay(2); } @@ -190,7 +202,7 @@ int StepperMotor::alignSensor() { if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor // set angle -90(270 = 3PI/2) degrees - setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); + setPhaseVoltage(voltage_align, 0, _3PI_2); _delay(700); // read the sensor sensor->update(); From 65e2208ec9dd446ea345aeb4f2063c679333856c Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 20:13:49 +0200 Subject: [PATCH 50/65] readme prepare for v2.3.3 --- README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 7d8150d0..e115ee51 100644 --- a/README.md +++ b/README.md @@ -26,10 +26,14 @@ Therefore this is an attempt to: - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 -> - And more bugfixes - see the complete list of 2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) +> - Teensy4 +> - support for low-side current sensing +> - support for center aligned 6pwm and 3pwm +> - stm32 +> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) -## Arduino *SimpleFOClibrary* v2.3.2 +## Arduino *SimpleFOClibrary* v2.3.3

From af2c7c0e22e7c704a9c6f1257dbff9fde43baf66 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 20:17:10 +0200 Subject: [PATCH 51/65] readme prepare for v2.3.3 v2 --- README.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index e115ee51..29e715fe 100644 --- a/README.md +++ b/README.md @@ -27,10 +27,15 @@ Therefore this is an attempt to: > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 > - Teensy4 -> - support for low-side current sensing -> - support for center aligned 6pwm and 3pwm +> - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) +> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) > - stm32 -> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) +> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) +> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378), +> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) +> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) +> - And much more: +> - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1) ## Arduino *SimpleFOClibrary* v2.3.3 From e6b3f4f65283fe0f34f49edad42020c65d51ba90 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 07:59:07 +0200 Subject: [PATCH 52/65] Update ccpp.yml --- .github/workflows/ccpp.yml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6c396ea9..bcb316df 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,7 +26,8 @@ jobs: - 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:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 @@ -92,6 +93,10 @@ jobs: 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, double_full_control_example.ino, stm32_current_control_low_side.ino + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # 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, double_full_control_example.ino, stm32_current_control_low_side.ino + # Do not cancel all jobs / architectures if one job fails fail-fast: false From 1d92c3da5a6b5d2566f946124f52c55591fbc101 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:02:50 +0200 Subject: [PATCH 53/65] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index bcb316df..c19b651f 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -27,7 +27,7 @@ jobs: - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg + - STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 @@ -93,7 +93,7 @@ jobs: 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, double_full_control_example.ino, stm32_current_control_low_side.ino - - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F746ZG # nucleo one full example + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # nucleo f7 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, double_full_control_example.ino, stm32_current_control_low_side.ino From be24ec7346f1bec7e4c4b2b3cc17de933e1c0705 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:09:23 +0200 Subject: [PATCH 54/65] Update ccpp.yml --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index c19b651f..fa3cf972 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -27,7 +27,7 @@ jobs: - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # stm32 nucleo f746zg + - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 @@ -93,7 +93,7 @@ jobs: 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, double_full_control_example.ino, stm32_current_control_low_side.ino - - arduino-boards-fqbn: STMicroelectronics:stm32:GenF7:pnum=GENERIC_F746ZGTX # nucleo f7 one full example + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # nucleo f7 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, double_full_control_example.ino, stm32_current_control_low_side.ino From e8421ed2f001ad6eb8064fbecd9f0cb9142eb0b3 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:23:14 +0200 Subject: [PATCH 55/65] added teesny low-side example, and complted the readme with f7 --- README.md | 11 +- .../teensy4_current_control_low_side.ino | 170 ++++++++++++++++++ 2 files changed, 175 insertions(+), 6 deletions(-) create mode 100644 examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino diff --git a/README.md b/README.md index 29e715fe..5b311224 100644 --- a/README.md +++ b/README.md @@ -19,11 +19,9 @@ Additionally, most of the efforts at this moment are still channeled towards the Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there -- 🎯 Develop a modular FOC supporting BLDC driver boards: - - ***NEW*** 📢: *Minimalistic* BLDC driver (<3Amps) : [SimpleFOCMini ](https://github.com/simplefoc/SimpleFOCMini). - - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase). - - *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) +- 🎯 Develop modular and easy to use FOC supporting BLDC driver boards + - For official driver boards see [SimpleFOCBoards](boards) + - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 > - Teensy4 @@ -31,7 +29,8 @@ Therefore this is an attempt to: > - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392) > - stm32 > - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388) -> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378), +> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378) +> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394) > - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347) > - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340) > - And much more: diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino new file mode 100644 index 00000000..67fec8a1 --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -0,0 +1,170 @@ +/** + * 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 EN_GATE 11 +#define M_PWM 22 +#define GAIN 20 +#define M_OC 23 +#define OC_ADJ 19 + +#define INH_A 2 +#define INL_A 3 +#define INH_B 8 +#define INL_B 7 +#define INH_C 6 +#define INL_C 9 + +#define IOUTA 14 +#define IOUTB 15 +#define IOUTC 16 + +// 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); + +// encoder instance +Encoder encoder = Encoder(10, 11, 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 6pwm mode + pinMode(M_PWM, OUTPUT); + digitalWrite(M_PWM,LOW); // high for 3pwm + // 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 = 20000; // suggested not higher than 22khz + 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 719279147bf92ff1a1b0b7e693dd4c1cc6a33244 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:24:34 +0200 Subject: [PATCH 56/65] typo in readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5b311224..3323a794 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ Therefore this is an attempt to: - 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase) - Support as many motor + sensor + driver + mcu combinations out there - 🎯 Develop modular and easy to use FOC supporting BLDC driver boards - - For official driver boards see [SimpleFOCBoards](boards) + - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards) - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/) > NEW RELEASE 📢 : SimpleFOClibrary v2.3.3 From 4fa7cf2b0fccb36abcd318c8c20d19b69bf182fa Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:27:11 +0200 Subject: [PATCH 57/65] Update teensy.yml --- .github/workflows/teensy.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index 9fc88b9a..f1f89185 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -29,6 +29,11 @@ jobs: run: pio ci --lib="." --board=teensy41 --board=teensy40 env: PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino + + - name: PIO Run Teensy 4 + run: pio ci --lib="." --board=teensy41 --board=teensy40 + env: + PLATFORMIO_CI_SRC: examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino - name: PIO Run Teensy 3 run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36 From cc6dfa06fac30db7f1727040dd7635689e694115 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:31:19 +0200 Subject: [PATCH 58/65] typo in the example --- .../teensy4_current_control_low_side.ino | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino index 67fec8a1..841134d8 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino @@ -71,8 +71,6 @@ void setup() { // 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 From 4d33cd79eaa99b2e3035cbe9669cc5661b3a1c11 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 08:37:51 +0200 Subject: [PATCH 59/65] Update ccpp.yml --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6c396ea9..0ffbed91 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -36,7 +36,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From 3942e88e09592a37dd21d426769ba6e32a04d97d Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 08:45:26 +0200 Subject: [PATCH 60/65] remove teensy sketch form uno --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index fa3cf972..f870e780 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -37,7 +37,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino From c54623de5efd12f8c8f958146222564d1675dff9 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 10:08:28 +0200 Subject: [PATCH 61/65] restructured the CI --- .github/workflows/arduino.yml | 53 +++++++++++++++++++++++ .github/workflows/esp32.yml | 50 +++++++++++++++++++++ .github/workflows/rpi.yml | 39 +++++++++++++++++ .github/workflows/samd.yml | 44 +++++++++++++++++++ .github/workflows/{ccpp.yml => stm32.yml} | 50 +-------------------- .github/workflows/teensy.yml | 2 +- README.md | 7 ++- 7 files changed, 195 insertions(+), 50 deletions(-) create mode 100644 .github/workflows/arduino.yml create mode 100644 .github/workflows/esp32.yml create mode 100644 .github/workflows/rpi.yml create mode 100644 .github/workflows/samd.yml rename .github/workflows/{ccpp.yml => stm32.yml} (52%) diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml new file mode 100644 index 00000000..639526ac --- /dev/null +++ b/.github/workflows/arduino.yml @@ -0,0 +1,53 @@ +name: AVR build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:avr:uno # arudino uno + - arduino:sam:arduino_due_x # arduino due + - arduino:avr:mega # arduino mega2650 + - arduino:avr:leonardo # arduino leonardo + + include: + - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples + sketch-names: '**.ino' + required-libraries: PciManager + sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm + + - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example + sketch-names: single_full_control_example.ino + + - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example + sketch-names: open_loop_position_example.ino + + - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example + sketch-names: single_full_control_example.ino + + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml new file mode 100644 index 00000000..12f8e3b8 --- /dev/null +++ b/.github/workflows/esp32.yml @@ -0,0 +1,50 @@ +name: ESP32 build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - esp32:esp32:esp32doit-devkit-v1 # esp32 + - esp32:esp32:esp32s2 # esp32s2 + - esp32:esp32:esp32s3 # esp32s3 + + include: + + - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino + + - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 + platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/rpi.yml b/.github/workflows/rpi.yml new file mode 100644 index 00000000..ac18a920 --- /dev/null +++ b/.github/workflows/rpi.yml @@ -0,0 +1,39 @@ +name: RP2040 build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:mbed_rp2040:pico # rpi pico + + include: + + - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example + sketch-names: open_loop_position_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/samd.yml b/.github/workflows/samd.yml new file mode 100644 index 00000000..a31c0788 --- /dev/null +++ b/.github/workflows/samd.yml @@ -0,0 +1,44 @@ +name: SAMD build +on: [push, pull_request] +jobs: + lint: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: arduino/arduino-lint-action@v1 + with: + library-manager: update + project-type: library + build: + name: Test compiling + runs-on: ubuntu-latest + + strategy: + matrix: + arduino-boards-fqbn: + - arduino:samd:nano_33_iot # samd21 + - adafruit:samd:adafruit_metro_m4 # samd51 + + include: + + - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 + sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino + + - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example + platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json + sketch-names: single_full_control_example.ino + + # Do not cancel all jobs / architectures if one job fails + fail-fast: false + steps: + - name: Checkout + uses: actions/checkout@master + - name: Compile all examples + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + required-libraries: ${{ matrix.required-libraries }} + platform-url: ${{ matrix.platform-url }} + sketch-names: ${{ matrix.sketch-names }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/.github/workflows/ccpp.yml b/.github/workflows/stm32.yml similarity index 52% rename from .github/workflows/ccpp.yml rename to .github/workflows/stm32.yml index f870e780..50224a1f 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/stm32.yml @@ -1,4 +1,4 @@ -name: Library Compile +name: STM32 build on: [push, pull_request] jobs: lint: @@ -16,60 +16,14 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - arduino:avr:uno # arudino uno - - arduino:sam:arduino_due_x # arduino due - - arduino:avr:mega # arduino mega2650 - - arduino:avr:leonardo # arduino leonardo - - arduino:samd:nano_33_iot # samd21 - - 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:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 - - arduino:mbed_rp2040:pico # rpi pico - - include: - - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples - sketch-names: '**.ino' - required-libraries: PciManager - sketches-exclude: teensy4_current_control_low_side, full_control_serial, angle_control, bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm - - - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:avr:leonardo # arduino leonardo - one full example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: arduino:avr:mega # arduino mega2660 - one full example - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: arduino:samd:nano_33_iot # samd21 - sketch-names: nano33IoT_velocity_control.ino, smartstepper_control.ino - - - arduino-boards-fqbn: arduino:mbed_rp2040:pico # raspberry pi pico - one example - sketch-names: open_loop_position_example.ino - - - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 # samd51 - one full example - platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json - sketch-names: single_full_control_example.ino - - - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino - - - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino - - - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 - platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json - sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino + include: - 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 diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index f1f89185..e126c616 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -1,4 +1,4 @@ -name: PlatformIO - Teensy build +name: Teensy build on: [push] diff --git a/README.md b/README.md index 3323a794..adbdd09a 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,12 @@ # 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) [![PlatformIO - Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) +![AVR](https://github.com/simplefoc/Arduino-FOC/workflows/AVR%20build/badge.svg) +![ESP32](https://github.com/simplefoc/Arduino-FOC/workflows/ESP32%20build/badge.svg) +![STM32](https://github.com/simplefoc/Arduino-FOC/workflows/STM32%20build/badge.svg) +![RP2040](https://github.com/simplefoc/Arduino-FOC/workflows/RP2040%20build/badge.svg) +![SAMD](https://github.com/simplefoc/Arduino-FOC/workflows/SAMD%20build/badge.svg) +[![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) From 5c9e32a28b50597b9cc5051caab5cfef7a0f4f48 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 10:13:07 +0200 Subject: [PATCH 62/65] added links to the new CI to readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index adbdd09a..86551f79 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,11 @@ # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO -![AVR](https://github.com/simplefoc/Arduino-FOC/workflows/AVR%20build/badge.svg) -![ESP32](https://github.com/simplefoc/Arduino-FOC/workflows/ESP32%20build/badge.svg) -![STM32](https://github.com/simplefoc/Arduino-FOC/workflows/STM32%20build/badge.svg) -![RP2040](https://github.com/simplefoc/Arduino-FOC/workflows/RP2040%20build/badge.svg) -![SAMD](https://github.com/simplefoc/Arduino-FOC/workflows/SAMD%20build/badge.svg) +[![AVR build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/arduino.yml) +[![STM32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/stm32.yml) +[![ESP32 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/esp32.yml) +[![RP2040 build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/rpi.yml) +[![SAMD build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/samd.yml) [![Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) From 26c631d5a48aad09025211cd917e821f1094de0e Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 21 Apr 2024 10:19:37 +0200 Subject: [PATCH 63/65] renamed the actions --- .github/workflows/arduino.yml | 2 +- .github/workflows/esp32.yml | 2 +- .github/workflows/rpi.yml | 2 +- .github/workflows/samd.yml | 2 +- .github/workflows/stm32.yml | 2 +- .github/workflows/teensy.yml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml index 639526ac..cadd8ccc 100644 --- a/.github/workflows/arduino.yml +++ b/.github/workflows/arduino.yml @@ -1,4 +1,4 @@ -name: AVR build +name: AVR on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/esp32.yml b/.github/workflows/esp32.yml index 12f8e3b8..e0899bf1 100644 --- a/.github/workflows/esp32.yml +++ b/.github/workflows/esp32.yml @@ -1,4 +1,4 @@ -name: ESP32 build +name: ESP32 on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/rpi.yml b/.github/workflows/rpi.yml index ac18a920..d6d72b95 100644 --- a/.github/workflows/rpi.yml +++ b/.github/workflows/rpi.yml @@ -1,4 +1,4 @@ -name: RP2040 build +name: RP2040 on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/samd.yml b/.github/workflows/samd.yml index a31c0788..c4329869 100644 --- a/.github/workflows/samd.yml +++ b/.github/workflows/samd.yml @@ -1,4 +1,4 @@ -name: SAMD build +name: SAMD on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/stm32.yml b/.github/workflows/stm32.yml index 50224a1f..52b5cc94 100644 --- a/.github/workflows/stm32.yml +++ b/.github/workflows/stm32.yml @@ -1,4 +1,4 @@ -name: STM32 build +name: STM32 on: [push, pull_request] jobs: lint: diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml index e126c616..6ad953fe 100644 --- a/.github/workflows/teensy.yml +++ b/.github/workflows/teensy.yml @@ -1,4 +1,4 @@ -name: Teensy build +name: Teensy on: [push] From abda7543c78006cde7ae84fc76678bd4a56c1ca2 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 16:54:28 +0200 Subject: [PATCH 64/65] Update teensy4_mcu.cpp --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 7c4d0dbb..47108447 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -10,7 +10,7 @@ #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") -// #define TEENSY4_FORCE_CENTER_ALIGNED_3PWM +// #define SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifdef TEENSY4_FORCE_CENTER_ALIGNED_3PWM +#ifdef SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting @@ -663,4 +663,4 @@ void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ #endif -#endif \ No newline at end of file +#endif From 6fafa35b73e9c5ba6f5d9259640a5ed328dc2750 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Sun, 21 Apr 2024 16:55:11 +0200 Subject: [PATCH 65/65] Update teensy4_mcu.cpp --- .../hardware_specific/teensy/teensy4_mcu.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 564f4f46..70815d0f 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -9,7 +9,7 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) -// #define TEENSY4_ADC_INTERRUPT_DEBUG +// #define SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG volatile uint32_t val0, val1, val2; @@ -28,7 +28,7 @@ void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); #endif // page 3509 , section 66.5.1.3.3 @@ -37,21 +37,21 @@ void adcetc0_isr() { val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); #endif } void adcetc1_isr() { -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); #endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; // val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); #endif } @@ -167,7 +167,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); #endif @@ -228,7 +228,7 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center -#ifdef TEENSY4_ADC_INTERRUPT_DEBUG +#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG // pin 4 observes out trigger line for 'scope xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 @@ -241,4 +241,4 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ } -#endif \ No newline at end of file +#endif