uint8 InitDevice()//TODO:异常处理
{
uint8 r=0,t=0;
//GPIO
PinInit();
IO0DIR=1<<29|1<<30;//P0.29,P0.30必须同为输出才能输出(SSP0的片选).
//GpioSpeedHigh();//全局高速模式,因为P0.29问题无法使用。
//指示系统启动
LED1ON();
//UART0-GCS
r+=UARTInit(FCUARTPORT,FCUARTBPS);//UART0-PC
UARTSendChar(FCUARTPORT,0x40);
UARTSendChar(FCUARTPORT,0x40);
//UART2-INS
r+=UARTInit(INSUARTPORT,INSUARTBPS);
//UARTSendChar(2,0x40);
//UARTSendChar(2,0x40);
//SSP0-FPGA
r+=SSP0FPGAMode();
//r+=SPIInit();
//while(1)
//{
t=FPGACheck();
//}
if(t==TRUE)
{
FCEventSend(OKFPGA);
}
else
{
FCEventSend(ErrFPGA);
}
r+=t;
//SSP1-FLASH
r+=FlashInit(FlashQueueSize);
//Timer0-MainLoop
r+=TimerInit(0,1000/MainLoopHz);
TimerDisable(0);
//TimerEnable(0);
//Timer1-Time+LED1
r+=TimerInit(1,1000);
TimerEnable(1);
//Timer2-Working-LED2
r+=TimerInit(2,100);
TimerDisable(2);
//启动中断
IRQEnable();
return r;
}
int main(void)
{
sbi(DDRC,6);
sbi(DDRC,7);
sbi(PORTD,6);
sei();
LED1ON();
while(1)
{
if(SWITCH1ON())
LED2ON();
else
LED2OFF();
_delay_ms(5);
}
}
int main (void)
{
//Test
//Test();
//Init
InitDevice();
//Send Startup Message
FCMessageSend("vFlyOK");
//等待INS的GPS完成启动
//WaitGPS();
//改变运行状态并发送状态信息
RunState=DefaultRunState;
FCEventSend(RunState);
//初始化参数
InitPara();
FCMessageSend("InitParaOK");
//启动主计时器
TimerEnable(0);
while(1)
{
if(Do==DoWorkLoop)
{
LED2ON();
INS();
SwitchMode();
if(FPGAState==TRUE)
{
GetPWM();
}
//FCA();
//FCALimitPWM(void);
FCAGetNav();
FCAPWMOut();
SmoothPWM(40);
SetPWM();
//CollectMedianDataFun(40);
FCACollectPosYaw(5);
FCACollectMedianDataFun(40);
FCFrameProcess();
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
MainLoopRunCounter++;
LED2OFF();
Do=DoNothing;
}
if(Do==DoFactoryLoop)
{
LED1ON();
GpioClrLOW(P1_25);//一律切换到手驾
INS();
FCFrameProcess();
if(DataToDownload==0)
{
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
/*
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
*/
}
else
{
DataDownload();
}
MainLoopRunCounter++;
LED1OFF();
Do=DoNothing;
}
}
return 0;
}