ControlLoop.c 17.9 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
Alessio Igor Bogani committed
123
124
volatile int Run; // while 0xFFFF run compensator
volatile float OutputCurrent;
125
struct pru2pru_data_transfer {
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
126
	float OCnorm;
127
} p2pdt;
128

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

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
143
volatile float SetPoint;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
144
volatile unsigned int IOStatus;
145
volatile float Debug0,Debug1,Debug2,Debug3;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
146
147
148
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
149
volatile unsigned int PulseDuration;
150
151
152
153
154
155
volatile float ScaleA0;
volatile float ScaleA1;
volatile float ScaleA2;
volatile float ScaleA3;  
volatile float ScaleA4; 
volatile float ScaleA5;
156
157
volatile float OvA0;
volatile float OvA1;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
158

Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
159
unsigned int trigger, timer, delay, flagDC;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
160
161
162
163
164
165
166
167
168
169
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
170
171
172
173
174
175
176
177
178
179
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
180
int main(void)
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
181
{
182
183
184
185
186
187
188
	struct pru_rpmsg_transport transport;
	uint16_t src, dst, len;
	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
189
	//enable OCP master ports in SYSCFG (PRU_ICSS_CFG)
190
191
192
193
194
	//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
195
196
197
198
199
200
	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
201
	p2udt.SetPoint = FLT_MAX;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
202
	p2udt.IOStatus = UINT_MAX;
203
204
205
206
207
208
	p2udt.ScaleA0 = FLT_MAX;
	p2udt.ScaleA1 = FLT_MAX;
	p2udt.ScaleA2 = FLT_MAX;
	p2udt.ScaleA3 = FLT_MAX;
	p2udt.ScaleA4 = FLT_MAX;
	p2udt.ScaleA5 = FLT_MAX;
209
210
	p2udt.OvA0 = FLT_MAX;
	p2udt.OvA1 = FLT_MAX;
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
211

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
212
213
214
215
216
217
218
219
220
	//init IO readback
	IOStatus = 0x0;

	ReadIO();

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

221
222
223
224
225
226
227
228
229
230
231
232
233
	/* 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);
	while (1) {
		/* Check bit 30 of register R31 to see if the ARM has kicked us */
		if (__R31 & HOST_INT) {
			/* Clear the event status */
234
235
236
237
			if (pru_rpmsg_receive(&transport, &src, &dst, NULL, &len) == PRU_RPMSG_SUCCESS) {
				CT_INTC.SICR_bit.STS_CLR_IDX = FROM_ARM_HOST;
				break;
			}
238
239
240
		}
	}

Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
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
	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

277
278
				__xin(10, 5, 0, p2pdt);
				OutputCurrent = p2pdt.OCnorm;    // read from PRU adc normalized value
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
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

				Compensator(SetPoint, OutputCurrent);
				ReadIO();

#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();    
		}
321

322
323
		if (SetPoint != p2udt.SetPoint || IOStatus != p2udt.IOStatus || ScaleA0 != p2udt.ScaleA0 ||
				ScaleA1 != p2udt.ScaleA1 || ScaleA2 != p2udt.ScaleA2 || ScaleA3 != p2udt.ScaleA3 ||
324
325
				ScaleA4 != p2udt.ScaleA4 || ScaleA5 != p2udt.ScaleA5 || OvA0 != p2udt.OvA0 ||
				OvA1 != p2udt.OvA1) {
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
326
327
			p2udt.SetPoint = SetPoint;
			p2udt.IOStatus = IOStatus;
328
329
330
331
332
333
			p2udt.ScaleA0 = ScaleA0;
			p2udt.ScaleA1 = ScaleA1;
			p2udt.ScaleA2 = ScaleA2;
			p2udt.ScaleA3 = ScaleA3;
			p2udt.ScaleA4 = ScaleA4;
			p2udt.ScaleA5 = ScaleA5;
334
335
			p2udt.OvA0 = OvA0;
			p2udt.OvA1 = OvA1;
336
337
			if (pru_rpmsg_send(&transport, dst, src, &p2udt,
						sizeof(struct pru2user_data_transfer)) != PRU_RPMSG_SUCCESS) {
338
				// Notification failed
339
			}
Alessio Igor Bogani's avatar
Changes    
Alessio Igor Bogani committed
340
341
342
			if (pru_rpmsg_receive(&transport, &src, &dst, NULL, &len) == PRU_RPMSG_SUCCESS) {
				__halt();
			}
343
		}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
344
345
	}
}
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
346
347
348
349
350
351
352
353


#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
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
{
	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);    

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

	// 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    

458
	Debug3=pwm1corr;
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
	//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
475
476

void ReadIO()
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
477
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
{
	/*
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
504
	{
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
		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
524
525

void SetPWM()
Alessio Igor Bogani's avatar
Alessio Igor Bogani committed
526
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
{
	//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
564