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
}
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);
}
}
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);
}
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