void mainloop(void)
{
static unsigned char led_cnt = 0, led_state = 1;
led_cnt++;
if ((GPS_Data.status & 0xFF) == 0x03)
{
LED(0, OFF);
}
else
{
if (led_cnt == 150)
{
LED(0, ON);
}
else if (led_cnt == 200)
{
led_cnt = 0;
LED(0, OFF);
}
}
SDK_mainloop();
HL2LL_write_cycle(); //write data to transmit buffer for immediate transfer to LL processor
if (GPS_init_status == GPS_NEEDS_CONFIGURATION) //configuration SM of GPS at startup
{
GPS_configure();
}
if (gpsDataOkTrigger)
{
if (GPS_Data.horizontal_accuracy > 12000)
GPS_Data.status &= ~0x03;
if (GPS_timeout > 50)//(GPS_Data.status&0xFF)!=0x03)
{
if (led_state)
{
led_state = 0;
LED(1, OFF);
}
else
{
LED(1, ON);
led_state = 1;
}
}
GPS_timeout = 0;
if (GPS_init_status == GPS_STARTUP)
GPS_init_status = GPS_IS_CONFIGURED; //gps config valid, if received correct packet
HL_Status.latitude = GPS_Data.latitude;
HL_Status.longitude = GPS_Data.longitude;
}
}
void mainloop(void) //mainloop is triggered at 1 kHz
{
static unsigned char led_cnt=0, led_state=1;
static int Firefly_led_fin_cnt=0;
unsigned char t;
//blink red led if no GPS lock available
led_cnt++;
if((GPS_Data.status&0xFF)==0x03)
{
LED(0,OFF);
}
else
{
if(led_cnt==150)
{
LED(0,ON);
}
else if(led_cnt==200)
{
led_cnt=0;
LED(0,OFF);
}
}
//after first lock, determine magnetic inclination and declination
if (SYSTEM_initialized)
{
if ((!declinationAvailable) && (GPS_Data.horizontal_accuracy<10000) && ((GPS_Data.status&0x03)==0x03)) //make sure GPS lock is valid
{
int status;
estimatedDeclination=getDeclination(GPS_Data.latitude,GPS_Data.longitude,GPS_Data.height/1000,2012,&status);
if (estimatedDeclination<-32000) estimatedDeclination=-32000;
if (estimatedDeclination>32000) estimatedDeclination=32000;
declinationAvailable=1;
}
}
//toggle green LED and update SDK input struct when GPS data packet is received
if (gpsLEDTrigger)
{
if(led_state)
{
led_state=0;
LED(1,OFF);
}
else
{
LED(1,ON);
led_state=1;
}
RO_ALL_Data.GPS_height=GPS_Data.height;
RO_ALL_Data.GPS_latitude=GPS_Data.latitude;
RO_ALL_Data.GPS_longitude=GPS_Data.longitude;
RO_ALL_Data.GPS_speed_x=GPS_Data.speed_x;
RO_ALL_Data.GPS_speed_y=GPS_Data.speed_y;
RO_ALL_Data.GPS_status=GPS_Data.status;
RO_ALL_Data.GPS_sat_num=GPS_Data.numSV;
RO_ALL_Data.GPS_week=GPS_Time.week;
RO_ALL_Data.GPS_time_of_week=GPS_Time.time_of_week;
RO_ALL_Data.GPS_heading=GPS_Data.heading;
RO_ALL_Data.GPS_position_accuracy=GPS_Data.horizontal_accuracy;
RO_ALL_Data.GPS_speed_accuracy=GPS_Data.speed_accuracy;
RO_ALL_Data.GPS_height_accuracy=GPS_Data.vertical_accuracy;
gpsLEDTrigger=0;
}
//re-trigger UART-transmission if it was paused by modem CTS pin
if(trigger_transmission)
{
if(!(IOPIN0&(1<<CTS_RADIO)))
{
trigger_transmission=0;
if(ringbuffer(RBREAD, &t, 1))
{
transmission_running=1;
UARTWriteChar(t);
}
}
}
#ifdef MATLAB
//re-trigger UART-transmission if it was paused by modem CTS pin
if(trigger_transmission)
{
if(!(IOPIN0&(1<<CTS_RADIO)))
{
trigger_transmission=0;
if(UART_Matlab_fifo(RBREAD, &t, 1))
{
transmission_running=1;
UARTWriteChar(t);
}
}
}
#endif
//send data packet as an example how to use HL_serial_0 (please refer to uart.c for details)
/*
if(uart_cnt++==ControllerCyclesPerSecond/DataOutputsPerSecond)
{
uart_cnt=0;
if((sizeof(RO_ALL_Data))<ringbuffer(RBFREE, 0, 0))
{
UART_SendPacket(&RO_ALL_Data, sizeof(RO_ALL_Data), PD_RO_ALL_DATA);
}
}
*/
//handle gps data reception
uBloxReceiveEngine();
//run SDK mainloop. Please put all your data handling / controller code in sdk.c
SDK_mainloop();
//write data to transmit buffer for immediate transfer to LL processor
HL2LL_write_cycle();
//control pan-tilt-unit ("cam option 4" @ AscTec Pelican and AscTec Firefly)
PTU_update();
//synchronize all variables, commands and parameters with ACI
aciSyncVar();
aciSyncCmd();
aciSyncPar();
//run ACI engine
aciEngine();
//send buildinfo
if ((SYSTEM_initialized) && (!transmitBuildInfoTrigger))
transmitBuildInfoTrigger=1;
//Firefly LED
if (SYSTEM_initialized&&fireflyLedEnabled)
{
if(++Firefly_led_fin_cnt==10)
{
Firefly_led_fin_cnt=0;
fireFlyLedHandler();
}
}
}