Skip to content

Commit d4a978b

Browse files
askuricaskuric
committed
Merge branch 'feat_h7_current_sensing' into feat_default_dir_only_no_sensor
2 parents 412c47a + b824026 commit d4a978b

File tree

5 files changed

+606
-0
lines changed

5 files changed

+606
-0
lines changed

‎src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.cpp‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
#include "stm32h7_hal.h"
2+
3+
#if defined(STM32H7xx)
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+
/**
13+
* Function initializing the ADC and the injected channels for the low-side current sensing
14+
*
15+
* @param cs_params - current sense parameters
16+
* @param driver_params - driver parameters
17+
*
18+
* @return int - 0 if success
19+
*/
20+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
21+
{
22+
ADC_InjectionConfTypeDef sConfigInjected;
23+
24+
// check if all pins belong to the same ADC
25+
ADC_TypeDef* adc_pin1 = _isset(cs_params->pins[0]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC) : nullptr;
26+
ADC_TypeDef* adc_pin2 = _isset(cs_params->pins[1]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC) : nullptr;
27+
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
28+
if ( ((adc_pin1 != adc_pin2) && (adc_pin1 && adc_pin2)) ||
29+
((adc_pin2 != adc_pin3) && (adc_pin2 && adc_pin3)) ||
30+
((adc_pin1 != adc_pin3) && (adc_pin1 && adc_pin3))
31+
){
32+
#ifdef SIMPLEFOC_STM32_DEBUG
33+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
34+
#endif
35+
return -1;
36+
}
37+
38+
39+
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
40+
*/
41+
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
42+
43+
if(hadc.Instance == ADC1) __HAL_RCC_ADC12_CLK_ENABLE();
44+
#ifdef ADC2 // if defined ADC2
45+
else if(hadc.Instance == ADC2) __HAL_RCC_ADC12_CLK_ENABLE();
46+
#endif
47+
#ifdef ADC3 // if defined ADC3
48+
else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
49+
#endif
50+
else{
51+
#ifdef SIMPLEFOC_STM32_DEBUG
52+
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
53+
#endif
54+
return -1; // error not a valid ADC instance
55+
}
56+
57+
#ifdef SIMPLEFOC_STM32_DEBUG
58+
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
59+
#endif
60+
61+
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
62+
hadc.Init.Resolution = ADC_RESOLUTION_12B;
63+
hadc.Init.ScanConvMode = ENABLE;
64+
hadc.Init.ContinuousConvMode = DISABLE;
65+
hadc.Init.DiscontinuousConvMode = DISABLE;
66+
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
67+
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
68+
#if defined(ADC_VER_V5_V90)
69+
// only for ADC3
70+
if(hadc.Instance == ADC3){
71+
hadc.Init.DataAlign = ADC3_DATAALIGN_RIGHT;
72+
}
73+
// more info here
74+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L170C13-L170C27
75+
hadc.Init.DMAContinuousRequests = DISABLE;
76+
// not sure about this one!!! maybe use: ADC_SAMPLING_MODE_NORMAL
77+
hadc.Init.SamplingMode = ADC_SAMPLING_MODE_TRIGGER_CONTROLED;
78+
#endif
79+
hadc.Init.NbrOfConversion = 1;
80+
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
81+
if ( HAL_ADC_Init(&hadc) != HAL_OK){
82+
#ifdef SIMPLEFOC_STM32_DEBUG
83+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
84+
#endif
85+
return -1;
86+
}
87+
88+
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
89+
*/
90+
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
91+
// if ADC1 or ADC2
92+
if(hadc.Instance == ADC1 || hadc.Instance == ADC2){
93+
// more info here: https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L658
94+
sConfigInjected.InjectedNbrOfConversion = ADC_SAMPLETIME_2CYCLE_5;
95+
}else {
96+
// adc3
97+
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32H7xx_HAL_Driver/Inc/stm32h7xx_hal_adc.h#L673
98+
sConfigInjected.InjectedNbrOfConversion = ADC3_SAMPLETIME_2CYCLES_5;
99+
}
100+
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISINGFALLING;
101+
sConfigInjected.AutoInjectedConv = DISABLE;
102+
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
103+
sConfigInjected.InjectedOffset = 0;
104+
105+
// automating TRGO flag finding - hardware specific
106+
uint8_t tim_num = 0;
107+
for (size_t i=0; i<6; i++) {
108+
TIM_HandleTypeDef *timer_to_check = driver_params->timers_handle[tim_num++];
109+
TIM_TypeDef *instance_to_check = timer_to_check->Instance;
110+
111+
uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check);
112+
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
113+
114+
// check if TRGO used already - if yes use the next timer
115+
if((timer_to_check->Instance->CR2 & LL_TIM_TRGO_ENABLE) || // if used for timer sync
116+
(timer_to_check->Instance->CR2 & LL_TIM_TRGO_UPDATE)) // if used for ADC sync
117+
{
118+
continue;
119+
}
120+
121+
// if the code comes here, it has found the timer available
122+
// timer does have trgo flag for injected channels
123+
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
124+
125+
// this will be the timer with which the ADC will sync
126+
cs_params->timer_handle = timer_to_check;
127+
// done
128+
break;
129+
}
130+
if( cs_params->timer_handle == NP ){
131+
// not possible to use these timers for low-side current sense
132+
#ifdef SIMPLEFOC_STM32_DEBUG
133+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
134+
#endif
135+
return -1;
136+
}else{
137+
#ifdef SIMPLEFOC_STM32_DEBUG
138+
SIMPLEFOC_DEBUG("STM32-CS: Using timer: ", stm32_getTimerNumber(cs_params->timer_handle->Instance));
139+
#endif
140+
}
141+
142+
for(int i=0; i<3; i++){
143+
// skip if not set
144+
if (!_isset(cs_params->pins[i])) continue;
145+
146+
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1 + i;
147+
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[i]));
148+
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
149+
#ifdef SIMPLEFOC_STM32_DEBUG
150+
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[i])) );
151+
#endif
152+
return -1;
153+
}
154+
}
155+
156+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
157+
// enable interrupt
158+
HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
159+
HAL_NVIC_EnableIRQ(ADC_IRQn);
160+
#endif
161+
162+
cs_params->adc_handle = &hadc;
163+
return 0;
164+
}
165+
166+
167+
/**
168+
* Function to initialize the ADC GPIO pins
169+
*
170+
* @param cs_params current sense parameters
171+
* @param pinA pin number for phase A
172+
* @param pinB pin number for phase B
173+
* @param pinC pin number for phase C
174+
* @return int 0 if success, -1 if error
175+
*/
176+
int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
177+
{
178+
int pins[3] = {pinA, pinB, pinC};
179+
const char* port_names[3] = {"A", "B", "C"};
180+
for(int i=0; i<3; i++){
181+
if(_isset(pins[i])){
182+
// check if pin is an analog pin
183+
if(pinmap_peripheral(analogInputToPinName(pins[i]), PinMap_ADC) == NP){
184+
#ifdef SIMPLEFOC_STM32_DEBUG
185+
SimpleFOCDebug::print("STM32-CS: ERR: Pin ");
186+
SimpleFOCDebug::print(port_names[i]);
187+
SimpleFOCDebug::println(" does not belong to any ADC!");
188+
#endif
189+
return -1;
190+
}
191+
pinmap_pinout(analogInputToPinName(pins[i]), PinMap_ADC);
192+
cs_params->pins[i] = pins[i];
193+
}
194+
}
195+
return 0;
196+
}
197+
198+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
199+
extern "C" {
200+
void ADC_IRQHandler(void)
201+
{
202+
HAL_ADC_IRQHandler(&hadc);
203+
}
204+
}
205+
#endif
206+
207+
#endif

