int main( int argc,char *argv[])
{
int ret = -1;
while (ret < 0 ){
ret = poweron_self_check();
}
while (get_flying_status() < AIRCRAFT_READY) {
usleep(500000);
flying_status_return();
}
while (get_flying_status() < AIRCRAFT_TAKEOFF) {
flying_status_return();
usleep(40000);
}
auto_flying_start();
while (1) {
usleep(20000);
flying_status_return();
}
sensor_close();
adc_close();
spi_close();
exit(0);
return 0;
}
void flying_status_return()
{
uint8 *fa = (uint8*)(get_flying_attitude());
uint8 buf[145];
uint16 crc_value;
buf[0] = CTRL_FRAME_START1;
buf[1] = CTRL_FRAME_START2;
buf[2] = get_aircraft_no()&0xFF;
buf[3] = get_aircraft_no()>>8;
*(uint32*)(buf+4) = 0x91;
buf[8] = 1;
buf[9] = 1;
buf[10] = CTRL_FRAME_TYPE_FLY_STATUS;
// compiler add 4bytes between float data and double data for flying_attitude struct
// so we have to copy separately
memcpy(buf+11,fa,60);
memcpy(buf+71,fa+64,36);
*(uint32*)(buf+107) = get_sonar_data();
buf[111] = 0;
buf[112] = 0; // next waypoint
memcpy(buf+113,(uint8 *)(&ppwm),20);//pwm output
buf[133] = get_flying_status()&0xFF;
buf[134] = get_flying_status()>>8;
buf[135] = 0; // gps status
buf[136] = 0; // imu status
buf[137] = 0; // AP status :cpu1 or cpu2
buf[138] = get_input_voltage()&0xFF;
buf[139] = 0;
buf[140] = get_cpu_temperature()/1000;
buf[141] = 0;
crc_value=crc_checksum16(buf, 142);
buf[142] = crc_value&0xFF;
buf[143] = crc_value>>8;
buf[144] = CTRL_FRAME_END;
control_cmd_send(buf, 145);
}
void flying_status_return(int transmit_data)
{
flying_attitude_s *fa = get_flying_attitude();
uint8 buf[256];
uint16 crc_value;
int pressure;
buf[0] = CTRL_FRAME_START1;
buf[1] = CTRL_FRAME_START2;
buf[2] = get_aircraft_no()&0xFF;
buf[3] = get_aircraft_no()>>8;
*(uint16*)(buf+4) = 0x91;
buf[6] = 1;
buf[7] = 0;
buf[8] = 1;
buf[9] = 0;
buf[10] = CTRL_FRAME_TYPE_FLY_STATUS;
*((float *)(buf+11))=fa->roll;
*((float *)(buf+15))=fa->pitch;
*((float *)(buf+19))=fa->yaw;
*((float *)(buf+23))=fa->gx;
*((float *)(buf+27))=fa->gy;
*((float *)(buf+31))=fa->gz;
*((float *)(buf+35)) =fa->ax;
*((float *)(buf+39))=fa->ay;
*((float *)(buf+43))=fa->az;
*((uint32 *)(buf+47))=fa->g_time;
*((int *)(buf+51))=fa->vn;
*((int *)(buf+55))=fa->ve;
*((int *)(buf+59))=fa->vd;
*(( int *)(buf+63))=fa->heading;
*(( int *)(buf+67))=fa->b_h;
*((double *)(buf+71))=fa->lat;
*((double *)(buf+79))=fa->Long;
*((double *)(buf+87))=fa->g_h;
*((float *)(buf+95))=fa->vx;
*((float *)(buf+99))=fa->vy;
*((float *)(buf+103))=fa->vz;
sonar_data=get_sonar_data();
*(uint32*)(buf+107) = sonar_data;
buf[111] = gepoint.id & 0xFF ;
buf[112] = gepoint.id >> 8 ;// next waypoint
read_rc_data(rc_data);
if(get_flying_status() == AIRCRAFT_MANUAL_MODE){
memcpy(buf+113,rc_data,14);
}else
memcpy(buf+113,(uint8 *)(&ppwm),20);//pwm output
*(uint16*)(buf+131) = ppwm.c[9];
buf[133] = get_flying_status()&0xFF;
buf[134] = get_flying_status()>>8;
buf[135] = 0; // gps status
buf[136] = 0; // imu status
buf[137] = 0; // AP status :cpu1 or cpu2
working_status.input_voltage = get_input_voltage();
working_status.engine_voltage = get_monitor_voltage();
working_status.cpu_temprature = get_cpu_temperature();
buf[138] = working_status.input_voltage&0xFF;//AP power
buf[139] = working_status.engine_voltage&0xFF;//UAV power
buf[140] = working_status.cpu_temprature/1000;
buf[141] = 0;
if(transmit_data){
crc_value=crc_checksum16(buf, 142);
buf[142] = crc_value&0xFF;
buf[143] = crc_value>>8;
buf[144] = CTRL_FRAME_END;
control_cmd_send(buf, 145);
}
*(uint16*)(buf+4) = 0xAF;
memcpy(buf+142,rc_data,14);
pressure = get_altimeter();
*(uint16*)(buf+154)= pressure>>16;
*(uint16*)(buf+156)= pressure&0xffff;
*(uint16*)(buf+158)=time_estimation.data_return;
*(uint16*)(buf+160)=time_estimation.algorithm;
crc_value=crc_checksum16(buf, 172);
buf[172] = crc_value&0xFF;
buf[173] = crc_value>>8;
buf[174] = CTRL_FRAME_END;
fwrite(buf,175,1,fp_fly_status);
}