/****************************************************************************/
/*                     Project name: THRU3.prj                              */
/*                                                                          */
/*                        File name: main.c                                 */
/*                             date: 2005/12/1                              */
/*                                                                          */
/*(c) Copyright 2005      HIRATUKA Engineering Co.,Ltd. All rights reserved.*/
/****************************************************************************/
/*
  ================  SampleProgram -- AI2 to AO2 through  ====================

Board setting
 ========= DSK6713IF/AI2 ==========
 DIPSW
  S11(MY_ADDRESS):    1:OFF  2:OFF  3:OFF
  S12(SAMPLING_FREQ): 1:OFF  2:OFF  3:OFF  4:OFF  5:OFF  6:OFF  (NoUse)
  S13(CONTROL):       1:OFF  2:OFF  3:OFF  4:OFF  5:OFF  6:OFF
  S14(USER_SW):       1:OFF  2:OFF  3:OFF  4:OFF  5:OFF  6:OFF  7:OFF  8:OFF (NoUse)

 JumperPin
  JP13(AD)           : CLOSE
  JP14(DA)           : OPEN
  JP12(SAMPLING_CLK) : MS
  JP11(SAMPLING)     : IN    (NoUse)
  
  JP15(INT4)         : CLOSE
  JP16(INT5)         : OPEN
  JP17(INT6)         : OPEN
  JP18(INT7)         : OPEN  (NoUse)


 ========= DSK6713IF/AO2 ==========
 DIPSW
  S1(MY_ADDRESS):    1:OFF  2:OFF  3:ON
  S2(SAMPLING_FREQ): 1:OFF  2:OFF  3:OFF  4:OFF  5:OFF  6:OFF (NoUse)
  S3(CONTROL):       1:OFF  2:OFF  3:OFF  4:OFF  5:OFF  6:OFF
  S4(USER_SW):       1:OFF  2:OFF  3:OFF  4:OFF  5:OFF  6:OFF  7:OFF  8:OFF (NoUse)

 JumperPin
  JP43(AD)           : CLOSE
  JP44(DA)           : OPEN
  JP42(SAMPLING_OUT) : SLV
  JP41(SAMPLING_IN)  : IN
  
  JP45(INT4)         : OPEN
  JP46(INT5)         : CLOSE (NoUse)
  JP47(INT6)         : OPEN
  JP48(INT7)         : OPEN  (NoUse)


Refer to Readme.txt for more information.
*/

/*	chip support library */
#define CHIP_6713 1

#include <csl.h>

#define BASE_AI20 0xA0000000	/* set DSK6713IF/AI2 BaseAddress ADDR=0 */
#define BASE_AI21 0xA0000100	/* set DSK6713IF/AI2 BaseAddress ADDR=1 */
#define BASE_AO22 0xA0000200	/* set DSK6713IF/AI2 BaseAddress ADDR=2 */
#define BASE_AO23 0xA0000300	/* set DSK6713IF/AO2 BaseAddress ADDR=3 */

#define	_CHIP_PLLCSR_ADDR	0x01B7C100
#define	_CHIP_PLLM_ADDR		0x01B7C110
#define	_CHIP_PLLDIV0_ADDR	0x01B7C114
#define	_CHIP_PLLDIV1_ADDR	0x01B7C118
#define	_CHIP_PLLDIV2_ADDR	0x01B7C11C
#define	_CHIP_PLLDIV3_ADDR	0x01B7C120

#include <stdio.h>
#include <ctype.h>
#include "c6713dsk.h"
#include "c6xai2.h"
#include "c6xao2.h"

/*	header file for the UART daughtercard*/
#include "./uart_test.h"

/*	header files for LED and DIP SW on DSK*/
#include "dsk6713.h"
#include "dsk6713_led.h"
#include "dsk6713_dip.h"

/*	Set up transmit and receive buffers and pointers for UART channel1 */
unsigned int uiUART_recv_count1=0, uiUART_xmit_count1=0;
unsigned int uiUART_recv_nextin1=0, uiUART_recv_nextout1=0;
unsigned int uiUART_xmit_nextin1=0, uiUART_xmit_nextout1=0;
unsigned char ucUART_recv_buffer1[RECV_BUF_SIZE];
unsigned char ucUART_xmit_buffer1[XMIT_BUF_SIZE];

#define RESET_REG	*(unsigned char*)0x90080001
#define DIP_LED_REG	*(unsigned char*)0x90080000
#define CE2_CONTROL *(int *)0x01800010

unsigned char cmdbuf[130];
unsigned int cmdcnt=0;

/* Matrix */
const double am[6][6] = 
{
 {-0.480451,   0.167233,   0.020017,     0.0506144,   0.0268921,   -0.000122855 },
 {-0.131518,  -0.216356,   0.00395423,  -0.00412539, -0.000732781, -0.0000237701},
 { 0.0510136, -0.0561895,  0.0114219,   -0.0053126,  -0.0134197,    0.0000219825}, 
 { 0.229394,  -0.0951799, -0.00695388,  -0.414313,    0.227936,    -0.0143399   },
 {-0.0432804, -0.0319576,  0.0000932942, 0.339973,    0.168408,    -0.0145028   },
 {-0.165397,   0.134008,   0.00561217,  -0.0199318,  -0.476113,     0.00987761  }
};      

/*Function genelator (62.5Hz) Sin*/
double ys[3] = {0., 3.926980723806e-3, 0};
double as1 = 1.999984578762941;
double as2 = -1.;
/*Function genelator (62.5Hz) Cos*/
double yc[3] = {0., 0.999992289381471, 1.};
double ac1 = 1.999984578762941;
double ac2 = -1.;

