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); }