ControlLoop.c 17.7 KB
Newer Older
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
/* 
    __   ___   ____   ______  ____    ___   _      _       ___    ___   ____          __ 
   /  ] /   \ |    \ |      T|    \  /   \ | T    | T     /   \  /   \ |    \        /  ]
  /  / Y     Y|  _  Y|      ||  D  )Y     Y| |    | |    Y     YY     Y|  o  )      /  / 
 /  /  |  O  ||  |  |l_j  l_j|    / |  O  || l___ | l___ |  O  ||  O  ||   _/      /  /  
/   \_ |     ||  |  |  |  |  |    \ |     ||     T|     T|     ||     ||  |    __ /   \_ 
\     |l     !|  |  |  |  |  |  .  Yl     !|     ||     |l     !l     !|  |   |  T\     |
 \____j \___/ l__j__j  l__j  l__j\_j \___/ l_____jl_____j \___/  \___/ l__j   l__j \____j


FILENAME :        ControlLoop.c             DESIGN REF: MC18

 DESCRIPTION :
       Control loop (PWM computation for A2720 series BeagleBone powered converters) 

       Copyright Elettra Sincrotrone Trieste S.C.p.A. 2018  All rights reserved.
 
 AUTHOR :    Marco Cautero        START DATE :    25 Feb 2018

 CHANGES :

 REF NO  VERSION  DATE    WHO     DETAIL
23

24
 xx      20210422 22Apr21 MC      removed zeroing of setpoint (this way the 0 is loaded from shared memory)
root's avatar
root committed
25
 xx      20210421 21Apr21 MC      Visualization fixes 
26
 xx      20210420 20Apr21 MC      Normalization coefficients  
root's avatar
root committed
27
 xx	 20210415 15Apr21 MC 	  importing normalized values from ReadADC
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
28
29
30
31
32
33
34
35
 xx      20200828 28Aug20 MC	  Added space for Kd parameter
 xx      20200824 24Aug20 MC      Added derivative part on PI loop (now PID)
 xx      20191202 02Dec19 MC      Added delay for DC link fault delaying for false triggers
 xx      20190503 03May19 MC      Added memory flag for DC link fault retention until reset
 xx      20190109 09Jan19 MC      PS state resetted to OFF state if dclink warning happened to 
                                  avoid turning ON after dclink warning cleared
 xx      20190108 08Jan19 MC      Added DClink warning on main loop
 xx      20180724 24Jul18 MC      Final version with Debug added  
root's avatar
root committed
36
37
38
39
40
41
42
43
44
45
46
47
                                    
 				  Debug0 = regulator;
                                  Debug1 = integrator;
                                  Debug2 = derivator;
				  Debug3 = pwm1corr;  

         error      = setpoint - ADCreadout
	 integrator += (Ki * error) 
         derivator  = (error - prev-error) * Kd       	 
         regulator  = ((Kp * error) + Integrator) + Derivator)
         prev-error = error

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
48
49
50

*/

51
#include <stdint.h>
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
52
53
#include <limits.h>
#include <float.h>
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
#include <pru_cfg.h>
#include <pru_intc.h>
#include <pru_rpmsg.h>
#include "resource_table_0.h"


volatile register uint32_t __R31;

/* Host-0 Interrupt sets bit 30 in register R31 */
#define HOST_INT			((uint32_t) 1 << 30)

/* The PRU-ICSS system events used for RPMsg are defined in the Linux device tree
 * PRU0 uses system event 16 (To ARM) and 17 (From ARM)
 * PRU1 uses system event 18 (To ARM) and 19 (From ARM)
 */
#define TO_ARM_HOST			16
#define FROM_ARM_HOST			17

/*
 * Using the name 'rpmsg-pru' will probe the rpmsg_pru driver found
 * at linux-x.y.z/drivers/rpmsg/rpmsg_pru.c
 */
#define CHAN_NAME			"rpmsg-pru"
#define CHAN_DESC			"Channel 30"
#define CHAN_PORT			30

/*
 * Used to make sure the Linux drivers are ready for RPMsg communication
 * Found at linux-x.y.z/include/uapi/linux/virtio_config.h
 */
