void send_version()
{ uint16 crc_value;
uint8 buf[31];
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) = 0x1F;
buf[6] = 1;
buf[7] = 0;
buf[8] = 1;
buf[9] = 0;
buf[10] = CTRL_FRAME_TYPE_VERSION ;
buf[11] = get_board_id();
*(uint32*)(buf+12) = 0x0; // kernel version
*(uint32*)(buf+16) = 0x0; // app version
*(uint32*)(buf+20) = get_fpga_version();
*(uint32*)(buf+24) = 0x0; // reserve
crc_value = crc_checksum16(buf, 28);
buf[28] = crc_value&0xFF;
buf[29] = crc_value>>8;
buf[30] = CTRL_FRAME_END;
control_cmd_send(buf, 31);
}
/* control_cmd_response_recv()
* send back the received command to ground after receiving
*/
void control_cmd_response_recv(uint8 *data,uint16 data_size)
{
uint16 crc_value;
uint8 buf[CTRL_FRAME_MAX_LEN];
uint16 frame_size=data_size + CTRL_FRAME_LEN_NO_DATA;
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) = frame_size;
buf[6] = 1;
buf[7] = 0;
buf[8] = 1;
buf[9] = 0;
buf[10] = CTRL_FRAME_TYPE_CMD_ACK ;
memcpy(buf+11,data,data_size);
crc_value = crc_checksum16(buf, data_size+11);
buf[frame_size-3] = crc_value&0xFF;
buf[frame_size-2] = crc_value>>8;
buf[frame_size-1] = CTRL_FRAME_END;
control_cmd_send(buf, frame_size);
if(debug_enable)
printf("control_cmd_response_recv-----type:%x------\n",data[10]);
}
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);
}
/* control_cmd_response_exe()
* send this command to ground after command execution
*/
void control_cmd_response_exe(uint8 data)
{
uint16 crc_value;
uint8 buf[15];
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) = 0x0F;
buf[8] = 1;
buf[9] = 1;
buf[10] = CTRL_FRAME_TYPE_CMD_EXE ;
buf[11] = data;
crc_value = crc_checksum16(buf, 12);
buf[12] = crc_value&0xFF;
buf[13] = crc_value>>8;
buf[14] = CTRL_FRAME_END;
control_cmd_send(buf, 15);
}
void fault_status_return(uint8 fault)
{
uint8 buf[15];
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) = 15;
buf[8] = 1;
buf[9] = 1;
buf[10] = CTRL_FRAME_TYPE_ERROR;
buf[11] = fault;
crc_value=crc_checksum16(buf, 12);
buf[12] = crc_value&0xFF;
buf[13] = crc_value>>8;
buf[14] = CTRL_FRAME_END;
control_cmd_send(buf, 15);
}
/* control_cmd_response_exe()
* send this command to ground after command execution
*/
void control_cmd_response_exe(uint8 data)
{
uint16 crc_value;
uint8 buf[15];
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) = 0x0F;
buf[6] = 1;
buf[7] = 0;
buf[8] = 1;
buf[9] = 0;
buf[10] = CTRL_FRAME_TYPE_CMD_EXE ;
buf[11] = data;
crc_value = crc_checksum16(buf, 12);
buf[12] = crc_value&0xFF;
buf[13] = crc_value>>8;
buf[14] = CTRL_FRAME_END;
control_cmd_send(buf, 15);
if(debug_enable)
printf("control_cmd_response_exe----type:%x-------\n",data);
}
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);
}