long   CTime = 0;

//double u0 =0.;
//double u1 =0.;
/*double Ixc = 0.,
       Iyc = 0.,
       Qxc = 0.,       
       Qyc = 0.,
       Exc = 0.,
       Eyc = 0.;*/

double databuf[128];
//int    i = 0;

/*Function genelator (7kHz)
double y[3] = {0., 0.2181432413965, 0};
double a1 = 1.951833524;
double a2 = -1.0;
(70kHz)
double y[3] = {0., 2.199113085e-3, 0};
double a1 = 1.999995164;
double a2 = -1.;
*/
/***********************/

void dsp_init(void);
void dskif_init(void);
void enableGlobalINT(void);						/* enable GIE                 */
void enableNMI(void);							/* enable NMIE                */
void enableSpecificINT(int INTnumber);			/* enable specific interrupt  */

void cmd_exec(unsigned char *cmdbuf);

/* define externally controllable parameters */
double gain1=0,gain2=0,gain3=0,gain4=0;
double offset1=0,offset2=0,offset3=0,offset4=0;
double polarity1=0,polarity2=0,polarity3=0,polarity4=0;

void main(void)
{
    DSK6713_init();
    DSK6713_LED_init();
    DSK6713_DIP_init();

	dsp_init();											/* Initialize C6713DSP Register */
	
//	*(volatile unsigned int *)EMIF_CE2 = 0x00E0C421;	/* CE2 Bus timming control	*/
	*(volatile unsigned int *)EMIF_CE2 = 0x03e0cf21;	/* CE2 Bus timming control	*/

	dskif_init();

	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG1) = 0x0000;  /* AD Sampling Clock Source select */
//	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG2) = 0x003F;  /* Sampling clock generator set : 500KHz */
//	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG2) = 0x0029;  /* Sampling clock generator set : 200KHz */
	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG2) = 0x0020;  /* Sampling clock generator set : 100KHz */
//	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG2) = 0x001B;  /* Sampling clock generator set : 50KHz */

//	*(volatile unsigned int *)(BASE_AO2 + AO2_SETREG1) = 0x0001;  /* DA Sampling Clock Source select : EXT_IN*/
//	*(volatile unsigned int *)(BASE_AO2 + AO2_SETREG2) = 0x0000;  /* No Use */

	CSL_init();					// Initialize the "chip support library"
	
	RESET_REG = 0x08;			// reset daughtercard
	delay(8000*250);			// delay ~ 250 milliseconds
	RESET_REG = 0x00;			//remove reset signal
	delay(8000*250);			// delay ~ 250 milliseconds

	initialize_UART(DATA_RATE_57600);		// initialize UART channel

	enableSpecificINT(4);								/* enable INT4             */
	enableNMI();										/* enable NMI(Non Maskable Interrupt)*/
	enableGlobalINT();									/* set GIE(Global Interrupt Enable)  */

	IRQ_enable(IRQ_EVT_EXTINT5);	// enable external interrupt 5 for UART

}

void main_loop(void)
{
//	static unsigned char prev;
//	unsigned char temp;
	unsigned int cnt,led;
	unsigned char a;
	unsigned int cmdfin;

//	temp = (DSK6713_rget(DSK6713_USER_REG) >> 4) & 0x0f;
    cnt=UART_recv_count();
	if(cnt>0)
	{
	  	UART_recv_byte(&a);
//	  	UART_send_byte(a);
		*(volatile int *)(BASE_AI20 + AI2_USERLED)=((int)a)&0x00FF;

		cmdbuf[cmdcnt]=toupper(a);
		++cmdcnt;

		if(a==0x0A)
			cmdfin=1;
		else 
			cmdfin=0;
//		led=cmdfin;
//		DSK6713_rset(DSK6713_USER_REG, ~led);
	}

	if(cmdfin==1) {
		cmd_exec(cmdbuf);
		UART_send_block(cmdbuf, cmdcnt);
		cmdcnt=0;
	}
}

void cmd_exec(unsigned char *cmdbuf)
{
	char cmd[20];
	int n;
	double val;
	
	n=sscanf(cmdbuf, "%s %lf",cmd,&val);

    printf("argnum: %d, cmd: %s, val: %f\n",n,cmd,val);
	
	if(strcmp(cmd,"GAIN1")==0) gain1=val;
	else
	if(strcmp(cmd,"GAIN2")==0) gain2=val;
	else
	if(strcmp(cmd,"GAIN3")==0) gain3=val;
	else
	if(strcmp(cmd,"GAIN4")==0) gain4=val;
	else
	if(strcmp(cmd,"SHOW")==0)
    {
		printf("---------\n");
		printf("gain1: %f\n",gain1);
		printf("gain2: %f\n",gain2);
		printf("gain3: %f\n",gain3);
		printf("gain4: %f\n",gain4);
		printf("---------\n");
	}

//	printf("%d\n",(int)(((signed short)gain1*10.)*0x00ff));
}	