#define VIRTIO_CONFIG_S_DRIVER_OK	4

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
86
87
88
#define HWREG(x) (*((volatile unsigned int *)(x)))
#define HWREGH(x) (*((volatile unsigned short *)(x)))
#define HWREGB(x) (*((volatile unsigned char *)(x)))
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
89

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
90
#define benchmark
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112

#pragma LOCATION(Run,0x0000)
#pragma LOCATION(OutputCurrent,0x0004)
#pragma LOCATION(SetPoint,0x0008)
#pragma LOCATION(IOStatus,0x000c)

#pragma LOCATION(Kp,0x0010)
#pragma LOCATION(Ki,0x0014)
#pragma LOCATION(Kd,0x0018)
#pragma LOCATION(Imin,0x001c)
#pragma LOCATION(Imax,0x0020)
#pragma LOCATION(Kff,0x0024)
#pragma LOCATION(Reset,0x0028)
#pragma LOCATION(PRU0FirmRev,0x002c)
#pragma LOCATION(PulseDuration,0x0030)
#pragma LOCATION(RampStep,0x0034)

#pragma LOCATION(Debug0,0x0038)
#pragma LOCATION(Debug1,0x003c)
#pragma LOCATION(Debug2,0x0040)
#pragma LOCATION(Debug3,0x0044)

113
114
115
116
117
118
#pragma LOCATION(ScaleA0,0x0048)
#pragma LOCATION(ScaleA1,0x004C)
#pragma LOCATION(ScaleA2,0x0050)
#pragma LOCATION(ScaleA3,0x0054)
#pragma LOCATION(ScaleA4,0x0058) 
#pragma LOCATION(ScaleA5,0x005c) 
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
119

120
121
122
#pragma LOCATION(OvA0,0x0060);
#pragma LOCATION(OvA1,0x0064);

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
123
#pragma LOCATION(SSF,0x0068); /* Sub-sampling frequency */
124

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
125
126
volatile int Run; // while 0xFFFF run compensator
volatile float OutputCurrent;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
127
volatile unsigned int SSF;
128
struct pru2pru_data_transfer {
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
129
	float OCnorm;
130
} p2pdt;
131

132
133
struct pru2user_data_transfer {
	float SetPoint;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
134
	unsigned int IOStatus;
135
136
137
138
139
140
	float ScaleA0;
	float ScaleA1;
	float ScaleA2;
	float ScaleA3;
	float ScaleA4;
	float ScaleA5;
141
142
	float OvA0;
	float OvA1;
143
144
};
struct pru2user_data_transfer p2udt;
145

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
146
volatile float SetPoint;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
147
volatile unsigned int IOStatus;
148
volatile float Debug0,Debug1,Debug2,Debug3;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
149
150
151
volatile float Kp,Ki,Kd,RampStep,Imin,Imax,Kff;
volatile int Reset; // 0xffff = retest interlocks  
volatile unsigned int PRU0FirmRev;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
152
volatile unsigned int PulseDuration;
153
154
155
156
157
158
volatile float ScaleA0;
volatile float ScaleA1;
volatile float ScaleA2;
volatile float ScaleA3;  
volatile float ScaleA4; 
volatile float ScaleA5;
159
160
volatile float OvA0;
volatile float OvA1;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
161

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
162
unsigned int trigger, timer, delay, flagDC;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
163
164
165
166
167
168
169
170
171
172
float Integrator, Derivator, prev_error;
float tempSP;

void Compensator(float, float);
void SetPWM();
void ReadIO();

float mpy(float, float);
float sum(float, float);

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
173
174
175
176
177
178
179
180
181
182
float mpy(float a, float b)
{
	return a*b;
}

