Skip to content

Commit 37e1a73

Browse files
askuricaskuric
committed
initial tested low-side current sensing for f1 and f4
1 parent 0400c38 commit 37e1a73

File tree

12 files changed

+458
-88
lines changed

12 files changed

+458
-88
lines changed

‎README.md

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,9 +24,15 @@ Therefore this is an attempt to:
2424
> - Low level API restructuring
2525
> - Driver API
2626
> - Current sense API
27+
> - New debugging interface
28+
> - Static class SimpleFOCDebug
2729
> - Low-side current sensing
2830
> - ESP32 generic support for multiple motors
29-
> - New handlign of current limit using voltage
31+
> - Added low-side current sensing support for stm32 - only one motor
32+
> - f1 family
33+
> - f4 family
34+
> - g4 family
35+
> - New handling of current limit using voltage
3036
> - Support for motor KV rating - back emf estimation
3137
> - Using motor phase resistance
3238

‎keywords.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ Commander KEYWORD1
2424
StepDirListener KEYWORD1
2525
GenericCurrentSense KEYWORD1
2626
GenericSensor KEYWORD1
27-
27+
SimpleFOCDebug KEYWORD1
2828

2929
initFOC KEYWORD2
3030
loopFOC KEYWORD2

‎src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp

Lines changed: 21 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -132,10 +132,27 @@ void DMA1_Channel2_IRQHandler(void) {
132132
}
133133
}
134134

135-
void _driverSyncLowSide(void* driver_params, void* cs_params){
136-
_UNUSED(cs_params);
137-
// Set Trigger out for DMA transfer
138-
LL_TIM_SetTriggerOutput(((STM32DriverParams*)driver_params)->timers[0]->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
135+
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
136+
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
137+
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
138+
139+
// stop all the timers for the driver
140+
_stopTimers(driver_params->timers, 6);
141+
142+
// if timer has repetition counter - it will downsample using it
143+
// and it does not need the software downsample
144+
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
145+
// adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs.
146+
// only necessary for the timers that have repetition counters
147+
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
148+
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
149+
}
150+
// set the trigger output event
151+
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
152+
153+
// restart all the timers of the driver
154+
_startTimers(driver_params->timers, 6);
155+
139156
}
140157

141158
#endif

‎src/current_sense/hardware_specific/stm32/stm32_mcu.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@
1313
typedef struct Stm32CurrentSenseParams {
1414
int pins[3] = {(int)NOT_SET};
1515
float adc_voltage_conv;
16-
ADC_HandleTypeDef* adc_handle = NULL;
17-
HardwareTimer* timer_handle = NULL;
16+
ADC_HandleTypeDef* adc_handle = NP;
17+
HardwareTimer* timer_handle = NP;
1818
} Stm32CurrentSenseParams;
1919

2020
#endif

‎src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp

