void Usual_Routine() {
	InitializeStack();
	OSChangePrio (MAIN_PRIO);
	EnableAutoUpdate();
	EnableTaskMonitor();
	EnableSmartTraps();
	initPINS();
	//DSPIInit(3, 2000000, 16, 0x00, 1, 1, 1, 0, 0, 0); //Initializing the Hardware to talk to IMU
}
Exemple #2
0
void UserMain( void * pd )
{
    initWithWeb();
    EnableSmartTraps();

    iprintf("Application: %s\r\nNNDK Revision: %s\r\n",AppName,GetReleaseTag());
    iprintf("FlashCompare: %s\n", FlashCompare( SPI_Config_Base, (const uint8_t *)&gConfigRec, GCSTORE_SIZE) == 0
                                ? "Success" : "Failure");
    iprintf("FlashCompare: %s\n", FlashCompare( SPI_Config_Base, ((const uint8_t *)&gConfigRec)+1, GCSTORE_SIZE) == 0
                                ? "Success" : "Failure");
    while(1) {
        OSTimeDly(TICKS_PER_SECOND);
    }
}
Exemple #3
0
void UserMain( void * pd )
{
    Config();
    initWithWeb();
    EnableSmartTraps();

    iprintf("Application: %s\r\nNNDK Revision: %s\r\n",AppName,GetReleaseTag());
    getchar();
    EnableTrace(CS_NUM);
    while(1) {
//        for (uint16_t i = 0; i < 512; i++) {
//            trace = i;
//        }
        iprintf("SR: 0x%04X\n", GetSR_IntLevel());
        OSTimeDly(TICKS_PER_SECOND);
    }
}
/*****************************UserMain***************************************************/
void UserMain(void* pd) {
	/////Usual Routine
	InitializeStack();
	OSChangePrio (MAIN_PRIO);
	EnableAutoUpdate();
	EnableTaskMonitor();
	EnableSmartTraps();
	OSSimpleTaskCreate(Read_ADC, MAIN_PRIO + 1);

	OSTimeDly(100);
	//f_enterFS();

	while (1) {
		OSTimeDly(TICKS_PER_SECOND * 30);

		//f_flush(fp);

	}
	//f_releaseFS();

}
void Usual_Routine() {
	InitializeStack();
	OSChangePrio(MAIN_PRIO);
	EnableAutoUpdate();
	EnableTaskMonitor();
	EnableSmartTraps();
	initPINS();

	J2[32] = 1; //Resets the OSD

	OSTimeDly(2);

	DSPIInit(3, 2000000, 16, 0x00, 1, 1, 1, 0, 0, 0); //Initializing the Hardware to talk to IMU
	DSPIInit(1, 2000000, 16, 0x00, 0x01, 0, 0, 0, 0, 0); //Initializing the Hardware to talk to OSD

	/***********************************Initializing Initial Artificial Horizon*****************************************************************/

	uint16_t x = 2834; //Center Position of the Center Circle
	Display_Center(x); //Displaying the Center, argument center address
	Display_Data(); //Initializing Pitch and Roll value Display
}
//=====================================System Initializations========================================//
void _init (void){
	int i; DWORD res = 0;
	//-------------------------------------General System--------------------------------------------//
	SimpleUart(0,SystemBaud);assign_stdio(0);				// Serial port 0 for Data
	SimpleUart(1,SystemBaud);								// Serial port 1 for Debug
	EnableSmartTraps();
	#ifdef _DEBUG
		InitGDBStubNoBreak( 1, 115200 );
	#endif
	OSChangePrio(MAIN_PRIO);								//Other
	EnableSerialUpdate();

	iprintf("\n\n\n\n\n\n\n\nINITIALIZATION\n");
	putleds(8);
	iprintf("Version Number --> %s\n", ftos(versionNo, 3));
	createVERIFY_KEY();										// Using versionNo to update Flash DDBB

	//--------------------------------------I2C Bus Connection---------------------------------------//
	I2CInit(MCF5213_I2C_ADDRESS, I2C_FREQUENCY); 			// Initialize I2C with Predefined Address (0x20)
															// Set frequency division to 768 (66Mhz CF clock --> 88,25 Khz I2C bus)
	iprintf(" .Initialized I2C address for MCF5213: 0x%x\r\n",MCF5213_I2C_ADDRESS);
	iprintf("   -Set I2C Frequency division: %x (MCF internal CLK / 1280)\r\n",I2C_FREQUENCY);


	//-------------------------------------SPI Bus Connection----------------------------------------//
	res=true;
	for (i = 0; i<PSU_NUMBER; i++){
		res &=configureSPI( INIT_SPI_ORDER, INIT_SPI_CPOL, INIT_SPI_CPHA, INIT_SPI_CLKRATE, i2CtoSPIAddress[i]);
	}
	if(res==I2C_OK){iprintf(" .SPI bus configured\r\n");}
	else{iprintf(" .SPI configuration ERROR - Coudn't be configured\r\n");}

	//-----------------------------------RDACs with minimum value------------------------------------//

	res = 0;
	for (i = 0; i<PSU_NUMBER; i++){
		setValRDAC(INITIAL_VOLTAGE, (i&0x1?UPPER_SLAVE_SPI_ADDRESS:LOWER_SLAVE_SPI_ADDRESS), i2CtoSPIAddress[i]);
		res |= getI2CResultRDAC();
	}

	if(res==I2C_OK){iprintf(" .RDACs set to minimum value\r\n");}
	else{iprintf(" .RDAC setting ERROR - Couldn't be set to minimum value\r\n");}


	//----------------------------------------DataLists----------------------------------------------//
	initializeValuesPSUsSnIs();				// Load psuList values from RAM or set them to default
	for (i = 0; i<PSU_NUMBER; i++){			// Start all PSU objects in OFF status, regardless of the loaded status
		psuList[i].relayStatus = OFF;
		psuList[i].psuStatus = OFF;
		psuList[i].ReadyToConnect = false;
	}

	//-------------------------------------------GPIO------------------------------------------------//
	RESET_RHEOSTATS.function( PIN21_GPIO ); 	// RDAC Reset
	LED_OUT_ON.function( PIN7_GPIO ); 			// LED OUT
	iprintf(" .Pin 21 (TIN3) set as GPIO for RDAC Reset\r\n");
	iprintf(" .Pin 7 (IRQ4) set as GPIO for LED OUT\r\n");

	//----------------------------------Analog to Digital Converter----------------------------------//
	Pins[13].function( PIN13_AN0 );					// Configure the A2D pin nº13 as analog input
	EnableAD();
	initMUX();										// Initializes all the Pins used for Mux control
	res=configureSPI( INIT_SPI_ORDER, INIT_SPI_CPOL, INIT_SPI_CPHA, INIT_SPI_CLKRATE, AGC_I2C_ADDRESS);
	minAGC();										// Initializes the AGC with minimum gain
	res = (getI2CResultAGC()==I2C_OK);
	if(res){iprintf(" .ADC initialized\r\n");}
	else{iprintf(" .ADC initialization ERROR\r\n");}


	//--------------------------------------Button Interruption--------------------------------------//
	SetUpIRQ1();
	iprintf(" .Button Interruption Set\r\n");


	//---------------------------------------TimerInterruptions--------------------------------------//
	//SetUpPITR(0, 8294, 1); //Use PITR 0, Wait 8294 clocks, Divide by 2 from table 17-3, 2KHz - NOT IN USE, messes with RTOS Tasks. Best left unused
	SetUpPITR(1, 16588, 1); // Use PITR 1, Wait 16588 clocks, Divide by 2 from table 17-3, 1KHz - Interrupt in charge of A2D sampling
	iprintf(" .Timers initialized\r\n");

    //----------------------------------------- RTOS Tasks-------------------------------------------//
	OSSimpleTaskCreate( alarmTask, ALARM_PRIO );
	iprintf(" .alarmTask initialized\r\n");
	iprintf(" .MonitorSem %s\n", (allSemInit()==OS_NO_ERR?"Initialized CORRECTLY":"Initialization ERROR"));
	OSSimpleTaskCreate( monitorTask, MONITOR_PRIO );
	iprintf(" .monitorTask initialized\r\n");



	iprintf("APPLICATION INITIALIZED\n\nPress one key to begin \n\n");
	WAIT_FOR_KEYBOARD
	putleds(0);
}
Exemple #7
0
void UserMain(void * pd) {
    InitializeStack();
    OSChangePrio(MAIN_PRIO);
    EnableAutoUpdate();
    EnableTaskMonitor();

    #ifndef _DEBUG
    	EnableSmartTraps();
    #endif

    #ifdef _DEBUG
    	InitializeNetworkGDB_and_Wait();
    #endif

//    int fd = OpenSerial(9, 115200, 1, 8, eParityNone); // NetBurner 2.0: jack X5 - UART9, jack X4 - UART8

    PortsInit();

	iprintf("Initialization the LCD...\r\n");
	Display dis;
	dis.Init();

//	Tab *ta = new Tab("A", 170, 0, 22, 17, 8, 5 );
//	Tab *tb = new Tab("B", 170, 16, 22, 17, 8, 5 );
//	Tab *tc = new Tab("C", 170, 32, 22, 17, 7, 4 );
//	Tab *td = new Tab("All", 170, 48, 22, 16, 5, 4 );

		Tab *ta = new Tab("A", 170, 0, 22, 22, 8, 6 );
		Tab *tb = new Tab("B", 170, 21, 22, 22, 8, 6 );
		Tab *tc = new Tab("C", 170, 42, 22, 22, 7, 6 );


	Menu m;
	m.ta = ta;
	m.tb = tb;
	m.tc = tc;
//	m.td = td;
	m.disp = &dis;


	// Initialization Queues
	OSQInit( &BuzzerQueue, Buzzer_queue_data, BUZZER_QUEUE_SIZE );
	OSQInit( &DisplayQueue, Display_queue_data, DISPLAY_QUEUE_SIZE );

	// Initialization Tasks
	if( OSTaskCreate(PollKeyboardTask,
//					(void *) &fd,
					(void *) &m,
					(void *) &PollKeyboardTaskStack[USER_TASK_STK_SIZE],
					(void *) PollKeyboardTaskStack,
					MAIN_PRIO - 2 ) != OS_NO_ERR)
	{
		iprintf( "*** Error creating PollKeyboardTask\r\n" );
	}

	if( OSTaskCreate(DisplayingLCDTask,
					(void *) &m,
					(void *) &DisplayingLCDTaskStack[USER_TASK_STK_SIZE],
					(void *) DisplayingLCDTaskStack,
					MAIN_PRIO - 1 ) != OS_NO_ERR)
	{
		iprintf( "*** Error creating DisplayingLCDTask\r\n" );
	}

	if( OSTaskCreate(BuzzerTask,
					 NULL,
					(void *) &BuzzerTaskStack[USER_TASK_STK_SIZE],
					(void *) BuzzerTaskStack,
					MAIN_PRIO - 3 ) != OS_NO_ERR)
	{
		iprintf( "*** Error creating BuzzerTask\r\n" );
	}

    iprintf("Application started\n");
    while (1) {
        OSTimeDly(20);
    }
}
///////////////////////////////////MAIN FUNCTION//////////////////////////////////////////////
void UserMain( void* pd ){

	/////Usual Routine
	InitializeStack();
	OSChangePrio( MAIN_PRIO );//Change Main Task number to MAIN_PRIO
	EnableAutoUpdate();
	EnableTaskMonitor();
	EnableSmartTraps();

	//serviceWatchDog();

	//Starting the Code
	iprintf("\n\n\n..................Starting Sensor Interface Board.....................\n\n\n");



	//Local Variables
	//////Scan Flag

	/***********Defining Interrupt Timers*****************/
	HiResTimer* timer2=0;//50 Hz Interrupt Timer
	char m=0;



	/***********File Descriptor Variables****************/
	int startup_timeout=0;

	unsigned char F_range_buff[4]={0};//Radio filtered range

	/**********Radio Debug Variable*******************/
	double TotalTime=0;	char time_ms[2]={0};
	/***********Radio control Radiocount and loop counter i********************/
	uint8_t Radiocount3=0,Ant_config=0;

	uint16_t CRME=0;

	/*********Laser Rangefinder Variables***************/
	float laser_range=0;


	/**********ADC channel Array***************************/
	uint16_t ADC_channel[8] = {0};

	/**********Navcomp send buffer and other vriables**********************/
	char Navcomp_send_buff[48]={0};
	Navcomp_send_buff[0]=0x41;
	Navcomp_send_buff[1]=0x7A;
	Navcomp_send_buff[2]=0x05;
	uint16_t netburner_counter=0;//netburner 16 bit counter

	//Initialize pins
	initPINS();

	//Initialize Analog to Digital
	InitSingleEndAD();

	//Initializing Serial Ports
	SerialClose(5);
	SerialClose(7);
	SerialClose(9);
	SerialClose(8);


	fdRadio= OpenSerial( 8, 115200, 1, 8, eParityNone );
	fdDebug = OpenSerial( 5, 115200, 1, 8, eParityNone );
	fdLaser = OpenSerial( 7, 115200, 1, 8, eParityNone );
	fdNAVcomp = OpenSerial( 9, 115200, 1, 8, eParityNone );

	/*
	ReplaceStdio(0,fdDebug);
	ReplaceStdio(1,fdDebug);
	ReplaceStdio(2,fdDebug);
	*/

	//Start the Timers and init the DSPI
	//DSPIInit(1,2000000,16,0x01,0x01,1,1,0,0,0);//initializing SPI

	//printf("Going to wait 3 sec\n");
	//OSTimeDly(3*TICKS_PER_SECOND);
	initTIMERS(timer2);
	J1[7]=0;
	/*startup_timeout=ReadWithTimeout(fdDebug,&m,1,2);
	if(startup_timeout==-1 || startup_timeout==0){
		Start_PAN=StartUpLaserScan(fdLaser);
		if(Start_PAN!=11110)
			Scan_Status=1;
		else
			Scan_Status=0;
	}

	else{
		Scan_Status=0;
		Start_PAN=11110;
	}*/

	//printf("Hi\n");
	Scan_Complete=1;
	/***********packing startup PAN angle*************************/
	Navcomp_send_buff[38] = (uint8_t)((Start_PAN & 0xFF00)>>8);
	Navcomp_send_buff[37] = (uint8_t)(Start_PAN & 0x00FF);
	Navcomp_send_buff[36] = Scan_Status;
	OSSimpleTaskCreate(NAVcompData,MAIN_PRIO-1);
	OSSimpleTaskCreate(RadioData,MAIN_PRIO-2);
	//OSSimpleTaskCreate(MIScompData,MAIN_PRIO+2);
	//enableWatchDog( 1, 0x001F );//0x001C
	//Creating Data Receiving task from the computer

	while(1){

		//printf("Hi\n");
		TotalTime=timer1->readTime();

		//First if statement to command host radio to get ranging data from 101 guest with antenna A
		if(FiveHzflag==1 && Radiocount3==0){
			//printf("In WHile Loop\n");
			CRME=(uint16_t)radio_in_buff1[28]*256+(uint16_t)radio_in_buff1[29];
			if((unsigned char)radio_in_buff1[12]==0 && CRME<60){
				F_range_buff[0]=radio_in_buff1[24];
				F_range_buff[1]=radio_in_buff1[25];
				F_range_buff[2]=radio_in_buff1[26];
				F_range_buff[3]=radio_in_buff1[27];
				F_range_buff[4]=radio_in_buff1[32];
				F_range_buff[5]=radio_in_buff1[33];
				F_range_buff[6]=radio_in_buff1[12];
				Ant_config=radio_in_buff1[11];
			}
			Radiocount3=1;
			FiveHzflag=0;
		}//first if bracket

		//second if statement to command host radio to get ranging data from 102 guest with antenna A
		if(FiveHzflag==1 && Radiocount3==1){

			CRME=(uint16_t)radio_in_buff2[28]*256+(uint16_t)radio_in_buff2[29];
			if((unsigned char)radio_in_buff2[12]==0 && CRME<60){
				F_range_buff[0]=radio_in_buff2[24];
				F_range_buff[1]=radio_in_buff2[25];
				F_range_buff[2]=radio_in_buff2[26];
				F_range_buff[3]=radio_in_buff2[27];
				F_range_buff[4]=radio_in_buff2[32];
				F_range_buff[5]=radio_in_buff2[33];
				Ant_config=radio_in_buff2[11];
			}
			Radiocount3=0;
			FiveHzflag=0;
		}//second if bracket

		if(FiftyHzflag==1){
			if(Scan_Complete==1){
			//printf("Start Pan=%d,Start Pan=%d\n",Start_PAN,(int16_t)Navcomp_send_buff[38]*256+(int16_t)Navcomp_send_buff[37]);
				laser_range=ReadLaser(fdLaser);
				Navcomp_send_buff[36] = Scan_Status;
				Navcomp_send_buff[38] = (uint8_t)((Start_PAN & 0xFF00)>>8);
				Navcomp_send_buff[37] = (uint8_t)(Start_PAN & 0x00FF);
				//printf("laser range=%g\n",laser_range);
				//printf("laser range=%g\n",laser_range);
				//uint32_t Range=(uint32_t)F_range_buff[0]*16777216+(uint32_t)F_range_buff[1]*65536+(uint32_t)F_range_buff[2]*256+(uint32_t)F_range_buff[3];

				//printf("%zu,%u,%u,%u\n",Range,Ant_config,(unsigned char)radio_in_buff[12],(uint16_t)radio_in_buff[32]*256+(uint16_t)radio_in_buff[33]);
				StartAD();
				while (!ADDone()){}
				asm("nop");
				for (int i = 0; i < 8; i++)
					ADC_channel[i] = (unsigned short int)(1000 * (((double)GetADResult(i)) * 3.3 / (32768.0)));

				//printf("%d \n", ADC_channel[1]);

				sprintf(time_ms,"%lf",TotalTime);
				//send data to the computer
				SendtoNAVCOMP(Navcomp_send_buff,ADC_channel,time_ms,netburner_counter,laser_range,F_range_buff,PanAngle,fdNAVcomp,fdDebug,sizeof(Navcomp_send_buff),Ant_config);
				netburner_counter ++;


				//printf("%g\n",dYaw);
				//dYaw=0;
				StartAD();
				 while (!ADDone()){}
				asm("nop");

				//dYaw=93;

				uint16_t ServoPot = GetADResult(0);
				////Servo PAN 1 numbers
				Pulse=12287-dYaw*20.51;
				//printf("%d\n",ServoPot);

				if(Pulse<8594 || Pulse==8594)
					sim1.mcpwm.sm[1].val[5]=8594;
				if(Pulse>15980 || Pulse==15980)
					sim1.mcpwm.sm[1].val[5]=15980;
				else
					sim1.mcpwm.sm[1].val[5]=Pulse;//PAN control

				/*////Servo PAN 2 numbers
				Pulse=12287-dYaw*20.14;

				if(Pulse<8594 || Pulse==8310)
					sim1.mcpwm.sm[1].val[5]=8310;
				if(Pulse>15980 || Pulse==15560)
					sim1.mcpwm.sm[1].val[5]=15560;
				else
					sim1.mcpwm.sm[1].val[5]=Pulse;//PAN control*/

				double cYaw=(ServoPot-12885)/63;//PAN 1
				//double cYaw=(ServoPot-14667)/63.05;//PAN 2

				//Calibration PAN servo 1
				//0-8594
				//90-10440
				//180-12287 Position in which PAN faces front
				//270-14844
				//360-15980

				//Calibration pot PAN servo 1
				//360=1564
				//180=12885 //position in which PAN faces front
				//270=7270
				//90=18560
				//0=24290

				//Calibration PAN servo 2
				//0-8310
				//90-10440
				//180-12287 Position in which PAN faces front
				//270-13900
				//360-15560

				//Calibration pot PAN servo 2
				//360=3264
				//180=14667 //position in which PAN faces front
				//270=8999
				//90=20085
				//0=25971

				pwmr_comp=sim1.mcpwm.mcr;
				sim1.mcpwm.mcr |=LDOK;

				PanAngle = cYaw * 10;
				//printf("%g\n",cYaw);


			}
				FiftyHzflag=0;

			//serviceWatchDog();//
		}//FiftyHzflag bracket

	}//Main While loop Bracket
void UserMain( void* pd ){
	/////Usual Routine

	InitializeStack();
	OSChangePrio( MAIN_PRIO );
	EnableAutoUpdate();
	EnableTaskMonitor();
	EnableSmartTraps();

	initPINS();
	initDSPI();

	//Local Variables
	/***********Defining Interrupt Timers*****************/
	HiResTimer* timer2=0;//50 Hz Interrupt Timer
	int32_t IMU_data[6]={0};
	char Navcomp_send_buff[64]={0},time_ms[2]={0},G[20]={0};
	uint16_t NB_counter=0,sum=0;
	double TotalTime=0;
	int fdDebug=0;
	uint8_t i=0;

	BYTE IMU_command[24]={xahigh,0,xalow,0,yahigh,0,yalow,0,zahigh,0,zalow,0,xghigh,0,xglow,0,yghigh,0,yglow,0,zghigh,0,zglow,0};
	BYTE IMU3_raw[24]={0};//IMU 3
	BYTE IMU4_raw[24]={0};//IMU 4

	Navcomp_send_buff[0] = 0x41;
	Navcomp_send_buff[1] = 0x7A;
	Navcomp_send_buff[2] = 0x04;

	SerialClose(0);
	SerialClose(2);
	SerialClose(7);
	SerialClose(9);

	fdNavcomp=OpenSerial(7,15200,1,8,eParityNone);
	fdcrank=OpenSerial(9,15200,1,8,eParityNone);
	fdgrabber=OpenSerial(2,15200,1,8,eParityNone);
	fdDebug=OpenSerial(0,15200,1,8,eParityNone);

	OSSimpleTaskCreate(NAVcompData,MAIN_PRIO-1);

	sprintf(G,"~ECHOF 1\r");
	i=0;
	for (i=0;i<sizeof(G);i++){
			write(fdcrank,&G[i],1);
			write(fdgrabber,&G[i],1);
	}

	initTIMERS(timer2);
	while(1){
		if (FiftyHzTaskFlag==1){
			DSPIStart(1,IMU_command,IMU3_raw,24,NULL);//IMU3
			while(!DSPIdone(1)){/*iprintf("DSPI1done state=%s\n",(DSPIdone(1))?"true":"false");*/};

			//iprintf("....................................................................\n");

			DSPIStart(3,IMU_command,IMU4_raw,24,NULL);//IMU3
			while(!DSPIdone(3)){/*iprintf("DSPI3done state=%s\n",(DSPIdone(3))?"true":"false");*/};

			//packaging NB counter
			Navcomp_send_buff[4]=(uint8_t)((NB_counter & 0xFF00)>>8);
			Navcomp_send_buff[3]=(uint8_t)(NB_counter & 0x00FF);
			NB_counter++;

			//getting time in ms
			TotalTime=timer2->readTime();
			sprintf(time_ms,"%lf",TotalTime);
			Navcomp_send_buff[6]=time_ms[0];
			Navcomp_send_buff[5]=time_ms[1];

			IMU_data[0]=(((int32_t)IMU3_raw[2]<<24|(int32_t)IMU3_raw[3]<<16|(int32_t)IMU3_raw[4]<<8|(int32_t)IMU3_raw[5])-((int32_t)IMU4_raw[2]<<24|(int32_t)IMU4_raw[3]<<16|(int32_t)IMU4_raw[4]<<8|(int32_t)IMU4_raw[5]))/2;//X-Accel
			IMU_data[1]=(((int32_t)IMU3_raw[6]<<24|(int32_t)IMU3_raw[7]<<16|(int32_t)IMU3_raw[8]<<8|(int32_t)IMU3_raw[9])-((int32_t)IMU4_raw[6]<<24|(int32_t)IMU4_raw[7]<<16|(int32_t)IMU4_raw[8]<<8|(int32_t)IMU4_raw[9]))/2;//Y-Accel
			IMU_data[2]=(((int32_t)IMU3_raw[10]<<24|(int32_t)IMU3_raw[11]<<16|(int32_t)IMU3_raw[12]<<8|(int32_t)IMU3_raw[13])+((int32_t)IMU4_raw[10]<<24|(int32_t)IMU4_raw[11]<<16|(int32_t)IMU4_raw[12]<<8|(int32_t)IMU4_raw[13]))/2;//Z-Accel
			IMU_data[3]=(((int32_t)IMU3_raw[14]<<24|(int32_t)IMU3_raw[15]<<16|(int32_t)IMU3_raw[16]<<8|(int32_t)IMU3_raw[17])-((int32_t)IMU4_raw[14]<<24|(int32_t)IMU4_raw[15]<<16|(int32_t)IMU4_raw[16]<<8|(int32_t)IMU4_raw[17]))/2;//X-Gyro
			IMU_data[4]=(((int32_t)IMU3_raw[18]<<24|(int32_t)IMU3_raw[19]<<16|(int32_t)IMU3_raw[20]<<8|(int32_t)IMU3_raw[21])-((int32_t)IMU4_raw[18]<<24|(int32_t)IMU4_raw[19]<<16|(int32_t)IMU4_raw[20]<<8|(int32_t)IMU4_raw[21]))/2;//Y-Gyro
			IMU_data[5]=(((int32_t)IMU3_raw[22]<<24|(int32_t)IMU3_raw[23]<<16|(int32_t)IMU3_raw[0]<<8|(int32_t)IMU3_raw[1])+((int32_t)IMU4_raw[22]<<24|(int32_t)IMU4_raw[23]<<16|(int32_t)IMU4_raw[0]<<8|(int32_t)IMU4_raw[1]))/2;//Z-Gyro

			i=0;
			for(i=0;i<6;i++){
				Navcomp_send_buff[4*i+10]=(BYTE)((uint32_t)(IMU_data[i] & 0xFF000000)>>24);
				Navcomp_send_buff[4*i+9]=(BYTE)((uint32_t)(IMU_data[i] & 0x00FF0000)>>16);
				Navcomp_send_buff[4*i+8]=(BYTE)((uint32_t)(IMU_data[i] & 0x0000FF00)>>8);
				Navcomp_send_buff[4*i+7]=(BYTE)((uint32_t)(IMU_data[i] & 0x000000FF));

			}

			Navcomp_send_buff[47]=statestatus;
			//Calculating checksum
			sum=0;
			i=0;
			for(i=3;i<63;i++){
				sum +=Navcomp_send_buff[i];
			}

			Navcomp_send_buff[63]=(uint8_t)(sum % 256);

			for(uint8_t j=0;j<sizeof(Navcomp_send_buff);j++){
				write(fdNavcomp,&Navcomp_send_buff[j],1);
			}

			sprintf(G,"!VAR 1 %d\r",commandstatus);
			i=0;
			for (i=0;i<sizeof(G);i++){
					write(fdcrank,&G[i],1);
					write(fdgrabber,&G[i],1);
			}


			//printf("Value:%f MSB:%d LSB:%d\n",0.00025*((int16_t)((uint16_t)send_buff[18]*256 + (uint16_t)send_buff[17])),(uint8_t)send_buff[18],(uint8_t) send_buff[17]);

			FiftyHzTaskFlag=0;
		}//FiftyHz if

	}//Main While loop