void UserMain( void * pd )
{
initWithWeb();
Nunchuck_Init(); // FIXME
LLInit(1);
TheWSHandler = MyDoWSUpgrade;
stepper.Init();
ConfigureStepper( &stepper, 1 );
stepSpeed = 100;
stepDir = 1;
StartStepper( 1, stepDir, stepSpeed );
stepForever = true;
OSSemInit( &SockReadySem, 0 );
OSCritInit( &I2CLock );
iprintf("Application: %s\r\nNNDK Revision: %s\r\n",AppName,GetReleaseTag());
OSTimeDly(2);
OSSimpleTaskCreate(ReportTask, 5);
OSSimpleTaskCreate(InputTask, MAIN_PRIO-1);
OSSimpleTaskCreate(RangingTask, 2);
EnableEncoder(EncodeStep, ButtonChange, ButtonDoubleClick );
ClearTaskTimes();
SMPoolPtr pp;
while(1) {
fd_set error_fds;
FD_ZERO( &error_fds );
OSCritEnter( &I2CLock, 0 );
nun.Update();
OSCritLeave( &I2CLock );
serv_x.SetPos(nun.stick.x);
serv_y.SetPos(nun.stick.y);
// iprintf("%lu\na_x: %4.4d - a_y: %4.4d - a_z: %4.4d\ns_x: %3.3d - s_y: %3.3d - b_z: %d - b_c: %d\n\n",
// TimeTick, nun.accel.x, nun.accel.y, nun.accel.z,
// nun.stick.x, nun.stick.y,
// nun.button.z, nun.button.c);
// iprintf("range: %u\n", lidarRange);
if (ws_fd > 0){
SendConfigReport(ws_fd);
}
}
}
void UserMain(void * pd)
{
// Initialize network stack and standard system utilities
init();
iprintf("Application started\n");
OSSemInit( &playbackSem, 0 );
WavPlayer::wavError ret;
ret = testPlayer.SetChannelDAC( 0, 1 );
ret = testPlayer.SetChannelDAC( 1, 0 );
ret = testPlayer.SetTimer( 3 );
while (1)
{
DisplayMenu();
char c = getchar();
iprintf("\r\n");
ProcessCommand( c );
}
}
void IMU_Task(void * p)
{
OS_SEM MySem;
OSSemInit(&MySem,0);
PiterSem(&MySem, 200);
while(1)
{
OSSemPend(&MySem,0);
if ((Local_IMU_Result.ReadingNum %10)==0)
ImuRead(Local_IMU_Result,true);
else
ImuRead(Local_IMU_Result,false);
Local_IMU_Result.ReadingNum++;
OSLock();
memcpy((void*)&IMU_Result,&Local_IMU_Result,sizeof(IMU_Result));
OSUnlock();
if(LocalSem) OSSemPost(LocalSem);
}
}
ADC::ADC(int dspiChannelInput) {
int SPIPins[12] = { 33, 35, 15, 31, //SPI1
24, 26, 28, 30, //SPI2
32, 34, 36, 38}; //SPI3
Pins[SPIPins[(dspiChannelInput - 1) * 4 + 0]].function(1); //SPI Input
Pins[SPIPins[(dspiChannelInput - 1) * 4 + 1]].function(1); //SPI Out
Pins[SPIPins[(dspiChannelInput - 1) * 4 + 2]].function(1); //SPI chip select 0
Pins[SPIPins[(dspiChannelInput - 1) * 4 + 3]].function(1); //SPI clock
dspiChannel = dspiChannelInput;
/*
for (int i = 0; i < size; i++)
table[i] = 0;
*/
OSSemInit(&SPISEM, 0);
DSPIInit(dspiChannel, ADCdspiBaudRate, ADCtransferSizeBits, ADCChipSelects,
ADCCSPolarityIdle, ADCClockPolarity, 1, 1, 100, 0, false);
}
/*-------------------------------------------------------------------
* 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
}