Lines changed: 162 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,162 @@
1+
#include "stm32f1_hal.h"
2+
3+
#if defined(STM32F1xx)
4+
5+
#include "../../../../communication/SimpleFOCDebug.h"
6+
#define _TRGO_NOT_AVAILABLE 12345
7+
8+
// timer to injected TRGO
9+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215
10+
uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
11+
if(timer->getHandle()->Instance == TIM1)
12+
return ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
13+
#ifdef TIM2 // if defined timer 2
14+
else if(timer->getHandle()->Instance == TIM2)
15+
return ADC_EXTERNALTRIGINJECCONV_T2_TRGO;
16+
#endif
17+
#ifdef TIM4 // if defined timer 4
18+
else if(timer->getHandle()->Instance == TIM4)
19+
return ADC_EXTERNALTRIGINJECCONV_T4_TRGO;
20+
#endif
21+
#ifdef TIM5 // if defined timer 5
22+
else if(timer->getHandle()->Instance == TIM5)
23+
return ADC_EXTERNALTRIGINJECCONV_T5_TRGO;
24+
#endif
25+
else
26+
return _TRGO_NOT_AVAILABLE;
27+
}
28+
29+
// timer to regular TRGO
30+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215
31+
uint32_t _timerToRegularTRGO(HardwareTimer* timer){
32+
if(timer->getHandle()->Instance == TIM3)
33+
return ADC_EXTERNALTRIGCONV_T3_TRGO;
34+
#ifdef TIM8 // if defined timer 8
35+
else if(timer->getHandle()->Instance == TIM8)
36+
return ADC_EXTERNALTRIGINJECCONV_T8_TRGO;
37+
#endif
38+
else
39+
return _TRGO_NOT_AVAILABLE;
40+
}
41+
42+
ADC_HandleTypeDef hadc;
43+
44+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
45+
{
46+
ADC_InjectionConfTypeDef sConfigInjected;
47+
48+
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
49+
*/
50+
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
51+
if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
52+
#ifdef ADC2 // if defined ADC2
53+
else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
54+
#endif
55+
#ifdef ADC3 // if defined ADC3
56+
else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
57+
#endif
58+
else{
59+
#ifdef SIMPLEFOC_STM32_DEBUG
60+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
61+
#endif
62+
return -1; // error not a valid ADC instance
63+
}
64+
65+
hadc.Init.ScanConvMode = ADC_SCAN_ENABLE;
66+
hadc.Init.ContinuousConvMode = ENABLE;
67+
hadc.Init.DiscontinuousConvMode = DISABLE;
68+
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START;
69+
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
70+
hadc.Init.NbrOfConversion = 0;
71+
HAL_ADC_Init(&hadc);
72+
/**Configure for the selected ADC regular channel to be converted.
73+
*/
74+
75+
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
76+
*/
77+
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
78+
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5;
79+
sConfigInjected.AutoInjectedConv = DISABLE;
80+
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
81+
sConfigInjected.InjectedOffset = 0;
82+
83+
// automating TRGO flag finding - hardware specific
84+
uint8_t tim_num = 0;
85+
while(driver_params->timers[tim_num] != NP && tim_num < 6){
86+
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
87+
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
88+
89+
// if the code comes here, it has found the timer available
90+
// timer does have trgo flag for injected channels
91+
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
92+
93+
// this will be the timer with which the ADC will sync
94+
cs_params->timer_handle = driver_params->timers[tim_num-1];
95+
// done
96+
break;
97+
}
98+
if( cs_params->timer_handle == NP ){
99+
// not possible to use these timers for low-side current sense
100+
#ifdef SIMPLEFOC_STM32_DEBUG
101+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
102+
#endif
103+
return -1;
104+
}
105+
// display which timer is being used
106+
#ifdef SIMPLEFOC_STM32_DEBUG
107+
// it would be better to use the getTimerNumber from driver
108+
SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
109+
#endif
110+
111+
// first channel
112+
sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1;
113+
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC));
114+
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
115+
// second channel
116+
sConfigInjected.InjectedRank = ADC_REGULAR_RANK_2;
117+
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC));
118+
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
119+
120+
// third channel - if exists
121+
if(_isset(cs_params->pins[2])){
122+
sConfigInjected.InjectedRank = ADC_REGULAR_RANK_3;
123+
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC));
124+
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
125+
}
126+
127+
// enable interrupt
128+
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
129+
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
130+
131+
cs_params->adc_handle = &hadc;
132+
133+
return 0;
134+
}
135+
136+
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
137+
{
138+
uint8_t cnt = 0;
139+
if(_isset(pinA)){
140+
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
141+
cs_params->pins[cnt++] = pinA;
142+
}
143+
if(_isset(pinB)){
144+
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
145+
cs_params->pins[cnt++] = pinB;
146+
}
147+
if(_isset(pinC)){
148+
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
149+
cs_params->pins[cnt] = pinC;
150+
}
151+
152+
}
153+
154+
extern "C" {
155+
void ADC1_2_IRQHandler(void)
156+
{
157+
HAL_ADC_IRQHandler(&hadc);
158+
}
159+
160+
}
161+
162+
#endif