‎src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_hal.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
#pragma once
2+
3+
#include "Arduino.h"
4+
5+
#if defined(STM32H7xx)
6+
#include "stm32h7xx_hal.h"
7+
#include "../stm32_mcu.h"
8+
#include "stm32h7_utils.h"
9+
10+
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
11+
int _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
12+
13+
#endif

‎src/current_sense/hardware_specific/stm32/stm32h7/stm32h7_mcu.cpp‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
#include "../../../hardware_api.h"
2+
3+
#if defined(STM32H7xx)
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 "stm32h7_hal.h"
10+
#include "stm32h7_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+
if(_adc_gpio_init(cs_params, pinA,pinB,pinC) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
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 SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
43+
44+
// stop all the timers for the driver
45+
stm32_pause(driver_params);
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->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->Instance->CR1 |= TIM_CR1_DIR;
56+
cs_params->timer_handle->Instance->CNT = cs_params->timer_handle->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->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+
stm32_resume(driver_params);
72+
73+
// return the cs parameters
74+
// successfully initialized
75+
// TODO verify if success in future
76+
return _cs_params;
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+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
85+
return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
86+
#else
87+
// an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3
88+
uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;
89+
return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
90+
#endif
91+
}
92+
}
93+
return 0;
94+
}
95+
96+
#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
97+
extern "C" {
98+
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
99+
100+
// calculate the instance
101+
int adc_index = _adcToIndex(AdcHandle);
102+
103+
// if the timer han't repetition counter - downsample two times
104+
if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
105+
tim_downsample[adc_index] = 0;
106+
return;
107+
}
108+
109+
adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
110+
adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
111+
adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
112+
}
113+
}
114+
#endif
115+
116+
#endif

0 commit comments

Comments
 (0)