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