‎src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
#ifndef STM32F1_LOWSIDE_HAL
2+
#define STM32F1_LOWSIDE_HAL
3+
4+
#include "Arduino.h"
5+
6+
#if defined(STM32F1xx)
7+
#include "stm32f1xx_hal.h"
8+
#include "../../../../common/foc_utils.h"
9+
#include "../../../../drivers/hardware_specific/stm32_mcu.h"
10+
#include "../stm32_mcu.h"
11+
12+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
13+
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
14+
15+
#endif
16+
17+
#endif

‎src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
#include "../../../hardware_api.h"
2+
3+
#if defined(STM32F1xx)
4+
#include "../../../../common/foc_utils.h"
5+
#include "../../../../drivers/hardware_api.h"
6+
#include "../../../../drivers/hardware_specific/stm32_mcu.h"
7+
#include "../../../hardware_api.h"
8+
#include "../stm32_mcu.h"
9+
#include "stm32f1_hal.h"
10+
#include "Arduino.h"
11+
12+
#define _ADC_VOLTAGE_F1 3.3f
13+
#define _ADC_RESOLUTION_F1 4096.0f
14+
15+
// array of values of 4 injected channels per adc instance (3)
16+
uint32_t adc_val[3][4]={0};
17+
// does adc interrupt need a downsample - per adc (3)
18+
bool needs_downsample[3] = {1};
19+
// downsampling variable - per adc (3)
20+
uint8_t tim_downsample[3] = {0};
21+
22+
int _adcToIndex(ADC_HandleTypeDef *AdcHandle){
23+
if(AdcHandle->Instance == ADC1) return 0;
24+
#ifdef ADC2 // if ADC2 exists
25+
else if(AdcHandle->Instance == ADC2) return 1;
26+
#endif
27+
#ifdef ADC3 // if ADC3 exists
28+
else if(AdcHandle->Instance == ADC3) return 2;
29+
#endif
30+
return 0;
31+
}
32+
33+
void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
34+
35+
Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
36+
.pins={0},
37+
.adc_voltage_conv = (_ADC_VOLTAGE_F1) / (_ADC_RESOLUTION_F1)
38+
};
39+
_adc_gpio_init(cs_params, pinA,pinB,pinC);
40+
if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
41+
return cs_params;
42+
}
43+
44+
45+
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
46+
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
47+
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
48+
49+
// if compatible timer has not been found
50+
if (cs_params->timer_handle == NULL) return;
51+
52+
// stop all the timers for the driver
53+
_stopTimers(driver_params->timers, 6);
54+
55+
// if timer has repetition counter - it will downsample using it
56+
// and it does not need the software downsample
57+
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
58+
// adjust the initial timer state such that the trigger
59+
// - for DMA transfer aligns with the pwm peaks instead of throughs.
60+
// - for interrupt based ADC transfer
61+
// - only necessary for the timers that have repetition counters
62+
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
63+
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
64+
// remember that this timer has repetition counter - no need to downasmple
65+
needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
66+
}
67+
// set the trigger output event
68+
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
69+
// start the adc
70+
HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
71+
72+
// restart all the timers of the driver
73+
_startTimers(driver_params->timers, 6);
74+
}
75+
76+
77+
// function reading an ADC value and returning the read voltage
78+
float _readADCVoltageLowSide(const int pin, const void* cs_params){
79+
for(int i=0; i < 3; i++){
80+
if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
81+
return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
82+
}
83+
return 0;
84+
}
85+
86+
87+
extern "C" {
88+
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
89+
// calculate the instance
90+
int adc_index = _adcToIndex(AdcHandle);
91+
92+
// if the timer han't repetition counter - downsample two times
93+
if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
94+
tim_downsample[adc_index] = 0;
95+
return;
96+
}
97+
98+
adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
99+
adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
100+
adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
101+
}
102+
}
103+
104+
#endif

0 commit comments

Comments
 (0)