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 }
/*****************************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 }
/*------------------------------------------------------------------- * UserMain *-----------------------------------------------------------------*/ void UserMain(void *pd) { SimpleUart(0,115200); assign_stdio(0); iprintf("Before init stack\r\n"); InitializeStack(); { WORD ncounts = 0; while ( ( !bEtherLink ) && ( ncounts < 2*TICKS_PER_SECOND ) ) { ncounts++; OSTimeDly( 1 ); } } EnableAutoUpdate(); EnableTaskMonitor(); OSChangePrio( MAIN_PRIO ); // set standard UserMain task priority //BcastSysLogPrintf("Application built on %s on %s\r\nWait...", __TIME__, __DATE__ ); pUserUdpProcessFunction=MyUDPProcessFunction; // UdpCheck(); OSSemInit(&DataSem,0); LoadSensorConfig(); // BcastSysLogPrintf("1"); InitLog(); ImuInit(IMU_PRIO,&DataSem); InitIDSM2SubSystem(RC_PRIO,&DataSem); InitGpsSubSystem(GPS_PRIO,&DataSem); InitServos(); SetServo(0,0); SetServo(1,0); SetServo(2,0); SetServo(3,0); WORD LastGps=GPS_Result.ReadingNum; WORD LastRc =DSM2_Result.ReadingNum; WORD LastImu =IMU_Result.ReadingNum; DWORD LSecs=Secs; static short mmx; static short mmy; static short nmx; static short nmy; static short mgz; static short ngz; DWORD ZeroTime=Secs+5; long GZSum=0; int GZCnt=0; float fmh=0.0; while(ZeroTime > Secs) { OSSemPend(&DataSem,20); if(LastImu !=IMU_Result.ReadingNum) { GZSum+=IMU_Result.gz; GZCnt++; LastImu =IMU_Result.ReadingNum; if (IMU_Result.mx !=0 ) fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my); } } short gzoff=(GZSum/GZCnt); float fIheading=fmh; while(1) { OSSemPend(&DataSem,20); if (LastGps!=GPS_Result.ReadingNum ) { LastGps=GPS_Result.ReadingNum; LogSmGps(GPS_Result); } if (LastRc !=DSM2_Result.ReadingNum) { WORD w=DSM2_Result.val[1]; float f=DSM_Con(w); SetServo(STEER_CH,f); w=DSM2_Result.val[0]; f=DSM_Con(w); SetServo(THROTTLE_CH,f); LastRc =DSM2_Result.ReadingNum; // LogRC(DSM2_Result); } if(LSecs!=Secs) { // BcastSysLogPrintf("Tick %d Iat:%ld lon:%ld SAT:%d\r\n",Secs,GPS_Result.LAT,GPS_Result.LON,GPS_Result.numSV); //SysLogPrintf(ipa_syslog_addr,514, LogMaxMin(mgz,mmx,mmy,ngz,nmx,nmy); //static char tbuf[256]; // siprintf(tbuf,"TCN=%ld\r\n",sim.timer[0].tcn); // writestring(LOG_UART,tbuf); mgz=IMU_Result.mz; mmx=IMU_Result.mx; mmy=IMU_Result.my; ngz=IMU_Result.mz; nmx=IMU_Result.mx; nmy=IMU_Result.my; LSecs=Secs; } if(LastImu !=IMU_Result.ReadingNum) { if(IMU_Result.gz<ngz) ngz=IMU_Result.gz; if(IMU_Result.gz>mgz) mgz=IMU_Result.gz; fIheading+=(fGZHeadingScale_deg*(IMU_Result.gz-gzoff)); if(fIheading > 180.0 ) fIheading-=360.0; if(fIheading <-180.0 ) fIheading+=360.0; if (IMU_Result.mx !=0 ) { if(IMU_Result.mx<nmx) nmx=IMU_Result.mx; if(IMU_Result.mx>mmx) mmx=IMU_Result.mx; if(IMU_Result.my<nmy) nmy=IMU_Result.my; if(IMU_Result.my>mmy) mmy=IMU_Result.my; volatile ImuRegisters il; OSLock(); il.ax=IMU_Result.ax; il.ay=IMU_Result.ay; il.az=IMU_Result.az; il.gx=IMU_Result.gx; il.gy=IMU_Result.gy; il.gz=IMU_Result.gz; il.mx=IMU_Result.mx; il.my=IMU_Result.my; il.mz=IMU_Result.mz; il.t =IMU_Result.t ; il.ReadingNum=IMU_Result.ReadingNum; OSUnlock(); fmh=CalcMagHeading(IMU_Result.mx,IMU_Result.my); CorrectHeading(fIheading,fmh,0.005);//20 times per second correct in 10 seconds so 0.005 il.fIhead=fIheading; il.fMhead=fmh; il.GHeading= GPS_Result.Heading; il.odo=sim.timer[0].tcn; LogImu(il); } LastImu =IMU_Result.ReadingNum; } }//While }
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