void *aciThread(void) { int result = 0; unsigned char data = 0; while(1) { result = read(fd, &data, 1); while (result > 0) { aciReceiveHandler(data); result = read(fd, &data, 1); } if(var_getted) { aciSynchronizeVars(); state_msg.timestamp = utime_now(); state_msg.position[0] = 0; state_msg.position[1] = 0; state_msg.position[2] = 0; state_msg.velocity[0] = 0; state_msg.velocity[1] = 0; state_msg.velocity[2] = 0; state_msg.accel[0] = acc_x; state_msg.accel[1] = acc_y; state_msg.accel[2] = acc_z; state_msg.angle[0] = angle_roll; state_msg.angle[1] = angle_pitch; state_msg.angle[2] = angle_yaw; state_msg.angular_vel[0] = roll_vel; state_msg.angular_vel[1] = pitch_vel; state_msg.angular_vel[2] = yaw_vel; state_msg.angular_accel[0] = 0; state_msg.angular_accel[1] = 0; state_msg.angular_accel[2] = 0; trans_msg.timestamp = utime_now(); trans_msg.ch0 = ch0; trans_msg.ch1 = ch1; trans_msg.ch2 = ch2; trans_msg.ch3 = ch3; trans_msg.ch4 = ch4; trans_msg.ch5 = ch5; trans_msg.ch6 = ch6; trans_msg.ch7 = ch7; state_t_publish(lcm, "state", &state_msg); transmitter_t_publish(lcm, "transmitter", &trans_msg); } aciEngine(); usleep(10000); } return NULL; }
void *aciThread(void) { int result = 0; unsigned char data = 0; while(1) { result = read(fd, &data, 1); while (result > 0) { aciReceiveHandler(data); result = read(fd, &data, 1); } aciEngine(); usleep(10000); } return NULL; }
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(); } } }