Skip to content

Commit a8a78e8

Browse files
runger1101001runger1101001
authored
Merge pull request #257 from runger1101001/dev
STM32L4xx current sense by @Triple6 (discord)
2 parents 2dc0498 + be10f53 commit a8a78e8

File tree

7 files changed

+641
-0
lines changed

7 files changed

+641
-0
lines changed

‎README.md

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,8 @@ Therefore this is an attempt to:
3333
> - decimal places (settable through commander)
3434
> - bugfix - current sense align - added offset exchange when exchanging pins
3535
> - bugfix - trapezoid 150 fixed
36+
> - bugfix - MagneticSensorPWM bugfixes
37+
> - current sensing support for STM32L4xx series thanks to @triple6 (discord)
3638
> - phase disable in 6pwm mode
3739
> - stm32 - software and hardware 6pwm
3840
> - atmega328

‎src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp

Lines changed: 266 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,266 @@
1+
#include "stm32l4_hal.h"
2+
3+
#if defined(STM32L4xx)
4+
5+
#include "../../../../communication/SimpleFOCDebug.h"
6+
7+
#define SIMPLEFOC_STM32_DEBUG
8+
9+
ADC_HandleTypeDef hadc;
10+
11+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
12+
{
13+
ADC_InjectionConfTypeDef sConfigInjected;
14+
15+
// check if all pins belong to the same ADC
16+
ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
17+
ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
18+
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
19+
if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
20+
#ifdef SIMPLEFOC_STM32_DEBUG
21+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
22+
#endif
23+
return -1;
24+
}
25+
26+
27+
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
28+
*/
29+
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
30+
31+
if(hadc.Instance == ADC1) {
32+
#ifdef __HAL_RCC_ADC1_CLK_ENABLE
33+
__HAL_RCC_ADC1_CLK_ENABLE();
34+
#endif
35+
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
36+
__HAL_RCC_ADC12_CLK_ENABLE();
37+
#endif
38+
}
39+
#ifdef ADC2
40+
else if (hadc.Instance == ADC2) {
41+
#ifdef __HAL_RCC_ADC2_CLK_ENABLE
42+
__HAL_RCC_ADC2_CLK_ENABLE();
43+
#endif
44+
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
45+
__HAL_RCC_ADC12_CLK_ENABLE();
46+
#endif
47+
}
48+
#endif
49+
#ifdef ADC3
50+
else if (hadc.Instance == ADC3) {
51+
#ifdef __HAL_RCC_ADC3_CLK_ENABLE
52+
__HAL_RCC_ADC3_CLK_ENABLE();
53+
#endif
54+
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
55+
__HAL_RCC_ADC34_CLK_ENABLE();
56+
#endif
57+
#if defined(ADC345_COMMON)
58+
__HAL_RCC_ADC345_CLK_ENABLE();
59+
#endif
60+
}
61+
#endif
62+
#ifdef ADC4
63+
else if (hadc.Instance == ADC4) {
64+
#ifdef __HAL_RCC_ADC4_CLK_ENABLE
65+
__HAL_RCC_ADC4_CLK_ENABLE();
66+
#endif
67+
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
68+
__HAL_RCC_ADC34_CLK_ENABLE();
69+
#endif
70+
#if defined(ADC345_COMMON)
71+
__HAL_RCC_ADC345_CLK_ENABLE();
72+
#endif
73+
}
74+
#endif
75+
#ifdef ADC5
76+
else if (hadc.Instance == ADC5) {
77+
#if defined(ADC345_COMMON)
78+
__HAL_RCC_ADC345_CLK_ENABLE();
79+
#endif
80+
}
81+
#endif
82+
else{
83+
#ifdef SIMPLEFOC_STM32_DEBUG
84+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
85+
#endif
86+
return -1; // error not a valid ADC instance
87+
}
88+
89+
#ifdef SIMPLEFOC_STM32_DEBUG
90+
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
91+
#endif
92+
93+
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
94+
hadc.Init.Resolution = ADC_RESOLUTION_12B;
95+
hadc.Init.ScanConvMode = ADC_SCAN_ENABLE;
96+
hadc.Init.ContinuousConvMode = DISABLE;
97+
hadc.Init.LowPowerAutoWait = DISABLE;
98+
hadc.Init.DiscontinuousConvMode = DISABLE;
99+
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
100+
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
101+
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
102+
hadc.Init.NbrOfConversion = 2;
103+
hadc.Init.DMAContinuousRequests = DISABLE;
104+
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
105+
hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED;
106+
if ( HAL_ADC_Init(&hadc) != HAL_OK){
107+
#ifdef SIMPLEFOC_STM32_DEBUG
108+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
109+
#endif
110+
return -1;
111+
}
112+
113+
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
114+
*/
115+
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
116+
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5;
117+
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;
118+
sConfigInjected.AutoInjectedConv = DISABLE;
119+
sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;
120+
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
121+
sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE;
122+
sConfigInjected.InjectedOffset = 0;
123+
sConfigInjected.InjecOversamplingMode = DISABLE;
124+
sConfigInjected.QueueInjectedContext = DISABLE;
125+
126+
// automating TRGO flag finding - hardware specific
127+
uint8_t tim_num = 0;
128+
while(driver_params->timers[tim_num] != NP && tim_num < 6){
129+
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
130+
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
131+
132+
// if the code comes here, it has found the timer available
133+
// timer does have trgo flag for injected channels
134+
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
135+
136+
// this will be the timer with which the ADC will sync
137+
cs_params->timer_handle = driver_params->timers[tim_num-1];
138+
// done
139+
break;
140+
}
141+
if( cs_params->timer_handle == NP ){
142+
// not possible to use these timers for low-side current sense
143+
#ifdef SIMPLEFOC_STM32_DEBUG
144+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
145+
#endif
146+
return -1;
147+
}
148+
149+
150+
// first channel
151+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
152+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
153+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
154+
#ifdef SIMPLEFOC_STM32_DEBUG
155+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
156+
#endif
157+
return -1;
158+
}
159+
160+
// second channel
161+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
162+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
163+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
164+
#ifdef SIMPLEFOC_STM32_DEBUG
165+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
166+
#endif
167+
return -1;
168+
}
169+
170+
// third channel - if exists
171+
if(_isset(cs_params->pins[2])){
172+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
173+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
174+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
175+
#ifdef SIMPLEFOC_STM32_DEBUG
176+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
177+
#endif
178+
return -1;
179+
}
180+
}
181+
182+
183+
184+
if(hadc.Instance == ADC1) {
185+
// enable interrupt
186+
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
187+
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
188+
}
189+
#ifdef ADC2
190+
else if (hadc.Instance == ADC2) {
191+
// enable interrupt
192+
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
193+
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
194+
}
195+
#endif
196+
#ifdef ADC3
197+
else if (hadc.Instance == ADC3) {
198+
// enable interrupt
199+
HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0);
200+
HAL_NVIC_EnableIRQ(ADC3_IRQn);
201+
}
202+
#endif
203+
#ifdef ADC4
204+
else if (hadc.Instance == ADC4) {
205+
// enable interrupt
206+
HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0);
207+
HAL_NVIC_EnableIRQ(ADC4_IRQn);
208+
}
209+
#endif
210+
#ifdef ADC5
211+
else if (hadc.Instance == ADC5) {
212+
// enable interrupt
213+
HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0);
214+
HAL_NVIC_EnableIRQ(ADC5_IRQn);
215+
}
216+
#endif
217+
218+
cs_params->adc_handle = &hadc;
219+
return 0;
220+
}
221+
222+
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
223+
{
224+
uint8_t cnt = 0;
225+
if(_isset(pinA)){
226+
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
227+
cs_params->pins[cnt++] = pinA;
228+
}
229+
if(_isset(pinB)){
230+
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
231+
cs_params->pins[cnt++] = pinB;
232+
}
233+
if(_isset(pinC)){
234+
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
235+
cs_params->pins[cnt] = pinC;
236+
}
237+
}
238+
239+
extern "C" {
240+
void ADC1_2_IRQHandler(void)
241+
{
242+
HAL_ADC_IRQHandler(&hadc);
243+
}
244+
#ifdef ADC3
245+
void ADC3_IRQHandler(void)
246+
{
247+
HAL_ADC_IRQHandler(&hadc);
248+
}
249+
#endif
250+
251+
#ifdef ADC4
252+
void ADC4_IRQHandler(void)
253+
{
254+
HAL_ADC_IRQHandler(&hadc);
255+
}
256+
#endif
257+
258+
#ifdef ADC5
259+
void ADC5_IRQHandler(void)
260+
{
261+
HAL_ADC_IRQHandler(&hadc);
262+
}
263+
#endif
264+
}
265+
266+
#endif

‎src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h

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

‎src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp

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

0 commit comments

Comments
 (0)