static inline void __uart0_putchar(uint8_t ch){ if(ch == '\n'){ UARTWriteChar('\r'); } else{ UARTWriteChar(ch); } }
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(); } } }