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)
{
InitializeStack();/* Setup the TCP/IP stack buffers etc.. */
GetDHCPAddressIfNecessary();/*Get a DHCP address if needed*/
/*You may want to add a check for the return value from this function*/
/*See the function definition in \nburn\include\dhcpclient.h*/
OSChangePrio(MAIN_PRIO);/* Change our priority from highest to something in the middle */
EnableAutoUpdate();/* Enable the ability to update code over the network */
iprintf("Application started\n");
// Setup DSPI3
Pins[36].function(PIN_36_DSPI3_PCS0);
Pins[38].function(PIN_38_DSPI3_SCK);
Pins[32].function(PIN_32_DSPI3_SIN);
Pins[34].function(PIN_34_DSPI3_SOUT);
Pins[49].function(PIN_49_GPIO);
PoolPtr pp;
volatile dspistruct &spi = sim1.dspi3;
int spiCh = DMA_CH_DSPI_3_RX;
int RemoteFD;
Pins[49] = 0;
getchar();
while ((RemoteFD = connect( TARGET_ADDR, 0, 20035, TICKS_PER_SECOND*5)) < 0) {
iprintf("Error: %d\n", RemoteFD);
}
iprintf("Connected: %d\n", RemoteFD);
ConfigRX(spi, spiCh);
iprintf("Running\n");
// OSSimpleTaskCreate(MonTask, MAIN_PRIO+1);
while (1) {
pp = (PoolPtr)OSFifoPend( &dspiRXFifo, 0 );
// iprintf("pp: %p\n", pp);
// iprintf("sr: %08lX\n", spi.sr);
// ShowData( pp->pData, pp->usedsize);
writeall( RemoteFD, (char *)pp->pData, pp->usedsize );
Pins[49] = 0;
DestBuffers.InsertTail(pp);
// putchar('.');
// FreeFrameBuf( pp );
}
}
void UserMain(void * pd) {
// Stock initialization code
InitializeStack();
if (EthernetIP == 0)
GetDHCPAddress();
OSChangePrio(MAIN_PRIO);
EnableAutoUpdate();
Pins[16].function(PIN_16_GPIO); // Set module Pin 16 to GPIO
while (1) { // Main Loop
Pins[16] = 1; // Toggle Pin 16 HIGH
OSTimeDly(TICKS_PER_SECOND); // Wait 1 second
Pins[16] = 0; // Toggle Pin 16 LOW
OSTimeDly(TICKS_PER_SECOND); // Wait 1 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
}
/*-------------------------------------------------------------------
* 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
}
//=====================================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
void UserMain(void * pd) {
int x;
DWORD flag = 0;
int cnt;
InitializeStack();
GetDHCPAddressIfNecessary();
if (EthernetIP == 0) GetDHCPAddress();
OSChangePrio(MAIN_PRIO);
EnableAutoUpdate();
#ifdef _DEBUG
/* InitializeNetworkGDB_and_Wait(); */
InitializeNetworkGDB();
#endif
/* note, this may not work in all cases.
1) all systems might boot at the same time
2) when they get to this point, they may not have an ip yet
*/
/* init our acn stack */
acn_port_protect_startup();
#if CONFIG_NSK
srand(GetPreciseTime() + netx_getmyip(0));
netx_init();
netx_startup();
#endif
#if CONFIG_SLP
slp_init();
slp_open();
slp_active_discovery_start();
#endif
#if CONFIG_RLP
rlp_init();
#if CONFIG_SDT
sdt_init(); /* indirectly calls sdtm_init(), rlp_init(), rlpm_init() */
sdt_startup(true);
#endif
#if CONFIG_DMP
/* dmp_startup(); */
#endif
#endif /* RLP */
CreateTasks();
x = OSChangePrio( DEBUG_TERM_PRIORITY );
if (x) {
PRINTF("%s","OUCH\n");
}
PRINTF("%s","Hello ACN World\n");
ioctl( 0, IOCTL_CLR | IOCTL_RX_ECHO ); /* turn sdtin echo off */
process_keys();
/* shut things down - these require the threads to continue run... */
#if CONFIG_SDT
sdt_shutdown();
#endif
#if CONFIG_SLP
slp_close();
#endif
#if CONFIG_DMP
/* dmp_shutdown() */
#endif
/* shut down receive thread */
OSFlagClear(&recv_flag, 1);
PRINTF("%s","Waiting for task to recv to shut down..");
cnt = 0;
flag = 0;
while (!flag) {
cnt++;
if (cnt == 80) {
cnt = 0;
PRINTF("%s","\n");
}
PRINTF("%s",".");
flag = OSFlagState(&recv_flag);
}
/* shut down tick thread */
OSFlagClear(&tick_flag, 1);
PRINTF("%s","Waiting for task to tick to shut down..");
cnt = 0;
flag = 0;
while (!flag) {
cnt++;
if (cnt == 80) {
cnt = 0;
PRINTF("%s","\n");
}
PRINTF("%s",".");
flag = OSFlagState(&tick_flag);
}
#if CONFIG_NSK
netx_shutdown();
#endif
acn_port_protect_shutdown();
slp_stats();
sdt_stats();
PRINTF("%s","========================\n");
PRINTF("%s","\nDone....\n");
while (1) {
OSTimeDly(20);
}
}