float sum(float a, float b)
{
	return a+b;
}

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
int changed() {
	if (SetPoint != p2udt.SetPoint || IOStatus != p2udt.IOStatus ||
				ScaleA0 != p2udt.ScaleA0 || ScaleA1 != p2udt.ScaleA1 || 
				ScaleA2 != p2udt.ScaleA2 || ScaleA3 != p2udt.ScaleA3 ||
				ScaleA4 != p2udt.ScaleA4 || ScaleA5 != p2udt.ScaleA5 ||
				OvA0 != p2udt.OvA0 || OvA1 != p2udt.OvA1) {
		p2udt.SetPoint = SetPoint;
		p2udt.IOStatus = IOStatus;
		p2udt.ScaleA0 = ScaleA0;
		p2udt.ScaleA1 = ScaleA1;
		p2udt.ScaleA2 = ScaleA2;
		p2udt.ScaleA3 = ScaleA3;
		p2udt.ScaleA4 = ScaleA4;
		p2udt.ScaleA5 = ScaleA5;
		p2udt.OvA0 = OvA0;
		p2udt.OvA1 = OvA1;
		return 1;
	}
	return 0;
}

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
204
int main(void)
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
205
{
206
	struct pru_rpmsg_transport transport;
207
	uint16_t src = 1024, dst = CHAN_PORT;
208
209
210
211
212
	volatile uint8_t *status;

	/* Allow OCP master port access by the PRU so the PRU can read external memories */
	CT_CFG.SYSCFG_bit.STANDBY_INIT = 0;

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
213
	//enable OCP master ports in SYSCFG (PRU_ICSS_CFG)
214
215
216
217
218
	//HWREG(0x26004) = 0x2A;
	
	/* Clear the status of the PRU-ICSS system event that the ARM will use to 'kick' us */
	CT_INTC.SICR_bit.STS_CLR_IDX = FROM_ARM_HOST;

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
219
220
221
222
223
224
	PRU0FirmRev = 20210422;
	trigger = 1;  //for pulse operation
	timer = 0;
	flagDC = 0;     // resets flag for dclink warning
	delay = 0;

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
225
	p2udt.SetPoint = FLT_MAX;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
226
	p2udt.IOStatus = UINT_MAX;
227
228
229
230
231
232
	p2udt.ScaleA0 = FLT_MAX;
	p2udt.ScaleA1 = FLT_MAX;
	p2udt.ScaleA2 = FLT_MAX;
	p2udt.ScaleA3 = FLT_MAX;
	p2udt.ScaleA4 = FLT_MAX;
	p2udt.ScaleA5 = FLT_MAX;
233
234
	p2udt.OvA0 = FLT_MAX;
	p2udt.OvA1 = FLT_MAX;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
235
236
	
	SSF = 0;
237

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
238
239
240
241
242
243
244
245
246
	//init IO readback
	IOStatus = 0x0;

	ReadIO();

	//init PWM
	SetPWM();
	__delay_cycles(200000);

247
248
249
250
251
252
253
254
255
256
	/* Make sure the Linux drivers are ready for RPMsg communication */
	status = &pru_remoteproc_ResourceTable.rpmsg_vdev.status;
	while (!(*status & VIRTIO_CONFIG_S_DRIVER_OK));

	/* Initialize the RPMsg transport structure */
	pru_rpmsg_init(&transport, &pru_remoteproc_ResourceTable.rpmsg_vring0, &pru_remoteproc_ResourceTable.rpmsg_vring1, TO_ARM_HOST, FROM_ARM_HOST);

	/* Create the RPMsg channel between the PRU and ARM user space using the transport structure. */
	while (pru_rpmsg_channel(RPMSG_NS_CREATE, &transport, CHAN_NAME, CHAN_DESC, CHAN_PORT) != PRU_RPMSG_SUCCESS);

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
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
	while(1)
	{      
		if((Run == 0xffff) && ((IOStatus & 0xfffffffe) == 0))  // checks if a ON command issued  and any error but only GATE OFF (LSB)
		{   // 1111 1111 1111 1111 1111 1111 1111 1110 mask blocking bit 24
			//clear values
			tempSP = 0;
			Integrator = 0;
			//SetPoint = 0;  so the value is loaded from shared memory at first and not zeroed from here
			prev_error=0;                                       // derivative error init
			//interlock reset
			HWREG(0x481AC194) = 0b100000000000; //gpio2_11 P8-42   GPIO2_SETDATAOUT
			__delay_cycles(10000000);
			HWREG(0x481AC190) = 0b100000000000;                 // GPIO2_CLEARDATAOUT
			__delay_cycles(10000000);
			//gate on
			HWREG(0x481AC190) = 0b10000000;      // gpio2_7 P8_46  GPIO2_SETDATAOUT
			//update IOStatus
			ReadIO();
			//demagnetize DCCT core
			//HWREG(0x481AC194) = 0b1000000000;
			//__delay_cycles(1000000); //50ms
			//HWREG(0x481AC190) = 0b1000000000;
			//__delay_cycles(200000000);



			while((Run == 0xffff) && ((IOStatus & 0xfdffffff) == 0))  
				//  0xfdffffff is masking dc-link warning contact on 25th bit, so if a warning happens the converter is not switched off  
			{
				//syncronize with T0
				while(HWREGH(0x48300208) > 200)  {} // >200 PWMSSePWM0_TBCNT (100MHz TBCLK period) 200*10ns=2us,T0 si resetta a 20us 7d0 (50KHz period)

#ifdef benchmark
				HWREG(0x481AC194) = 0b10;          // GPIO2_SETDATAOUT gpio2_1
#endif

293
294
				__xin(10, 5, 0, p2pdt);
				OutputCurrent = p2pdt.OCnorm;    // read from PRU adc normalized value
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
295
296
297
298

				Compensator(SetPoint, OutputCurrent);
				ReadIO();

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
299

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
300
				if (changed()) {
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
301
302
					pru_rpmsg_send(&transport, dst, src, &p2udt, sizeof(struct pru2user_data_transfer));
				}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
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
#ifdef benchmark
				HWREG(0x481AC190)= 0b10;           // GPIO2_CLEARDATAOUT gpio2_1 P8_18
#endif
			}

			// converter is switched off
			HWREG(0x481AC194) = 0b10000000;         // Gate OFF GPIO2_SETDATAOUT  GPIO2_7 P8-46
			//sync with T0
			while(HWREGH(0x48302208) < 15)          // PWMSSePWM1_TBCNT T1(10us period) time base count register 15*10us=150us 
			{}
			//set initial 50% duty cycle
			HWREGH(0x48302212) = 0xF5;              // PWMSSePWM1_CMPA
			HWREGH(0x48304212) = 0xFF;              // PWMSSePWM2_CMPA
			//clear Reset & Run commands
			Reset = 0;
			Run = 0;
			flagDC = 0;             
		}
		else    // converter is not ON or there is an active error
		{           
			if ((IOStatus & 0xfdfffffe) != 0) Run = 0; // prevents switch ON with DClink warning cleared and ON command issued before
			__delay_cycles(100000);     // 0xfdfffffe
			//handle reset command
			if(Reset == 0xffff)
			{
				//interlock reset
				IOStatus = (HWREG(0x4804c138) & 0b1011000011001100);    // resets status for 25th bit (dclink fault)
				IOStatus &= 0x0000ffff;              // sw clearing 26,25,24th bit of Status   
				HWREG(0x481AC194) = 0b100000000000;  // GPIO2_SETDATAOUT
				__delay_cycles(10000000);
				HWREG(0x481AC190) = 0b100000000000;  // GPIO2_CLEARDATAOUT
				__delay_cycles(10000000);
				Reset = 0;
				Run = 0;
				flagDC = 0;                            // resets flag for delaying DClink
			}
			ReadIO();    
		}
341

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
342
		if (SSF && changed()) {
343
			pru_rpmsg_send(&transport, dst, src, &p2udt, sizeof(struct pru2user_data_transfer));
344
		}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
345
346
	}
}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
347
348
349
350
351
352
353
354


