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