forked from Candas1/Arduino-FOC
-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathdue_mcu.cpp
More file actions
449 lines (384 loc) · 15.6 KB
/
Copy pathdue_mcu.cpp
File metadata and controls
449 lines (384 loc) · 15.6 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
#include "../hardware_api.h"
#if defined(__arm__) && defined(__SAM3X8E__)
#pragma message("")
#pragma message("SimpleFOC: compiling for Arduino/Due")
#pragma message("")
#define _PWM_FREQUENCY 25000 // 25khz
#define _PWM_FREQUENCY_MAX 50000 // 50khz
#define _PWM_RES_MIN 255 // 50khz
// pwm frequency and max duty cycle
static unsigned long _pwm_frequency;
static int _max_pwm_value = 1023;
// array mapping the timer values to the interrupt handlers
static IRQn_Type irq_type[] = {TC0_IRQn, TC0_IRQn, TC1_IRQn, TC1_IRQn, TC2_IRQn, TC2_IRQn, TC3_IRQn, TC3_IRQn, TC4_IRQn, TC4_IRQn, TC5_IRQn, TC5_IRQn, TC6_IRQn, TC6_IRQn, TC7_IRQn, TC7_IRQn, TC8_IRQn, TC8_IRQn};
// current counter values
static volatile uint32_t pwm_counter_vals[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
// variables copied from wiring_analog.cpp for arduino due
static uint8_t PWMEnabled = 0;
static uint8_t TCChanEnabled[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
static const uint32_t channelToChNo[] = { 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2 };
static const uint32_t channelToAB[] = { 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0 };
static Tc *channelToTC[] = {
TC0, TC0, TC0, TC0, TC0, TC0,
TC1, TC1, TC1, TC1, TC1, TC1,
TC2, TC2, TC2, TC2, TC2, TC2 };
static const uint32_t channelToId[] = { 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8 };
// function setting the CMR register
static void TC_SetCMR_ChannelA(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xFFF0FFFF) | v;}
static void TC_SetCMR_ChannelB(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xF0FFFFFF) | v; }
// function which starts and syncs the timers
// if the pin is the true PWM pin this function does not do anything
void syncTimers(uint32_t ulPin1,uint32_t ulPin2, uint32_t ulPin3 = -1, uint32_t ulPin4 = -1){
uint32_t chNo1,chNo2,chNo3,chNo4;
Tc *chTC1 = nullptr,*chTC2 = nullptr,*chTC3 = nullptr,*chTC4 = nullptr;
// configure timer channel for the first pin if it is a timer pin
uint32_t attr = g_APinDescription[ulPin1].ulPinAttribute;
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
ETCChannel channel1 = g_APinDescription[ulPin1].ulTCChannel;
chNo1 = channelToChNo[channel1];
chTC1 = channelToTC[channel1];
TCChanEnabled[channelToId[channel1]] = 1;
}
// configure timer channel for the first pin if it is a timer pin
attr = g_APinDescription[ulPin2].ulPinAttribute;
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
ETCChannel channel2 = g_APinDescription[ulPin2].ulTCChannel;
chNo2 = channelToChNo[channel2];
chTC2 = channelToTC[channel2];
TCChanEnabled[channelToId[channel2]] = 1;
}
if(ulPin3 > 0 ){
// configure timer channel for the first pin if it is a timer pin
attr = g_APinDescription[ulPin3].ulPinAttribute;
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
ETCChannel channel3 = g_APinDescription[ulPin3].ulTCChannel;
chNo3 = channelToChNo[channel3];
chTC3 = channelToTC[channel3];
TCChanEnabled[channelToId[channel3]] = 1;
}
}
if(ulPin4 > 0 ){
// configure timer channel for the first pin if it is a timer pin
attr = g_APinDescription[ulPin4].ulPinAttribute;
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
ETCChannel channel4 = g_APinDescription[ulPin4].ulTCChannel;
chNo4 = channelToChNo[channel4];
chTC4 = channelToTC[channel4];
TCChanEnabled[channelToId[channel4]] = 1;
}
}
// start timers and make them synced
if(chTC1){
TC_Start(chTC1, chNo1);
chTC1->TC_BCR = TC_BCR_SYNC;
}
if(chTC2){
TC_Start(chTC2, chNo2);
chTC2->TC_BCR = TC_BCR_SYNC;
}
if(chTC3 && ulPin3){
TC_Start(chTC3, chNo3);
chTC3->TC_BCR = TC_BCR_SYNC;
}
if(chTC4 && ulPin4){
TC_Start(chTC4, chNo4);
chTC4->TC_BCR = TC_BCR_SYNC;
}
}
// function configuring the pwm frequency for given pin
// possible to supply the pwm pin and the timer pin
void initPWM(uint32_t ulPin, uint32_t pwm_freq){
// check which pin type
uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm pin
if (!PWMEnabled) {
// PWM Startup code
pmc_enable_periph_clk(PWM_INTERFACE_ID);
// this function does not work too well - I'll rewrite it
// PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK);
// finding the divisors an prescalers form FindClockConfiguration function
uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024};
uint8_t divisor = 0;
uint32_t prescaler;
/* Find prescaler and divisor values */
prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
while ((prescaler > 255) && (divisor < 11)) {
divisor++;
prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
}
// update the divisor*prescaler value
prescaler = prescaler | (divisor << 8);
// now calculate the real resolution timer period necessary (pwm resolution)
// pwm_res = bus_freq / (pwm_freq * (prescaler))
_max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler);
// set the prescaler value
PWM->PWM_CLK = prescaler;
PWMEnabled = 1;
}
uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
// Setup PWM for this pin
PIO_Configure(g_APinDescription[ulPin].pPort,
g_APinDescription[ulPin].ulPinType,
g_APinDescription[ulPin].ulPin,
g_APinDescription[ulPin].ulPinConfiguration);
// PWM_CMR_CALG - center align
// PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0);
PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0);
PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value);
PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0);
PWMC_EnableChannel(PWM_INTERFACE, chan);
g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
}
return;
}
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
// We use MCLK/2 as clock.
const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ;
// Setup Timer for this pin
ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
uint32_t chNo = channelToChNo[channel];
uint32_t chA = channelToAB[channel];
Tc *chTC = channelToTC[channel];
uint32_t interfaceID = channelToId[channel];
if (!TCChanEnabled[interfaceID]) {
pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID);
TC_Configure(chTC, chNo,
TC_CMR_TCCLKS_TIMER_CLOCK1 |
TC_CMR_WAVE | // Waveform mode
TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC
TC_CMR_EEVT_XC0 | // Set external events from XC0 (this setup TIOB as output)
TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR |
TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
TC_SetRC(chTC, chNo, TC);
}
// disable the counter on start
if (chA){
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
}else{
TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
}
// configure input-ouput structure
if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
PIO_Configure(g_APinDescription[ulPin].pPort,
g_APinDescription[ulPin].ulPinType,
g_APinDescription[ulPin].ulPin,
g_APinDescription[ulPin].ulPinConfiguration);
g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
}
// enable interrupts
chTC->TC_CHANNEL[chNo].TC_IER = TC_IER_CPAS // interrupt on RA compare match
| TC_IER_CPBS // interrupt on RB compare match
| TC_IER_CPCS; // interrupt on RC compare match
chTC->TC_CHANNEL[chNo].TC_IDR = ~TC_IER_CPAS // interrupt on RA compare match
& ~TC_IER_CPBS // interrupt on RB compare match
& ~ TC_IER_CPCS; // interrupt on RC compare match
// enable interrupts for this timer
NVIC_EnableIRQ(irq_type[channel]);
return;
}
}
// pwm setting function
// it sets the duty cycle for pwm pin or timer pin
void setPwm(uint32_t ulPin, uint32_t ulValue) {
// check pin type
uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm
uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
PWMC_SetDutyCycle(PWM_INTERFACE, chan, ulValue);
return;
}
if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
// get the timer variables
ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
Tc *chTC = channelToTC[channel];
uint32_t chNo = channelToChNo[channel];
if(!ulValue) {
// if the value 0 disable counter
if (channelToAB[channel])
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR);
else
TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
}else{
// if the value not zero
// calculate clock
const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency;
// Map value to Timer ranges 0..max_duty_cycle => 0..TC
// Setup Timer for this pin
ulValue = ulValue * TC ;
pwm_counter_vals[channel] = ulValue / _max_pwm_value;
// enable counter
if (channelToAB[channel])
TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
else
TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
}
return;
}
}
// interrupt handlers for seamless pwm duty-cycle setting
void TC0_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC0, 0);
// update the counters
if(pwm_counter_vals[0]) TC_SetRA(TC0, 0, pwm_counter_vals[0]);
if(pwm_counter_vals[1]) TC_SetRB(TC0, 0, pwm_counter_vals[1]);
}
void TC1_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC0, 1);
// update the counters
if(pwm_counter_vals[2]) TC_SetRA(TC0, 1, pwm_counter_vals[2]);
if(pwm_counter_vals[3]) TC_SetRB(TC0, 1, pwm_counter_vals[3]);
}
void TC2_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC0, 2);
// update the counters
if(pwm_counter_vals[4]) TC_SetRA(TC0, 2, pwm_counter_vals[4]);
if(pwm_counter_vals[5]) TC_SetRB(TC0, 2, pwm_counter_vals[5]);
}
void TC3_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC1, 0);
// update the counters
if(pwm_counter_vals[6]) TC_SetRA(TC1, 0, pwm_counter_vals[6]);
if(pwm_counter_vals[7]) TC_SetRB(TC1, 0, pwm_counter_vals[7]);
}
void TC4_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC1, 1);
// update the counters
if(pwm_counter_vals[8]) TC_SetRA(TC1, 1, pwm_counter_vals[8]);
if(pwm_counter_vals[9]) TC_SetRB(TC1, 1, pwm_counter_vals[9]);
}
void TC5_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC1, 2);
// update the counters
if(pwm_counter_vals[10]) TC_SetRA(TC1, 2, pwm_counter_vals[10]);
if(pwm_counter_vals[11]) TC_SetRB(TC1, 2, pwm_counter_vals[11]);
}
void TC6_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC2, 0);
// update the counters
if(pwm_counter_vals[12]) TC_SetRA(TC2, 0, pwm_counter_vals[12]);
if(pwm_counter_vals[13]) TC_SetRB(TC2, 0, pwm_counter_vals[13]);
}
void TC7_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC2, 1);
// update the counters
if(pwm_counter_vals[14]) TC_SetRA(TC2, 1, pwm_counter_vals[14]);
if(pwm_counter_vals[15]) TC_SetRB(TC2, 1, pwm_counter_vals[15]);
}
void TC8_Handler()
{
// read/clear interrupt status
TC_GetStatus(TC2, 2);
// update the counters
if(pwm_counter_vals[16]) TC_SetRA(TC2, 2, pwm_counter_vals[16]);
if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]);
}
// implementation of the hardware_api.cpp
// ---------------------------------------------------------------------------------------------------------------------------------
// function setting the high pwm frequency to the supplied pins
// - BLDC motor - 3PWM setting
// - hardware specific
void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
// save the pwm frequency
_pwm_frequency = pwm_frequency;
// cinfigure pwm pins
initPWM(pinA, _pwm_frequency);
initPWM(pinB, _pwm_frequency);
initPWM(pinC, _pwm_frequency);
// sync the timers if possible
syncTimers(pinA, pinB, pinC);
GenericDriverParams* params = new GenericDriverParams {
.pins = { pinA, pinB, pinC },
.pwm_frequency = pwm_frequency
};
return params;
}
// Configuring PWM frequency, resolution and alignment
//- Stepper driver - 2PWM setting
// - hardware specific
void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
// save the pwm frequency
_pwm_frequency = pwm_frequency;
// cinfigure pwm pins
initPWM(pinA, _pwm_frequency);
initPWM(pinB, _pwm_frequency);
// sync the timers if possible
syncTimers(pinA, pinB);
GenericDriverParams* params = new GenericDriverParams {
.pins = { pinA, pinB },
.pwm_frequency = pwm_frequency
};
return params;
}
// function setting the high pwm frequency to the supplied pins
// - Stepper motor - 4PWM setting
// - hardware speciffic
void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
// save the pwm frequency
_pwm_frequency = pwm_frequency;
// cinfigure pwm pins
initPWM(pinA, _pwm_frequency);
initPWM(pinB, _pwm_frequency);
initPWM(pinC, _pwm_frequency);
initPWM(pinD, _pwm_frequency);
// sync the timers if possible
syncTimers(pinA, pinB, pinC, pinD);
GenericDriverParams* params = new GenericDriverParams {
.pins = { pinA, pinB, pinC, pinD },
.pwm_frequency = pwm_frequency
};
return params;
}
// function setting the pwm duty cycle to the hardware
// - BLDC motor - 3PWM setting
// - hardware speciffic
void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* param){
// transform duty cycle from [0,1] to [0,_max_pwm_value]
GenericDriverParams* p = (GenericDriverParams*)param;
setPwm(p->pins[0], _max_pwm_value*dc_a);
setPwm(p->pins[1], _max_pwm_value*dc_b);
setPwm(p->pins[2], _max_pwm_value*dc_c);
}
// function setting the pwm duty cycle to the hardware
// - Stepper motor - 4PWM setting
// - hardware speciffic
void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* param){
// transform duty cycle from [0,1] to [0,_max_pwm_value]
GenericDriverParams* p = (GenericDriverParams*)param;
setPwm(p->pins[0], _max_pwm_value*dc_1a);
setPwm(p->pins[1], _max_pwm_value*dc_1b);
setPwm(p->pins[2], _max_pwm_value*dc_2a);
setPwm(p->pins[3], _max_pwm_value*dc_2b);
}
// Function setting the duty cycle to the pwm pin (ex. analogWrite())
// - Stepper driver - 2PWM setting
// - hardware specific
void _writeDutyCycle2PWM(float dc_a, float dc_b, void* param){
// transform duty cycle from [0,1] to [0,_max_pwm_value]
GenericDriverParams* p = (GenericDriverParams*)param;
setPwm(p->pins[0], _max_pwm_value*dc_a);
setPwm(p->pins[1], _max_pwm_value*dc_b);
}
#endif