#define Tdead 0xA
#define PWMminLimit 20
#define PWMmaxLimit 480
#define pwm1offset 0xF5   //pwm1 0xF5

void Compensator(float SP, float Iout)  // PWM frequency 100kHz, control loop 50kHz
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
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
{
	unsigned int pwm1corr, pwm2corr; //fs=(460 << 15); //480-20 <<15
	float regulator, error, SP1;


	// pulse operation   PulseDuration viene moltiplicato dall'ARM per 50
	if (PulseDuration!=0)
	{
		if(timer >= PulseDuration)
		{
			timer = 0;	//reset timer
			SP1 = 0;	//switch off
		}
		else if(timer != 0) timer += 1;  // wait for pulse duration to elapse 

		if(trigger==1 & (HWREG(0x481AE138) & (0b1<<21))==0)	   // testa il GPIO3 di trigger se attivo (basso)
		{
			trigger = 0;    //non ricevi piu altri trigger  
			timer = 1;		//start timer
			SP1 = SP;	//switch on
		}

		if(timer==0 & (HWREG(0x481AE138) & (0b1<<21))!=0) trigger = 1;  // trigger disattivo (alto) abilita nuovamente la modalita' pulsata
	}
	else SP1=SP;        // se non e'modalita'pulsata allora setta il setpoint


	//ramp
	if(tempSP == SP1) { }
	else
	{
		if(tempSP > SP1)
		{
			tempSP = sum(tempSP, -RampStep);
			if(tempSP < SP1)
			{tempSP = SP1;}
		}
		else
		{
			tempSP = sum(tempSP, RampStep);
			if(tempSP > SP1)
			{tempSP = SP1;}
		}
	}

	//limiter
	if(tempSP > Imax)
		tempSP = Imax;
	else                // aggiunto 17/5
		if(tempSP < Imin)
			tempSP = Imin;

	//global limiter
	if(tempSP > 1.25) 	//25 Amperes
		tempSP = 1.25;
	else                // aggiunto 17/5
		if(tempSP < -1.25)	// 25 Amperes
			tempSP = -1.25;


	// Kp, Ki coefficients have scaling factor embedded for translation from regulator to actuator
	error = sum(tempSP, -Iout);
	Integrator = sum(Integrator, mpy(Ki, error)); //24/08/20
	//Integrator = mpy(sum(Integrator, error), Ki);

	//saturation for integrator
	if(Integrator > (300 << 16))
		Integrator = (300 << 16);         // 0x012C0000 o 19660800 dec
	else                // aggiunto 17/5
		if(Integrator < -(300 << 16))
			Integrator = -(300 << 16);        // 0xFED40000

	Derivator = mpy(sum(error, -prev_error),Kd);  // 28/08/20

	//regulator = sum(mpy(Kp, error), Integrator);
	regulator = sum(sum(mpy(Kp, error), Integrator), Derivator);    

432
433
434
	Debug0 = regulator;
	Debug1 = Integrator;
	Debug2 = Derivator;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458

	// pwm1offset e'l'offset per andare al 50% duty cycle (16056320 dec)
	regulator=sum(regulator, (pwm1offset << 16));

	//convert to duty cycle
	if(regulator < (20 << 16))              // 0x00140000 o 1310720
	{
		pwm1corr = (PWMminLimit << 16);
		pwm2corr = ((PWMminLimit + Tdead) << 16);
	}
	else
		if(regulator > (480 << 16))             // 0x01e00000 o 31457280
		{
			pwm1corr = (PWMmaxLimit << 16);
			pwm2corr = ((PWMmaxLimit + Tdead) << 16);
		}
		else
		{
			pwm1corr = (unsigned int) regulator;
			pwm2corr = pwm1corr +(Tdead << 16);    //Tdead<<16=A0000
		}

	prev_error = error;                       // update error for derivative part    

459
	Debug3=pwm1corr;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
	//Debug2=pwm2corr;    



	//synchronize with timer
	while(HWREGH(0x48302208) < 15)          // PWMSSePWM0_TBCNT T1(10us period) time base count register 15*10us=150us 
	{}
	//write calculated duty cycle
	HWREG(0x48302210) = pwm1corr;         // PWMSSePWM1_CMPAHR
	HWREG(0x48304210) = pwm2corr;         // PWMSSePWM2_CMPAHR

	//HWREGH(0x48302212) = 0xF5;              // PWMSSePWM1_CMPA
	//HWREGH(0x48304212) = 0xFF;              // PWMSSePWM2_CMPA
	//HWREG(0x48302210) = 16056320;            // 50% duty cycle
	//HWREG(0x48304210) = 16056320+(Tdead << 16);
}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
476
477

