Skip to content

Commit 6ecb091

Browse files
askuricaskuric
committed
g4 initial push
1 parent c02da01 commit 6ecb091

File tree

3 files changed

+363
-0
lines changed

3 files changed

+363
-0
lines changed

‎src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp

Lines changed: 238 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,238 @@
1+
#include "stm32g4_hal.h"
2+
3+
#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1)
4+
5+
#include "../../../../communication/SimpleFOCDebug.h"
6+
#define _TRGO_NOT_AVAILABLE 12345
7+
8+
9+
// timer to injected TRGO
10+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217
11+
uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
12+
if(timer->getHandle()->Instance == TIM1)
13+
return ADC_EXTERNALTRIGINJEC_T1_TRGO;
14+
#ifdef TIM2 // if defined timer 2
15+
else if(timer->getHandle()->Instance == TIM2)
16+
return ADC_EXTERNALTRIGINJEC_T2_TRGO;
17+
#endif
18+
#ifdef TIM3 // if defined timer 3
19+
else if(timer->getHandle()->Instance == TIM2)
20+
return ADC_EXTERNALTRIGINJEC_T3_TRGO;
21+
#endif
22+
#ifdef TIM4 // if defined timer 4
23+
else if(timer->getHandle()->Instance == TIM4)
24+
return ADC_EXTERNALTRIGINJEC_T4_TRGO;
25+
#endif
26+
#ifdef TIM6 // if defined timer 6
27+
else if(timer->getHandle()->Instance == TIM6)
28+
return ADC_EXTERNALTRIGINJEC_T6_TRGO;
29+
#endif
30+
#ifdef TIM7 // if defined timer 7
31+
else if(timer->getHandle()->Instance == TIM7)
32+
return ADC_EXTERNALTRIGINJEC_T7_TRGO;
33+
#endif
34+
#ifdef TIM8 // if defined timer 8
35+
else if(timer->getHandle()->Instance == TIM8)
36+
return ADC_EXTERNALTRIGINJEC_T8_TRGO;
37+
#endif
38+
#ifdef TIM15 // if defined timer 15
39+
else if(timer->getHandle()->Instance == TIM15)
40+
return ADC_EXTERNALTRIGINJEC_T15_TRGO;
41+
#endif
42+
#ifdef TIM20 // if defined timer 15
43+
else if(timer->getHandle()->Instance == TIM20)
44+
return ADC_EXTERNALTRIGINJEC_T20_TRGO;
45+
#endif
46+
else
47+
return _TRGO_NOT_AVAILABLE;
48+
}
49+
50+
// timer to regular TRGO
51+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519
52+
uint32_t _timerToRegularTRGO(HardwareTimer* timer){
53+
if(timer->getHandle()->Instance == TIM1)
54+
return ADC_EXTERNALTRIG_T1_TRGO;
55+
#ifdef TIM2 // if defined timer 2
56+
else if(timer->getHandle()->Instance == TIM2)
57+
return ADC_EXTERNALTRIG_T2_TRGO;
58+
#endif
59+
#ifdef TIM3 // if defined timer 3
60+
else if(timer->getHandle()->Instance == TIM2)
61+
return ADC_EXTERNALTRIG_T3_TRGO;
62+
#endif
63+
#ifdef TIM4 // if defined timer 4
64+
else if(timer->getHandle()->Instance == TIM4)
65+
return ADC_EXTERNALTRIG_T4_TRGO;
66+
#endif
67+
#ifdef TIM6 // if defined timer 6
68+
else if(timer->getHandle()->Instance == TIM6)
69+
return ADC_EXTERNALTRIG_T6_TRGO;
70+
#endif
71+
#ifdef TIM7 // if defined timer 7
72+
else if(timer->getHandle()->Instance == TIM7)
73+
return ADC_EXTERNALTRIG_T7_TRGO;
74+
#endif
75+
#ifdef TIM8 // if defined timer 8
76+
else if(timer->getHandle()->Instance == TIM8)
77+
return ADC_EXTERNALTRIG_T7_TRGO;
78+
#endif
79+
#ifdef TIM15 // if defined timer 15
80+
else if(timer->getHandle()->Instance == TIM15)
81+
return ADC_EXTERNALTRIG_T15_TRGO;
82+
#endif
83+
#ifdef TIM20 // if defined timer 15
84+
else if(timer->getHandle()->Instance == TIM20)
85+
return ADC_EXTERNALTRIG_T20_TRGO;
86+
#endif
87+
else
88+
return _TRGO_NOT_AVAILABLE;
89+
}
90+
91+
ADC_HandleTypeDef hadc;
92+
93+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
94+
{
95+
ADC_InjectionConfTypeDef sConfigInjected;
96+
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
97+
*/
98+
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
99+
100+
if(hadc.Instance == ADC1) {
101+
#ifdef __HAL_RCC_ADC1_CLK_ENABLE
102+
__HAL_RCC_ADC1_CLK_ENABLE();
103+
#endif
104+
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
105+
__HAL_RCC_ADC12_CLK_ENABLE();
106+
#endif
107+
}
108+
#ifdef ADC2
109+
else if (hadc.Instance == ADC2) {
110+
#ifdef __HAL_RCC_ADC2_CLK_ENABLE
111+
__HAL_RCC_ADC2_CLK_ENABLE();
112+
#endif
113+
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
114+
__HAL_RCC_ADC12_CLK_ENABLE();
115+
#endif
116+
}
117+
#endif
118+
#ifdef ADC3
119+
else if (hadc.Instance == ADC3) {
120+
#ifdef __HAL_RCC_ADC3_CLK_ENABLE
121+
__HAL_RCC_ADC3_CLK_ENABLE();
122+
#endif
123+
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
124+
__HAL_RCC_ADC34_CLK_ENABLE();
125+
#endif
126+
#if defined(ADC345_COMMON)
127+
__HAL_RCC_ADC345_CLK_ENABLE();
128+
#endif
129+
}
130+
#endif
131+
else{
132+
#ifdef SIMPLEFOC_STM32_DEBUG
133+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
134+
#endif
135+
return -1; // error not a valid ADC instance
136+
}
137+
138+
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
139+
hadc.Init.Resolution = ADC_RESOLUTION_12B;
140+
hadc.Init.ScanConvMode = ENABLE;
141+
hadc.Init.ContinuousConvMode = ENABLE;
142+
hadc.Init.DiscontinuousConvMode = DISABLE;
143+
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
144+
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
145+
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
146+
hadc.Init.NbrOfConversion = 1;
147+
hadc.Init.DMAContinuousRequests = DISABLE;
148+
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
149+
HAL_ADC_Init(&hadc);
150+
/**Configure for the selected ADC regular channel to be converted.
151+
*/
152+
153+
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
154+
*/
155+
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
156+
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5;
157+
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;
158+
sConfigInjected.AutoInjectedConv = DISABLE;
159+
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
160+
sConfigInjected.InjectedOffset = 0;
161+
162+
// automating TRGO flag finding - hardware specific
163+
uint8_t tim_num = 0;
164+
while(driver_params->timers[tim_num] != NP && tim_num < 6){
165+
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
166+
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
167+
168+
// if the code comes here, it has found the timer available
169+
// timer does have trgo flag for injected channels
170+
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
171+
172+
// this will be the timer with which the ADC will sync
173+
cs_params->timer_handle = driver_params->timers[tim_num-1];
174+
// done
175+
break;
176+
}
177+
if( cs_params->timer_handle == NP ){
178+
// not possible to use these timers for low-side current sense
179+
#ifdef SIMPLEFOC_STM32_DEBUG
180+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
181+
#endif
182+
return -1;
183+
}
184+
// display which timer is being used
185+
#ifdef SIMPLEFOC_STM32_DEBUG
186+
// it would be better to use the getTimerNumber from driver
187+
SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
188+
#endif
189+
190+
// first channel
191+
sConfigInjected.InjectedRank = 1;
192+
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC));
193+
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
194+
// second channel
195+
sConfigInjected.InjectedRank = 2;
196+
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC));
197+
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
198+
199+
// third channel - if exists
200+
if(_isset(cs_params->pins[2])){
201+
sConfigInjected.InjectedRank = 3;
202+
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC));
203+
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
204+
}
205+
206+
// enable interrupt
207+
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
208+
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
209+
210+
cs_params->adc_handle = &hadc;
211+
return 0;
212+
}
213+
214+
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
215+
{
216+
uint8_t cnt = 0;
217+
if(_isset(pinA)){
218+
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
219+
cs_params->pins[cnt++] = pinA;
220+
}
221+
if(_isset(pinB)){
222+
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
223+
cs_params->pins[cnt++] = pinB;
224+
}
225+
if(_isset(pinC)){
226+
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
227+
cs_params->pins[cnt] = pinC;
228+
}
229+
}
230+
231+
extern "C" {
232+
void ADC_IRQHandler(void)
233+
{
234+
HAL_ADC_IRQHandler(&hadc);
235+
}
236+
}
237+
238+
#endif

‎src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h

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

‎src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp

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

0 commit comments

Comments
 (0)