interrupt void int4(void)
{
	signed short Ix_n, Qx_n, Ex_n, Iy_n, Qy_n, Ey_n, Sx_n, Sy_n,
	             M1x_o, M2x_o, M3x_o, M1y_o, M2y_o, M3y_o, M4x_o, M4y_o;
	double Ix, Qx, Ex, Iy, Qy, Ey, Sx, Sy, 
	       M1x, M2x, M3x, M1y, M2y, M3y, M4x, M4y;
    int m;

/**Input**/
	Ix_n = *(volatile signed short *)(BASE_AI20 + AI2_AD0);
    Qx_n = *(volatile signed short *)(BASE_AI20 + AI2_AD1);
	Ex_n = *(volatile signed short *)(BASE_AI20 + AI2_AD2);	
	Sx_n = *(volatile signed short *)(BASE_AI21 + AI2_AD2);
	Iy_n = *(volatile signed short *)(BASE_AI20 + AI2_AD3);
	Qy_n = *(volatile signed short *)(BASE_AI21 + AI2_AD0);
	Ey_n = *(volatile signed short *)(BASE_AI21 + AI2_AD1);
	Sy_n = *(volatile signed short *)(BASE_AI21 + AI2_AD3);

/**Input-change**/
	Ix = (double)Ix_n ;
	Qx = (double)Qx_n ;
	Ex = (double)Ex_n ;
	Sx = (double)Sx_n ;
	Iy = (double)Iy_n ;
	Qy = (double)Qy_n ;	
	Ey = (double)Ey_n ;
	Sy = (double)Sy_n ;
/** 6.10352e-5*/
/*Auto matrix measurement*/
/*	Ix = _fabs(Ix);
	Iy = _fabs(Iy);
	Qx = _fabs(Qx);
	Qy = _fabs(Qy);
	Ex = _fabs(Ex);
	Ey = _fabs(Ey);
*/
//	printf("%e\n", Ix);
//
	if (CTime < 1)
	{    /*buffer clear*/	
	    for (m=0; m<16; m++) 
    	{
    		databuf[m] = 0.0; 
    	}
	    M1x = ys[0]*3271.;
		M2x = 0.;
		M3x = 0.;
		M4x = 0.;
		M1y = 0.;
		M2y = 0.;
		M3y = 0.;
		M4y = 0.;
		CTime++;
	}	

	else if (CTime < 2000001) /* LPF 0.1Hz*/
	{
	    M1x = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[0] += 3.053e-10 * Ix * ys[0];
		databuf[1] += 3.053e-10 * Ix * yc[0];
       	databuf[2] += 3.053e-10 * Qx * ys[0];
		databuf[3] += 3.053e-10 * Qx * yc[0];
     	databuf[4] += 3.053e-10 * Ex * ys[0];
		databuf[5] += 3.053e-10 * Ex * yc[0];
     	databuf[6] += 3.053e-10 * Sx * ys[0];
		databuf[7] += 3.053e-10 * Sx * yc[0];
       	databuf[8] += 3.053e-10 * Iy * ys[0];
		databuf[9] += 3.053e-10 * Iy * yc[0];
       	databuf[10] += 3.053e-10 * Qy * ys[0];
		databuf[11] += 3.053e-10 * Qy * yc[0];
       	databuf[12] += 3.053e-10 * Ey * ys[0];
		databuf[13] += 3.053e-10 * Ey * yc[0];
       	databuf[14] += 3.053e-10 * Sy * ys[0];
		databuf[15] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}
	else if (CTime < 2000002)
	{    /*buffer clear*/	
	    for (m=16; m<32; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;
	    M1x = 0.;
	    M2x = ys[0]*3271.;
		M3x = 0.;
		M4x = 0.;
		M1y = 0.;
		M2y = 0.;
		M3y = 0.;
		M4y = 0.;
		CTime++;
	}	
	else if (CTime < 4000002) /* LPF 0.1Hz*/
	{
	    M2x = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[16] += 3.053e-10 * Ix * ys[0];
		databuf[17] += 3.053e-10 * Ix * yc[0];
       	databuf[18] += 3.053e-10 * Qx * ys[0];
		databuf[19] += 3.053e-10 * Qx * yc[0];
     	databuf[20] += 3.053e-10 * Ex * ys[0];
		databuf[21] += 3.053e-10 * Ex * yc[0];
     	databuf[22] += 3.053e-10 * Sx * ys[0];
		databuf[23] += 3.053e-10 * Sx * yc[0];
       	databuf[24] += 3.053e-10 * Iy * ys[0];
		databuf[25] += 3.053e-10 * Iy * yc[0];
       	databuf[26] += 3.053e-10 * Qy * ys[0];
		databuf[27] += 3.053e-10 * Qy * yc[0];
       	databuf[28] += 3.053e-10 * Ey * ys[0];
		databuf[29] += 3.053e-10 * Ey * yc[0];
       	databuf[30] += 3.053e-10 * Sy * ys[0];
		databuf[31] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}
		else if (CTime < 4000003)
	{    /*buffer clear*/	
	    for (m=32; m<48; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;	    
		M1x = 0.;
	    M2x = 0.;
		M3x = ys[0]*3271.;
		M4x = 0.;
		M1y = 0.;
		M2y = 0.;
		M3y = 0.;
		M4y = 0.;
		CTime++;
	}	
	else if (CTime < 6000003) /* LPF 0.1Hz*/
	{
	    M3x = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[32] += 3.053e-10 * Ix * ys[0];
		databuf[33] += 3.053e-10 * Ix * yc[0];
       	databuf[34] += 3.053e-10 * Qx * ys[0];
		databuf[35] += 3.053e-10 * Qx * yc[0];
     	databuf[36] += 3.053e-10 * Ex * ys[0];
		databuf[37] += 3.053e-10 * Ex * yc[0];
     	databuf[38] += 3.053e-10 * Sx * ys[0];
		databuf[39] += 3.053e-10 * Sx * yc[0];
       	databuf[40] += 3.053e-10 * Iy * ys[0];
		databuf[41] += 3.053e-10 * Iy * yc[0];
       	databuf[42] += 3.053e-10 * Qy * ys[0];
		databuf[43] += 3.053e-10 * Qy * yc[0];
       	databuf[44] += 3.053e-10 * Ey * ys[0];
		databuf[45] += 3.053e-10 * Ey * yc[0];
       	databuf[46] += 3.053e-10 * Sy * ys[0];
		databuf[47] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}

		else if (CTime < 6000004)
	{    /*buffer clear*/	
	    for (m=48; m<64; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;	    
		M1x = 0.;
	    M2x = 0.;
		M3x = 0.;
		M4x = ys[0]*3271.;
		M1y = 0.;
		M2y = 0.;
		M3y = 0.;
		M4y = 0.;
		CTime++;
	}	
	else if (CTime < 8000004) /* LPF 0.1Hz*/
	{
	    M4x = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[48] += 3.053e-10 * Ix * ys[0];
		databuf[49] += 3.053e-10 * Ix * yc[0];
       	databuf[50] += 3.053e-10 * Qx * ys[0];
		databuf[51] += 3.053e-10 * Qx * yc[0];
     	databuf[52] += 3.053e-10 * Ex * ys[0];
		databuf[53] += 3.053e-10 * Ex * yc[0];
     	databuf[54] += 3.053e-10 * Sx * ys[0];
		databuf[55] += 3.053e-10 * Sx * yc[0];
       	databuf[56] += 3.053e-10 * Iy * ys[0];
		databuf[57] += 3.053e-10 * Iy * yc[0];
       	databuf[58] += 3.053e-10 * Qy * ys[0];
		databuf[59] += 3.053e-10 * Qy * yc[0];
       	databuf[60] += 3.053e-10 * Ey * ys[0];
		databuf[61] += 3.053e-10 * Ey * yc[0];
       	databuf[62] += 3.053e-10 * Sy * ys[0];
		databuf[63] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}

		else if (CTime < 8000005)
	{    /*buffer clear*/	
	    for (m=64; m<80; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;
		M1x = 0.;
	    M2x = 0.;
		M3x = 0.;
		M4x = 0.;
		M1y = ys[0]*3271.;
		M2y = 0.;
		M3y = 0.;
		M4y = 0.;
		CTime++;
	}	
	else if (CTime < 10000005) /* LPF 0.1Hz*/
	{
	    M1y = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[64] += 3.053e-10 * Ix * ys[0];
		databuf[65] += 3.053e-10 * Ix * yc[0];
       	databuf[66] += 3.053e-10 * Qx * ys[0];
		databuf[67] += 3.053e-10 * Qx * yc[0];
     	databuf[68] += 3.053e-10 * Ex * ys[0];
		databuf[69] += 3.053e-10 * Ex * yc[0];
     	databuf[70] += 3.053e-10 * Sx * ys[0];
		databuf[71] += 3.053e-10 * Sx * yc[0];
       	databuf[72] += 3.053e-10 * Iy * ys[0];
		databuf[73] += 3.053e-10 * Iy * yc[0];
       	databuf[74] += 3.053e-10 * Qy * ys[0];
		databuf[75] += 3.053e-10 * Qy * yc[0];
       	databuf[76] += 3.053e-10 * Ey * ys[0];
		databuf[77] += 3.053e-10 * Ey * yc[0];
       	databuf[78] += 3.053e-10 * Sy * ys[0];
		databuf[79] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}
		else if (CTime < 10000006)
	{    /*buffer clear*/	
	    for (m=80; m<96; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;
	    M1x = 0.;
	    M2x = 0.;
		M3x = 0.;
		M4x = 0.;
		M1y = 0.;
		M2y = ys[0]*3271.;
		M3y = 0.;
		M4y = 0.;
		CTime++;
	}	
	else if (CTime < 12000006) /* LPF 0.1Hz*/
	{
	    M2y = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[80] += 3.053e-10 * Ix * ys[0];
		databuf[81] += 3.053e-10 * Ix * yc[0];
       	databuf[82] += 3.053e-10 * Qx * ys[0];
		databuf[83] += 3.053e-10 * Qx * yc[0];
     	databuf[84] += 3.053e-10 * Ex * ys[0];
		databuf[85] += 3.053e-10 * Ex * yc[0];
     	databuf[86] += 3.053e-10 * Sx * ys[0];
		databuf[87] += 3.053e-10 * Sx * yc[0];
       	databuf[88] += 3.053e-10 * Iy * ys[0];
		databuf[89] += 3.053e-10 * Iy * yc[0];
       	databuf[90] += 3.053e-10 * Qy * ys[0];
		databuf[91] += 3.053e-10 * Qy * yc[0];
       	databuf[92] += 3.053e-10 * Ey * ys[0];
		databuf[93] += 3.053e-10 * Ey * yc[0];
       	databuf[94] += 3.053e-10 * Sy * ys[0];
		databuf[95] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}
		else if (CTime < 12000007)
	{    /*buffer clear*/	
	    for (m=96; m<112; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;
	    M1x = 0.;
	    M2x = 0.;
		M3x = 0.;
		M4x = 0.;
		M1y = 0.;
		M2y = 0.;
		M3y = ys[0]*3271.;
		M4y = 0.;
		CTime++;
	}	
	else if (CTime < 14000007) /* LPF 0.1Hz*/
	{
	    M3y = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[96] += 3.053e-10 * Ix * ys[0];
		databuf[97] += 3.053e-10 * Ix * yc[0];
       	databuf[98] += 3.053e-10 * Qx * ys[0];
		databuf[99] += 3.053e-10 * Qx * yc[0];
     	databuf[100] += 3.053e-10 * Ex * ys[0];
		databuf[101] += 3.053e-10 * Ex * yc[0];
     	databuf[102] += 3.053e-10 * Sx * ys[0];
		databuf[103] += 3.053e-10 * Sx * yc[0];
       	databuf[104] += 3.053e-10 * Iy * ys[0];
		databuf[105] += 3.053e-10 * Iy * yc[0];
       	databuf[106] += 3.053e-10 * Qy * ys[0];
		databuf[107] += 3.053e-10 * Qy * yc[0];
       	databuf[108] += 3.053e-10 * Ey * ys[0];
		databuf[109] += 3.053e-10 * Ey * yc[0];
       	databuf[110] += 3.053e-10 * Sy * ys[0];
		databuf[111] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}

		else if (CTime < 14000008)
	{    /*buffer clear*/	
	    for (m=112; m<128; m++)
    	{
    		databuf[m] = 0.0; 
    	}
		ys[0] = 0.;
		ys[1] = 3.926980723806e-3;
		ys[2] = 0.;
		yc[0] = 0.;
		yc[0] = 0.999992289381471;
		yc[0] = 1.;
	    M1x = 0.;
	    M2x = 0.;
		M3x = 0.;
		M4x = 0.;
		M1y = 0.;
		M2y = 0.;
		M3y = 0.;
		M4y = ys[0]*3271.;
		CTime++;
	}	
	else if (CTime < 16000008) /* LPF 0.1Hz*/
	{
	    M4y = ys[0] * 3271.; /* 100mV sin wave*/
       	databuf[112] += 3.053e-10 * Ix * ys[0];
		databuf[113] += 3.053e-10 * Ix * yc[0];
       	databuf[114] += 3.053e-10 * Qx * ys[0];
		databuf[115] += 3.053e-10 * Qx * yc[0];
     	databuf[116] += 3.053e-10 * Ex * ys[0];
		databuf[117] += 3.053e-10 * Ex * yc[0];
     	databuf[118] += 3.053e-10 * Sx * ys[0];
		databuf[119] += 3.053e-10 * Sx * yc[0];
       	databuf[120] += 3.053e-10 * Iy * ys[0];
		databuf[121] += 3.053e-10 * Iy * yc[0];
       	databuf[122] += 3.053e-10 * Qy * ys[0];
		databuf[123] += 3.053e-10 * Qy * yc[0];
       	databuf[124] += 3.053e-10 * Ey * ys[0];
		databuf[125] += 3.053e-10 * Ey * yc[0];
       	databuf[126] += 3.053e-10 * Sy * ys[0];
		databuf[127] += 3.053e-10 * Sy * yc[0];
		CTime++;
	}

	else 
	{
//		databuf[2] = sqrt(databuf[0] * databuf[0] + databuf[1] * databuf[1]);
//		databuf[3] = atan2(databuf[1], databuf[0]);

/*		printf("IM-x    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[0], databuf[2], databuf[4], databuf[6], databuf[8], databuf[10], databuf[12], databuf[14]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[1], databuf[3], databuf[5], databuf[7], databuf[9], databuf[11], databuf[13], databuf[15]);

		printf("QM-x    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[16], databuf[18], databuf[20], databuf[22], databuf[24], databuf[26], databuf[28], databuf[30]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[17], databuf[19], databuf[21], databuf[23], databuf[25], databuf[27], databuf[29], databuf[31]);

		printf("EM-x    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[32], databuf[34], databuf[36], databuf[38], databuf[40], databuf[42], databuf[44], databuf[46]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[33], databuf[35], databuf[37], databuf[39], databuf[41], databuf[43], databuf[45], databuf[47]);

		printf("SM-x    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[48], databuf[50], databuf[52], databuf[54], databuf[56], databuf[58], databuf[60], databuf[62]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[49], databuf[51], databuf[53], databuf[55], databuf[57], databuf[59], databuf[61], databuf[63]);
*/
		printf("IM-y    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[64], databuf[66], databuf[68], databuf[70], databuf[72], databuf[74], databuf[76], databuf[78]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[65], databuf[67], databuf[69], databuf[71], databuf[73], databuf[75], databuf[77], databuf[79]);

		printf("QM-y    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[80], databuf[82], databuf[84], databuf[86], databuf[88], databuf[90], databuf[92], databuf[94]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[81], databuf[83], databuf[85], databuf[87], databuf[89], databuf[91], databuf[93], databuf[95]);

		printf("EM-y    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[96], databuf[98], databuf[100], databuf[102], databuf[104], databuf[106], databuf[108], databuf[110]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[97], databuf[99], databuf[101], databuf[103], databuf[105], databuf[107], databuf[109], databuf[111]);

		printf("SM-y    *** Ix ***    *** Qx ***    *** Ex ***    *** Sx ***    *** Iy ***    *** Qy ***    *** Ey ***    *** Sy *** \n");
		printf("Sin:%15e %13e %13e %13e %13e %13e %13e %13e\n"  , databuf[112], databuf[114], databuf[116], databuf[118], databuf[120], databuf[122], databuf[124], databuf[126]);
		printf("Cos:%15e %13e %13e %13e %13e %13e %13e %13e\n\n", databuf[113], databuf[115], databuf[117], databuf[119], databuf[121], databuf[123], databuf[125], databuf[127]);

		CTime =0;
	}

/*
	   if (CTime < 200000) 
		{
		   CTime++;
		   Ixc = Ixc + Ix;
		   Iyc = Iyc + Iy;
		   Qxc = Qxc + Qx;
		   Qyc = Qyc + Qy;
		   Exc = Exc + Ex;
		   Eyc = Eyc + Ey;
		}
	   else
	    {
		   printf("%15e %15e %15e %15e %15e %15e\n", Ixc, Iyc, Qxc, Qyc, Exc, Eyc);
	       CTime = 0;
		   Ixc = 0.;
		   Iyc = 0.;
		   Qxc = 0.;
		   Qyc = 0.;
		   Exc = 0.;
		   Eyc = 0.;
		}
*/	
/**Matrix*
	M1x =  am[0][0] * Ix + am[0][1] * Qx + am[0][2] * Ex
	     + am[0][3] * Iy + am[0][4] * Qy + am[0][5] * Ey;
	M2x =  am[1][0] * Ix + am[1][1] * Qx + am[1][2] * Ex
	     + am[1][3] * Iy + am[1][4] * Qy + am[1][5] * Ey;
	M3x =  am[2][0] * Ix + am[2][1] * Qx + am[2][2] * Ex
	     + am[2][3] * Iy + am[2][4] * Qy + am[2][5] * Ey;
	M1y =  am[3][0] * Ix + am[3][1] * Qx + am[3][2] * Ex
	     + am[3][3] * Iy + am[3][4] * Qy + am[3][5] * Ey;
	M2y =  am[4][0] * Ix + am[4][1] * Qx + am[4][2] * Ex
	     + am[4][3] * Iy + am[4][4] * Qy + am[4][5] * Ey;
	M3y =  am[5][0] * Ix + am[5][1] * Qx + am[5][2] * Ex
	     + am[5][3] * Iy + am[5][4] * Qy + am[5][5] * Ey;
*/
/*Function genelator*/

	ys[0] = as1*ys[1] + as2*ys[2];    /* Get Y(n)*/
	ys[2] = ys[1];                 /* delay   */
	ys[1] = ys[0];                 /* delay   */

//    M1x = ys[0]*20000.;

	yc[0] = ac1*yc[1] + ac2*yc[2];    /* Get Y(n)*/
	yc[2] = yc[1];                 /* delay   */
	yc[1] = yc[0];                 /* delay   */

//    M1y = yc[0]*20000;

/***************LPF************************/
/*        un = 1.999724537 * u0 -0.9997245557 * u1 + Ex;
        Ex = 1. * un 
            -1.996971989 * u0 
            + 0.9969742812 * u1;
        u1 = u0;          
        u0 = un;          

       M3x = Ex;
		M3y = Ey;*/
/***************************************/

/**Output-change**/
    M1x_o = (signed short)M1x;
    M2x_o = (signed short)M2x;
    M3x_o = (signed short)M3x;
    M4x_o = (signed short)M4x;
    M1y_o = (signed short)M1y;
    M2y_o = (signed short)M2y;
    M3y_o = (signed short)M3y;
    M4y_o = (signed short)M4y;    	

/**Output**/	
	*(volatile signed short *)(BASE_AO22 + AO2_DA0) = M1x_o;
	*(volatile signed short *)(BASE_AO22 + AO2_DA1) = M2x_o;
	*(volatile signed short *)(BASE_AO22 + AO2_DA2) = M3x_o;
	*(volatile signed short *)(BASE_AO23 + AO2_DA2) = M4x_o;
	*(volatile signed short *)(BASE_AO22 + AO2_DA3) = M1y_o;
	*(volatile signed short *)(BASE_AO23 + AO2_DA0) = M2y_o;
	*(volatile signed short *)(BASE_AO23 + AO2_DA1) = M3y_o;
	*(volatile signed short *)(BASE_AO23 + AO2_DA3) = M4y_o;
}

void dsp_init(void)
{
	volatile int i;
	
	// Clock Set
 	// PLLOUT:450MHz  SYSCLK1(DSP Core):225MHz  SYSCLK2(Peripherals):112.5MHz  SYSCLK3(EMIF):90MHz
	*(volatile unsigned int *)_CHIP_PLLCSR_ADDR &= ~0x00001;/* PLL Disable(BypassMode)	*/
	*(volatile unsigned int *)_CHIP_PLLCSR_ADDR |= 0x00008;	/* PLL Reset Asserte */
	*(volatile unsigned int *)_CHIP_PLLDIV0_ADDR = 0x08000;	/* Divide 1		*/
	*(volatile unsigned int *)_CHIP_PLLM_ADDR = 0x00009;	/* multiple 9 	*/
	*(volatile unsigned int *)_CHIP_PLLDIV1_ADDR = 0x08001;	/* Divide 2		*/
	*(volatile unsigned int *)_CHIP_PLLDIV2_ADDR = 0x08003;	/* Divide 4		*/
	*(volatile unsigned int *)_CHIP_PLLDIV3_ADDR = 0x08004;	/* Divide 5		*/
	*(volatile unsigned int *)_CHIP_DEVCFG_ADDR &= ~0x0010;	/* DEVCFG:EKSRC=0 (EMIF input clock source is SYSCLK3)	*/
	*(volatile unsigned int *)_CHIP_PLLCSR_ADDR &= ~0x00008;/* PLL Reset Release */
	for(i=0;i<=100;i++);
	*(volatile unsigned int *)_CHIP_PLLCSR_ADDR |= 0x00001;	/* PLL Enable	*/
}


/****************************************************************************/
/*  HEG DSK I/F Initialize routine                                          */
/****************************************************************************/
void dskif_init(void)
{
	*(volatile unsigned int *)(BASE_AI20 + AI2_ADSETFLG) = 0x0000;
	*(volatile unsigned int *)(BASE_AI20 + AI2_USERLED) = 0x0000;
	*(volatile unsigned int *)(BASE_AI20 + AI2_USERO) = 0x0000;
	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG1) = 0x0000;
	*(volatile unsigned int *)(BASE_AI20 + AI2_SETREG2) = 0x0000;

	*(volatile unsigned int *)(BASE_AI21 + AI2_ADSETFLG) = 0x0000;
	*(volatile unsigned int *)(BASE_AI21 + AI2_USERLED) = 0x0000;
	*(volatile unsigned int *)(BASE_AI21 + AI2_USERO) = 0x0000;
	*(volatile unsigned int *)(BASE_AI21 + AI2_SETREG1) = 0x0000;
	*(volatile unsigned int *)(BASE_AI21 + AI2_SETREG2) = 0x0000;

	*(volatile unsigned int *)(BASE_AO22 + AO2_DASETFLG) = 0x0000;
	*(volatile unsigned int *)(BASE_AO22 + AO2_USERLED) = 0x0000;
	*(volatile unsigned int *)(BASE_AO22 + AO2_USERO) = 0x0000;
	*(volatile unsigned int *)(BASE_AO22 + AO2_SETREG1) = 0x0000;
	*(volatile unsigned int *)(BASE_AO22 + AO2_SETREG2) = 0x0000;

	*(volatile unsigned int *)(BASE_AO23 + AO2_DASETFLG) = 0x0000;
	*(volatile unsigned int *)(BASE_AO23 + AO2_USERLED) = 0x0000;
	*(volatile unsigned int *)(BASE_AO23 + AO2_USERO) = 0x0000;
	*(volatile unsigned int *)(BASE_AO23 + AO2_SETREG1) = 0x0000;
	*(volatile unsigned int *)(BASE_AO23 + AO2_SETREG2) = 0x0000;
}


/****************************************************************************/
/*  enableGlobalINT() - enable GIE                                          */
/****************************************************************************/
void enableGlobalINT(void)
{
    CSR |= 0x1;
}

/****************************************************************************/
/*  enableNMI() - enable NMIE                                               */
/****************************************************************************/
void enableNMI(void)
{
    IER = _set(IER, 1, 1);
}

/****************************************************************************/
/*  enableSpecificINT() - enable specific interrupt                         */
/*                                                                          */
/*  Arguments -                                                             */
/*    INTnumber - interrupt number to be enabled                            */
/****************************************************************************/
void enableSpecificINT(INTnumber)
{
    IER = _set(IER, INTnumber, INTnumber);  /* set specific IER bits        */
}

/****************************************************************************/
/****************************************************************************/


/************************************************************	

	Below are the subroutines described in the documentation
	
************************************************************/
void initialize_UART (unsigned char divisor)
{
//		int temp;
		UART_LCR1 = 0x80;	// Set "Divisor Latch Access Bit"
		UART_DLL1 = divisor;	// Divisor set to 18h for 9600 bps, 0ch for 19.2K, 01h for 230,400 bps
		UART_DLM1 = 0x00;	// Upper 8 bits of divisor
	
		UART_LCR1 = 0x00;	// Clear DLAB bit in Line Control Register
//		UART_LCR1 = 0x07;	// 8 data bits, 2 stop bit, no parity
		UART_LCR1 = 0x03;	// 8 data bits, 1 stop bit, no parity
		UART_IER1 = 0x01;	// Enable RX int.
		UART_FCR1 = 0x09;	// Disable FIFO
		UART_MCR1 = 0x0b;	// Take Interrupt signals out of 3-state
}


/********************************************************
	The following routine first checks if there is space
	in the UART transmit buffer for one byte. If there is, it
	places that single character (byte)	into the buffer. 
	If there is no room in the transmit buffer,
	the routine returns -1. 
	The actual sending of the byte to the UART will be
	handled transparently by the interrupt routine when
	the UART is free to accept the byte.
********************************************************/	
int UART_send_byte (unsigned char byte)
{
	if (uiUART_xmit_count1 >= XMIT_BUF_SIZE) return (-1);
	ucUART_xmit_buffer1[uiUART_xmit_nextin1++]=byte;
	if (uiUART_xmit_nextin1 >= XMIT_BUF_SIZE) uiUART_xmit_nextin1=0;
		
	/*	disable interrupts around the following statement	*/
	IRQ_disable(IRQ_EVT_EXTINT5);	// disable external interrupt 5
	uiUART_xmit_count1++;
	UART_IER1 = 0x03;
		
	IRQ_enable(IRQ_EVT_EXTINT5);	// enable external interrupt 5
	return (0);
}

/********************************************************
	The following routine reads a single character (byte)
	from the UART receive buffer. If the receive buffer is
	empty, -1 is returned.
********************************************************/	
int UART_recv_byte (unsigned char* byte)
{
	if (uiUART_recv_count1 == 0) return (-1);
	*byte = ucUART_recv_buffer1[uiUART_recv_nextout1++];
	if (uiUART_recv_nextout1 >= RECV_BUF_SIZE) uiUART_recv_nextout1=0;
		
	/*	disable interrupts around the following statement	*/
	IRQ_disable(IRQ_EVT_EXTINT5);	// disable external interrupt 5
	uiUART_recv_count1--;
	IRQ_enable(IRQ_EVT_EXTINT5);	// enable external interrupt 5
	return (0);
}

/********************************************************
	The following routine first checks if there is space
	in the UART transmit buffer for 'length' characters.
	If there is, it	places the  characters (bytes) into
	the buffer. If there is not enough room in the
	transmit buffer, the routine returns -1, and does
	not place anything in the transmit buffer.
	The actual sending of the bytes to the UART will be
	handled transparently by the interrupt routine when
	the UART is free to accept data.
********************************************************/	
int UART_send_block (unsigned char* buf, unsigned int length)
{
unsigned int index;
	if ((uiUART_xmit_count1+length-1) >= XMIT_BUF_SIZE) return (-1);
	for (index=0; index<length; index++)
	{
		ucUART_xmit_buffer1[uiUART_xmit_nextin1++]=buf[index];
		if (uiUART_xmit_nextin1 >= XMIT_BUF_SIZE) uiUART_xmit_nextin1=0;
	}
	/*	disable interrupts around the following statement	*/
	IRQ_disable(IRQ_EVT_EXTINT5);	// disable external interrupt 5
	uiUART_xmit_count1 += length;
	UART_IER1 = 0x03;
	IRQ_enable(IRQ_EVT_EXTINT5);	// enable external interrupt 5
	return (0);
}

/********************************************************
	The following routine first checks to see if there are
	at least 'length' bytes in the receive buffer.
	If there are, it reads the  characters (bytes) into a
	memory buffer pointed to by the passed argument: *buf. 
	If there are not enough characters in the receive buffer
	to satisy the requested block length,
	the routine returns -1.
	
********************************************************/	
int UART_recv_block (unsigned char* buf, unsigned int length)
{
unsigned int index;
	if (uiUART_recv_count1 < length) return (-1);
	for (index=0; index<length; index++)
	{
		buf[index] = ucUART_recv_buffer1[uiUART_recv_nextout1++];
		if (uiUART_recv_nextout1 >= RECV_BUF_SIZE) uiUART_recv_nextout1=0;
	}
	/*	disable interrupts around the following statement	*/
	IRQ_disable(IRQ_EVT_EXTINT5);	// disable external interrupt 5
	uiUART_recv_count1 -= length;
	IRQ_enable(IRQ_EVT_EXTINT5);	// enable external interrupt 5
	return (0);
}

/********************************************************
	The following routine returns the number of characters
	in the receive memory buffer. Note that the interrupt
	service routine (ISR) could potentially add a character to
	the receive buffer immediately after this function is
	called. This will not harm the integrity of the data.
	Any characters placed in the receive buffer by the
	ISR in this way will be picked up the next time this
	function is called.
	
********************************************************/
unsigned int UART_recv_count(void)
{
	return (uiUART_recv_count1);
}

/********************************************************
	The following routine returns the number of characters
	in the transmit memory buffer. Note that the interrupt
	service routine (ISR) could potentially remove a character from
	the receive buffer immediately after this function is
	called. This will not harm the integrity of the data.
	
	
********************************************************/
unsigned int UART_xmit_count(void)
{
	return (uiUART_xmit_count1);
}

/*****************************************
	The following interrupt routine is triggered by a rising edge on 
	External Interrupt #4. This ISR handles the UART's transmit data and
	receive data interrupts for all UART channels.
	
*****************************************/

void ext_int(void)
{
	unsigned char temp;
	volatile unsigned char trash;

//   If the uart is ready to accept a char, AND there is something
//	 to send, send it, and adjust the pointer and count

	while(1)
	{		//  service channel 1 transmitter
		temp = UART_LSR1;
		if (((temp & 0x20) == 0x20) && uiUART_xmit_count1) 
		{
			if(uiUART_xmit_count1 == 1) UART_IER1 = 0x01; // if last byte, disable tx int
			UART_THR1 = ucUART_xmit_buffer1[uiUART_xmit_nextout1++];
			if(uiUART_xmit_nextout1 >= XMIT_BUF_SIZE) uiUART_xmit_nextout1 = 0;
			uiUART_xmit_count1--;
		}

// If a character is waiting in the UART FIFO, and there
//	is room in the receive buffer, get the character, and
//	adjust the pointer and count.

			//  service channel 1 receiver
	temp = UART_ISR1;
	if ((temp & 0x05) == 0x04)
	{
		if (uiUART_recv_count1 < RECV_BUF_SIZE) 
		{
			ucUART_recv_buffer1[uiUART_recv_nextin1++] = UART_RBR1;
			if(uiUART_recv_nextin1 >= RECV_BUF_SIZE) uiUART_recv_nextin1 = 0;
			uiUART_recv_count1++;
		}
		else
			trash = UART_RBR1;	// discard char if buffer is full						
	}	 
		 	
	// It is imperative that that no interrupts are pending when this ISR
	// exits. So check if any more interrupts are pending and service them.
	if ((UART_ISR1 & 0x01) == 0) continue;
	break;	
	}
}

/* The following delay routine is used to eat up some tine during
   initialization. For a 6713 running at 225 MHz (i.e. the 6713 DSK),
   a value of 8000 passed to this routine causes a delay of 
   approximately 1 millisecond.
*/   
void 	delay(long value)
{
  volatile double x;
  long i;
	for(i=0;i< value; i++)
	{
		x=1;
	}
}