void ReadIO()
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
{
	/*
status:
bit0  - Gate On (active low)

interlock inputs:
bit2  - Over Current (active high)
bit3  - Crow Bar (active high)

bit6  - DCCT fault (active high)
bit7  - Over Temperature (active high)
bit12 - Cross Conduction (active high)
bit13 - Vaux Undervoltage (active high)
bit15 - Magnet Fault (active high)
bit24 - DC link fault latched (active high)
bit25 - DC Link fault (active high) input signal is active low 
bit26 - Earth Fault (active high)
*/

	//IOStatus = (HWREG(0x4804c138) & 0b1011000011001100);    // GPIO1_DATAIN inital mask
	IOStatus = ((IOStatus & 0x01000000) | (HWREG(0x4804c138) & 0b1011000011001100)); //  do not clear flag for latched
	IOStatus |= (HWREG(0x44e07138) & (0b1<<26));            // GPIO0_DATAIN earth fault
	IOStatus |= ((HWREG(0x481ac13c) & (0b1<<7)) >> 7);      // GPIO2_DATAOUT gate


	// re-testing dclink fault after 1s   Delay viene moltiplicato dall'ARM per 50
	if (delay >= 100000)   //1 secs delay time
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
505
	{
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
		delay = 0;	//reset delay
		if (((~HWREG(0x481ac138)) & (0b1<<25)) != 0 )    // there is an event of DClink warning
			// the selected bit is 0 when error occours, so negating and masking, only if it is 1 (!=0) it is an error
		{       
			if (flagDC)  // to execute only if the flag is set.
			{
				IOStatus |= (((~HWREG(0x481ac138)) & (0b1<<25)) >> 1); // GPIO2_DATAIN dclink warning latched    
				flagDC = 0;
			}      
			else flagDC = 1;    // set flag as there has been an istance of dclink warning               
		}
		else flagDC = 0;        // as there is no event after 100000 cycles, the flag has to be reset.
	} 
	else delay += 1;  // wait for another tick 

	IOStatus |= ((~HWREG(0x481ac138)) & (0b1<<25));

	//Debug0= IOStatus;
}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
525
526

