Skip to content

Commit 9e3725c

Browse files
askuricaskuric
authored
Merge pull request #394 from Copper280z/f7_current_sense
Add lowside current sensing for STM32F7 microcontrollers
2 parents af2c7c0 + 49da7b0 commit 9e3725c

File tree

5 files changed

+596
-0
lines changed

5 files changed

+596
-0
lines changed

‎src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp

Lines changed: 185 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,185 @@
1+
#include "stm32f7_hal.h"
2+
3+
#if defined(STM32F7xx)
4+
5+
//#define SIMPLEFOC_STM32_DEBUG
6+
7+
#include "../../../../communication/SimpleFOCDebug.h"
8+
#define _TRGO_NOT_AVAILABLE 12345
9+
10+
ADC_HandleTypeDef hadc;
11+
12+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
13+
{
14+
ADC_InjectionConfTypeDef sConfigInjected;
15+
16+
// check if all pins belong to the same ADC
17+
ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
18+
ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
19+
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
20+
if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
21+
#ifdef SIMPLEFOC_STM32_DEBUG
22+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
23+
#endif
24+
return -1;
25+
}
26+
27+
28+
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
29+
*/
30+
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
31+
32+
if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
33+
#ifdef ADC2 // if defined ADC2
34+
else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
35+
#endif
36+
#ifdef ADC3 // if defined ADC3
37+
else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
38+
#endif
39+
else{
40+
#ifdef SIMPLEFOC_STM32_DEBUG
41+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
42+
#endif
43+
return -1; // error not a valid ADC instance
44+
}
45+
46+
#ifdef SIMPLEFOC_STM32_DEBUG
47+
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
48+
#endif
49+
50+
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
51+
hadc.Init.Resolution = ADC_RESOLUTION_12B;
52+
hadc.Init.ScanConvMode = ENABLE;
53+
hadc.Init.ContinuousConvMode = DISABLE;
54+
hadc.Init.DiscontinuousConvMode = DISABLE;
55+
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
56+
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
57+
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
58+
hadc.Init.NbrOfConversion = 1;
59+
hadc.Init.DMAContinuousRequests = DISABLE;
60+
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
61+
if ( HAL_ADC_Init(&hadc) != HAL_OK){
62+
#ifdef SIMPLEFOC_STM32_DEBUG
63+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
64+
#endif
65+
return -1;
66+
}
67+
68+
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
69+
*/
70+
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
71+
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
72+
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING;
73+
sConfigInjected.AutoInjectedConv = DISABLE;
74+
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
75+
sConfigInjected.InjectedOffset = 0;
76+
77+
// automating TRGO flag finding - hardware specific
78+
uint8_t tim_num = 0;
79+
for (size_t i=0; i<6; i++) {
80+
HardwareTimer *timer_to_check = driver_params->timers[tim_num++];
81+
TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance;
82+
83+
// bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE;
84+
// if(TRGO_already_configured) continue;
85+
86+
uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check);
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 = timer_to_check;
95+
if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) {
96+
// workaround for errata 2.2.1 in ES0290 Rev 7
97+
// https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf
98+
__HAL_RCC_DAC_CLK_ENABLE();
99+
}
100+
// done
101+
break;
102+
}
103+
if( cs_params->timer_handle == NP ){
104+
// not possible to use these timers for low-side current sense
105+
#ifdef SIMPLEFOC_STM32_DEBUG
106+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
107+
#endif
108+
return -1;
109+
}
110+
// display which timer is being used
111+
#ifdef SIMPLEFOC_STM32_DEBUG
112+
// it would be better to use the getTimerNumber from driver
113+
SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
114+
#endif
115+
116+
117+
// first channel
118+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
119+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
120+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
121+
#ifdef SIMPLEFOC_STM32_DEBUG
122+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
123+
#endif
124+
return -1;
125+
}
126+
127+
// second channel
128+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
129+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
130+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
131+
#ifdef SIMPLEFOC_STM32_DEBUG
132+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
133+
#endif
134+
return -1;
135+
}
136+
137+
// third channel - if exists
138+
if(_isset(cs_params->pins[2])){
139+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
140+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
141+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
142+
#ifdef SIMPLEFOC_STM32_DEBUG
143+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
144+
#endif
145+
return -1;
146+
}
147+
}
148+
149+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
150+
// enable interrupt
151+
HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
152+
HAL_NVIC_EnableIRQ(ADC_IRQn);
153+
#endif
154+
155+
cs_params->adc_handle = &hadc;
156+
return 0;
157+
}
158+
159+
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
160+
{
161+
uint8_t cnt = 0;
162+
if(_isset(pinA)){
163+
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
164+
cs_params->pins[cnt++] = pinA;
165+
}
166+
if(_isset(pinB)){
167+
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
168+
cs_params->pins[cnt++] = pinB;
169+
}
170+
if(_isset(pinC)){
171+
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
172+
cs_params->pins[cnt] = pinC;
173+
}
174+
}
175+
176+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
177+
extern "C" {
178+
void ADC_IRQHandler(void)
179+
{
180+
HAL_ADC_IRQHandler(&hadc);
181+
}
182+
}
183+
#endif
184+
185+
#endif

‎src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
#pragma once
2+
3+
#include "Arduino.h"
4+
5+
#if defined(STM32F7xx)
6+
#include "stm32f7xx_hal.h"
7+
#include "../../../../common/foc_utils.h"
8+
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
9+
#include "../stm32_mcu.h"
10+
#include "stm32f7_utils.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

‎src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp

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

0 commit comments

Comments
 (0)