Skip to content

Commit 1b2f5a7

Browse files
runger1101001runger1101001
authored
Merge pull request #165 from runger1101001/dev
STM32 driver - fixing software 6PWM
2 parents daaa0fb + 645b976 commit 1b2f5a7

File tree

1 file changed

+130
-50
lines changed

1 file changed

+130
-50
lines changed

‎src/drivers/hardware_specific/stm32_mcu.cpp

Lines changed: 130 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,19 @@ int getTimerNumber(int timerIndex);
1010
#endif
1111

1212

13+
14+
15+
#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED
16+
#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12
17+
#endif
18+
int numTimerPinsUsed;
19+
PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED];
20+
21+
22+
23+
24+
25+
1326
// setting pwm to hardware pin - instead analogWrite()
1427
void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution)
1528
{
@@ -33,17 +46,19 @@ HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) {
3346
if (timer==NP)
3447
return NP;
3548
uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral);
49+
bool init = false;
3650
if (HardwareTimer_Handle[index] == NULL) {
3751
HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral);
3852
HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
3953
HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
54+
init = true;
4055
}
4156
HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
4257
uint32_t channel = STM_PIN_CHANNEL(timer->function);
43-
HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin);
44-
HT->setOverflow(PWM_freq, HERTZ_FORMAT);
4558
HT->pause();
46-
HT->refresh();
59+
if (init)
60+
HT->setOverflow(PWM_freq, HERTZ_FORMAT);
61+
HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin);
4762
return HT;
4863
}
4964

@@ -54,8 +69,7 @@ HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) {
5469

5570

5671
// init high side pin
57-
HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer)
58-
{
72+
HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) {
5973
return _initPinPWM(PWM_freq, timer);
6074
}
6175

@@ -64,31 +78,39 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer)
6478
{
6579
uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral);
6680

81+
bool init = false;
6782
if (HardwareTimer_Handle[index] == NULL) {
6883
HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral);
6984
HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
7085
HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
71-
TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
72-
sConfigOC.OCMode = TIM_OCMODE_PWM2;
73-
sConfigOC.Pulse = 100;
74-
sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
75-
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
76-
#if defined(TIM_OCIDLESTATE_SET)
77-
sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET;
78-
#endif
79-
#if defined(TIM_OCNIDLESTATE_RESET)
80-
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
81-
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
82-
#endif
83-
uint32_t channel = STM_PIN_CHANNEL(timer->function);
84-
HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
86+
init = true;
8587
}
8688
HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
8789
uint32_t channel = STM_PIN_CHANNEL(timer->function);
88-
HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin);
89-
HT->setOverflow(PWM_freq, HERTZ_FORMAT);
90+
//SIMPLEFOC_DEBUG("Configuring low timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance)));
91+
//SIMPLEFOC_DEBUG("Configuring low channel ", (int)channel);
92+
93+
9094
HT->pause();
91-
HT->refresh();
95+
96+
if (init)
97+
HT->setOverflow(PWM_freq, HERTZ_FORMAT);
98+
// sets internal fields of HT, but we can't set polarity here
99+
HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin);
100+
// set polarity, unfortunately we have to set these other fields too
101+
// TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
102+
// sConfigOC.OCMode = TIM_OCMODE_PWM2;
103+
// sConfigOC.Pulse = __HAL_TIM_GET_COMPARE(&(HardwareTimer_Handle[index]->handle), HT->getChannel(channel));
104+
// sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
105+
// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
106+
// #if defined(TIM_OCIDLESTATE_SET) // TODO check this flag, looks like G4 uses something else...
107+
// sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET;
108+
// #endif
109+
// #if defined(TIM_OCNIDLESTATE_RESET) // TODO check this flag, looks like G4 uses something else...
110+
// sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
111+
// sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
112+
// #endif
113+
// HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
92114
return HT;
93115
}
94116

@@ -130,6 +152,40 @@ void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3,
130152

131153

132154

155+
void _alignTimersNew() {
156+
int numTimers = 0;
157+
HardwareTimer *timers[numTimerPinsUsed];
158+
159+
// reset timer counters
160+
for (int i=0; i<numTimerPinsUsed; i++) {
161+
uint32_t index = get_timer_index((TIM_TypeDef*)timerPinsUsed[i]->peripheral);
162+
HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
163+
bool found = false;
164+
for (int j=0; j<numTimers; j++) {
165+
if (timers[j] == timer) {
166+
found = true;
167+
break;
168+
}
169+
}
170+
if (!found)
171+
timers[numTimers++] = timer;
172+
}
173+
174+
// enable timer clock
175+
for (int i=0; i<numTimers; i++) {
176+
timers[i]->pause();
177+
timers[i]->refresh();
178+
}
179+
180+
for (int i=0; i<numTimers; i++) {
181+
//SIMPLEFOC_DEBUG("Resuming timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance)));
182+
timers[i]->resume();
183+
}
184+
185+
}
186+
187+
188+
133189