void SetPWM()
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
{
	//enable clock for pwm modules
	HWREGH(0x44e000cc) = 0x2;       // CM_PER_EPWMSS1_CLKCTRL
	HWREGH(0x44e000d4) = 0x2;       // CM_PER_EPWMSS0_CLKCTRL
	HWREGH(0x44e000d8) = 0x2;       // CM_PER_EPWMSS2_CLKCTRL

	HWREGH(0x44e10664) = 0x7;    // PWMSS_CTRL enable timebase clock for PWMSS0..2

	//configure pwm modules
	// T0 is Timer
	// T1,T2 are HRPWM

	HWREGH(0x4830020E) = 0x0;    // CMPCTL set T0 shadowing
	HWREGH(0x4830220E) = 0x0;    // CMPCTL set T1 shadowing
	HWREGH(0x4830420E) = 0x0;    // CMPCTL set T2 shadowing
	HWREGH(0x48300206) = 0x0;    // TBPHS clear T0 phase
	HWREGH(0x48302206) = 0x0;    // TBPHS clear T1 phase
	HWREGH(0x48304206) = 0x02;   // TBPHS set T2 phase (synchro with T1)
	HWREGH(0x4830020A) = 0x07CF; // TBPRD set T0 period; Tpwm=(TBPRD+1)*Ttbclk     // 7CF   20us
	HWREGH(0x4830220A) = 0x01f4; // TBPRD set T1 period // 2*TBPRD*Ttbclk
	HWREGH(0x4830420A) = 0x01f4; // TBPRD set T2 period // 
	HWREGH(0x48302212) = 0xF5;   // CMPA set initial T1 50% duty cycle
	HWREGH(0x48304212) = 0xFF;   // CMPA set initial T2 50% duty cycle
	HWREGH(0x48300216) = 0x0;    // AQCTLA set action qualifier for T0A
	HWREGH(0x48300218) = 0x0;    // AQCTLB set action qualifier for T0B
	HWREGH(0x48302216) = 0x90;   // AQCTLA set action qualifier for T1A
	HWREGH(0x48302218) = 0x0;    // AQCTLB set action qualifier for T1B
	HWREGH(0x48304216) = 0x60;   // AQCTLA set action qualifier for T2A
	HWREGH(0x48304218) = 0x0;    // AQCTLB set action qualifier for T2B
	HWREGH(0x48300200) = 0x10;   // TBCTL configure T0
	HWREGH(0x48302200) = 0x2016; // TBCTL configure T1, count UP/DN pag 1712 spruh73f.pdf
	HWREGH(0x48304200) = 0x2036; // TBCTL configure T2
	HWREGH(0x48302210) = 0x0;    // CMPAHR clear T1 high resolution
	HWREGH(0x48304210) = 0x0;    // CMPAHR clear T2 high resolution
	HWREGH(0x483002C0) = 0x0;    // HRCNFG configure T0 high resolution
	HWREGH(0x483022C0) = 0x02;   // HRCNFG configure T1 high resolution
	HWREGH(0x483042C0) = 0x01;   // HRCNFG configure T2 high resolution
}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
565