static void waypoint_list_clear()
{
waypoint_list_s *wp = waypoint_list_head;
waypoint_list_s *p = wp;
waypoint_list_s *wp_list = NULL;
flying_attitude_s *fa = get_flying_attitude();
while(wp) {
p = wp;
wp = wp->next;
free(p);
}
waypoint_list_head = waypoint_list_current = waypoint_list_tail = NULL;
//set current positon as waypoint 0
wp_list = malloc(sizeof(waypoint_list_s));
if (wp_list == NULL) {
print_err("malloc failed\n");
return ;
}
wp_list->waypoint.id = 0;
wp_list->waypoint.task = 0;
wp_list->waypoint.task_para = 0;
wp_list->waypoint.v = 0;
wp_list->waypoint.lon = fa->Long;
wp_list->waypoint.lat = fa->lat;
wp_list->waypoint.h = fa->g_h;
waypoint_list_head = waypoint_list_tail = wp_list;
wp_list->prev = NULL;
wp_list->next = NULL;
}
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);
}
int poweron_self_check()
{
int ret = -1;
int timeout = 0;
char set_system_time[50];
flying_attitude_s *p;
//--------------spi initial------------------
ret=spi_open();
printf("CPLD logic version:%d\n",get_fpga_version());
printf("----------------------------------------------------------------\n");
#ifndef debug
if(ret<0)
fault_status_return(ret);
#endif
/*
ret=adc_init();
#ifndef debug
if(ret<0)
fault_status_return(ret);
#endif
*/
//---------------imu initial-----------------
ret = sensor_open();
#ifndef debug
if (ret < 0) {
fault_status_return(ret);
}
#endif
pressure_sensor_init();
set_flying_status(AIRCRAFT_PREPARING);
if(command==0){
//UAV is working in normal mode
// reset CPLD ,this should be check later ,if CPLD should be reset when CPU recovers from failure.
reset_control_register(CTRL_REG_MASK_MANUAL);
// wait for flying attitude sensor data
while (timeout <= 600) {
sleep(1);
if(flying_attitude_sensor_is_active())
break;
timeout++;
}
if (timeout >600) {
print_err("flying attitude sensor is inactive, please check serial port is connected\n");
goto exit;
}
print_debug("Flying attitude sensor is active\n");
p=get_flying_attitude();
//printf("system beijing time %d-%d-%d,%d:%d:%d\n",p->year,p->month,p->day,p->hour+8,p->min,p->sec);
sprintf(set_system_time,"date -s \"%d-%d-%d %d:%d:%d\"",p->year+2000,p->month,p->day,p->hour+8,p->min,p->sec);
//printf("%s\n",set_system_time);
system(set_system_time);
}else if(command==1){
//UAV is working in test mode
reset_control_register(CTRL_REG_MASK_MANUAL);
printf("uav is working in test mode,IMU is by passed,using test data instead\n");
}else if(command==2){
//UAV is in mannual mode ,for pwm data capture
set_flying_status(AIRCRAFT_MANUAL_MODE);
set_control_register(CTRL_REG_MASK_MANUAL);
set_system_status(SYS_PREPARE_SETTING);
printf("UAV is now working in data capturing mode\n");
}else{
printf(" error command,please check the usage:\n");
printf(" usage: uav [command] [gap] ]\n");
printf(" [command]: 0--normal mode,used when run in the air\n");
printf(" [command]: 1--test mode,used for platform test.use this mode \n");
printf(" when no IMU connected ,\n");
printf(" [command]: 2--manual mode,used when capturing PWM data\n");
printf(" [frequency]: the frequency (ms/frame) in which UAV send fly status data\n");
}
generate_file_name(log_file_name);
if((fp_fly_status=fopen(log_file_name,"wb+"))==NULL){
printf("can not open file:%s\n",log_file_name);
}
heli_configuration_init();
control_parameter_init();
servo_test_enable=0;
while (1) {
if(get_system_status() < SYS_PREPARE_STEERING_TEST){
flying_status_return(1);
usleep(500000);
}else if(get_system_status() == SYS_PREPARE_STEERING_TEST){
steering_test();
usleep(20000);
}else if(get_system_status() == SYS_PREPARE_TAKEOFF){
return 0;
}
/*
if (get_system_status() >= SYS_PREPARE_SETTING) {
print_debug("Link is ready\n");
return 0;
}
link_testing_send();
usleep(20000); //20ms
*/
}
exit:
sensor_close();
return ret;
}
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);
}