134190
int getLLChannel(PinMap* timer) {
135191
#if defined(TIM_CCER_CC1NE)
@@ -243,13 +299,6 @@ STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, Pi
243299

244300

245301

246-
#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED
247-
#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12
248-
#endif
249-
int numTimerPinsUsed;
250-
PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED];
251-
252-
253302
/*
254303
timer combination scoring function
255304
assigns a score, and also checks the combination is valid
@@ -265,7 +314,10 @@ int scoreCombination(int numPins, PinMap* pinTimers[]) {
265314
&& STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(timerPinsUsed[i]->function))
266315
return -2; // bad combination - timer channel already used
267316
}
268-
// check for inverted channels - TODO move this to outer loop also...
317+
318+
// TODO LPTIM and HRTIM should be ignored for now
319+
320+
// check for inverted channels
269321
if (numPins < 6) {
270322
for (int i=0; i<numPins; i++) {
271323
if (STM_PIN_INVERTED(pinTimers[i]->function))
@@ -307,12 +359,30 @@ int scoreCombination(int numPins, PinMap* pinTimers[]) {
307359
&& STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function)
308360
&& STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) {
309361
// hardware 6pwm, score <10
362+
363+
// TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8
364+
// F301, F302 --> 6 channels, but only 1-3 have dead-time insertion
365+
// TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion
366+
// TIM15/TIM16/TIM17 do dead-time insertion only on channel 1
367+
368+
// TODO check these defines
369+
#if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H) || defined(STM32G0x0_HAL_TIM_H)
370+
if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 )
371+
return -8; // channel 4 does not have dead-time insertion
372+
#endif
373+
#ifdef STM32G4xx_HAL_TIM_H
374+
if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 )
375+
return -8; // channels 5 & 6 do not have dead-time insertion
376+
#endif
310377
}
311378
else {
312-
313379
// check for inverted low-side channels
314380
if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function))
315381
return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm
382+
if (pinTimers[0]->peripheral != pinTimers[1]->peripheral
383+
|| pinTimers[2]->peripheral != pinTimers[3]->peripheral
384+
|| pinTimers[4]->peripheral != pinTimers[5]->peripheral)
385+
return -7; // bad combination - non-matching timers for H/L side in software 6-pwm
316386
score += 10; // software 6pwm, score >10
317387
}
318388
}
@@ -387,19 +457,23 @@ int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTime
387457
}
388458

389459

460+
461+
462+
390463
int findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]) {
391464
int bestScore = findBestTimerCombination(numPins, 0, pins, pinTimers);
392465
if (bestScore == NOT_FOUND) {
393466
#ifdef SIMPLEFOC_STM32_DEBUG
394-
SimpleFOCDebug::print("STM32: no workable combination found on these pins ");
395-
printTimerCombination(numPins, pinTimers, bestScore);
467+
SimpleFOCDebug::println("STM32: no workable combination found on these pins");
396468
#endif
397469
return -10; // no workable combination found
398470
}
399-
#ifdef SIMPLEFOC_STM32_DEBUG
400-
SimpleFOCDebug::print("STM32: best: ");
401-
printTimerCombination(numPins, pinTimers, bestScore);
402-
#endif
471+
else if (bestScore >= 0) {
472+
#ifdef SIMPLEFOC_STM32_DEBUG
473+
SimpleFOCDebug::print("STM32: best: ");
474+
printTimerCombination(numPins, pinTimers, bestScore);
475+
#endif
476+
}
403477
return bestScore;
404478
};
405479

@@ -474,7 +548,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in
474548
HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]);
475549
HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]);
476550
// allign the timers
477-
_alignPWMTimers(HT1, HT2, HT3);
551+
//_alignPWMTimers(HT1, HT2, HT3);
478552

479553
uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
480554
uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function);
@@ -488,6 +562,9 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in
488562
timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
489563
timerPinsUsed[numTimerPinsUsed++] = pinTimers[1];
490564
timerPinsUsed[numTimerPinsUsed++] = pinTimers[2];
565+
566+
_alignTimersNew();
567+
491568
return params;
492569
}
493570

@@ -574,7 +651,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, vo
574651
// Configuring PWM frequency, resolution and alignment
575652
// - BLDC driver - 6PWM setting
576653
// - hardware specific
577-
void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
654+
void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
578655
if (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
579656
SIMPLEFOC_DEBUG("STM32: ERR: too many pins used");
580657
return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
@@ -602,7 +679,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons
602679
HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]);
603680
HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]);
604681
HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]);
605-
_alignPWMTimers(HT1, HT2, HT3);
682+
//_alignPWMTimers(HT1, HT2, HT3);
606683
uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
607684
uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function);
608685
uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function);
@@ -617,8 +694,11 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons
617694
.interface_type = _SOFTWARE_6PWM
618695
};
619696
}
620-
for (int i=0; i<6; i++)
621-
timerPinsUsed[numTimerPinsUsed++] = pinTimers[i];
697+
if (score>=0) {
698+
for (int i=0; i<6; i++)
699+
timerPinsUsed[numTimerPinsUsed++] = pinTimers[i];
700+
_alignTimersNew();
701+
}
622702
return params; // success
623703
}
624704

@@ -629,21 +709,21 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons
629709
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
630710
// - BLDC driver - 6PWM setting
631711
// - hardware specific
632-
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){
712+
void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, void* params){
633713
switch(((STM32DriverParams*)params)->interface_type){
634714
case _HARDWARE_6PWM:
635715
_setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION);
636716
_setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION);
637717
_setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION);
638718
break;
639719
case _SOFTWARE_6PWM:
640-
float dead_zone = ((STM32DriverParams*)params)->dead_zone;
641-
_setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
642-
_setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
643-
_setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
644-
_setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
645-
_setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
646-
_setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
720+
float dead_zone = ((STM32DriverParams*)params)->dead_zone / 2.0f;
721+
_setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
722+
_setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
723+
_setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
724+
_setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
725+
_setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
726+
_setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
647727
break;
648728
}
649729
}

0 commit comments

Comments
 (0)