使用mavlink指令，通过树莓派开发板间接控制来控制无人机APM.zip

// ------------------------------------------------------------------------------ // Includes // ------------------------------------------------------------------------------ #include "autopilot_interface.h" // ---------------------------------------------------------------------------------- // Time // ------------------- --------------------------------------------------------------- uint64_t get_time_usec() { static struct timeval _time_stamp; gettimeofday(&_time_stamp, NULL); return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec; } // ---------------------------------------------------------------------------------- // Setpoint Helper Functions // ---------------------------------------------------------------------------------- // choose one of the next three /* * Set target local ned position * * Modifies a mavlink_set_position_target_local_ned_t struct with target XYZ locations * in the Local NED frame, in meters. */ void set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t &sp) { sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION; sp.coordinate_frame = MAV_FRAME_LOCAL_NED; sp.x = x; sp.y = y; sp.z = z; printf("POSITION SETPOINT XYZ = [ %.4f , %.4f , %.4f ] \n", sp.x, sp.y, sp.z); } /* * Set target local ned velocity * * Modifies a mavlink_set_position_target_local_ned_t struct with target VX VY VZ * velocities in the Local NED frame, in meters per second. */ void set_velocity(float vx, float vy, float vz, mavlink_set_position_target_local_ned_t &sp) { sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY ; sp.coordinate_frame = MAV_FRAME_LOCAL_NED; sp.vx = vx; sp.vy = vy; sp.vz = vz; //printf("VELOCITY SETPOINT UVW = [ %.4f , %.4f , %.4f ] \n", sp.vx, sp.vy, sp.vz); } /* * Set target local ned acceleration * * Modifies a mavlink_set_position_target_local_ned_t struct with target AX AY AZ * accelerations in the Local NED frame, in meters per second squared. */ void set_acceleration(float ax, float ay, float az, mavlink_set_position_target_local_ned_t &sp) { // NOT IMPLEMENTED fprintf(stderr,"set_acceleration doesn't work yet \n"); throw 1; sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION & MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY ; sp.coordinate_frame = MAV_FRAME_LOCAL_NED; sp.afx = ax; sp.afy = ay; sp.afz = az; } // the next two need to be called after one of the above /* * Set target local ned yaw * * Modifies a mavlink_set_position_target_local_ned_t struct with a target yaw * in the Local NED frame, in radians. */ void set_yaw(float yaw, mavlink_set_position_target_local_ned_t &sp) { sp.type_mask &= MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE ; sp.yaw = yaw; printf("POSITION SETPOINT YAW = %.4f \n", sp.yaw); } /* * Set target local ned yaw rate * * Modifies a mavlink_set_position_target_local_ned_t struct with a target yaw rate * in the Local NED frame, in radians per second. */ void set_yaw_rate(float yaw_rate, mavlink_set_position_target_local_ned_t &sp) { sp.type_mask &= MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE ; sp.yaw_rate = yaw_rate; } // ---------------------------------------------------------------------------------- // Autopilot Interface Class // ---------------------------------------------------------------------------------- // ------------------------------------------------------------------------------ // Con/De structors // ------------------------------------------------------------------------------ Autopilot_Interface:: Autopilot_Interface(Serial_Port *serial_port_) { // initialize attributes write_count = 0; reading_status = 0; // whether the read thread is running writing_status = 0; // whether the write thread is running control_status = 0; // whether the autopilot is in offboard control mode time_to_exit = false; // flag to signal thread exit read_tid = 0; // read thread id write_tid = 0; // write thread id system_id = 0; // system id autopilot_id = 0; // autopilot component id companion_id = 0; // companion computer component id current_messages.sysid = system_id; current_messages.compid = autopilot_id; serial_port = serial_port_; // serial port management object } Autopilot_Interface:: ~Autopilot_Interface() {} // ------------------------------------------------------------------------------ // Update Setpoint // ------------------------------------------------------------------------------ void Autopilot_Interface:: update_setpoint(mavlink_set_position_target_local_ned_t setpoint) { current_setpoint = setpoint; } // ------------------------------------------------------------------------------ // Read Messages // ------------------------------------------------------------------------------ void Autopilot_Interface:: read_messages() { bool success; // receive success flag bool received_all = false; // receive only one message Time_Stamps this_timestamps; // Blocking wait for new data while ( !received_all and !time_to_exit ) { // ---------------------------------------------------------------------- // READ MESSAGE // ---------------------------------------------------------------------- mavlink_message_t message; success = serial_port->read_message(message); // ---------------------------------------------------------------------- // HANDLE MESSAGE // ---------------------------------------------------------------------- if( success ) { // Store message sysid and compid. // Note this doesn't handle multiple message sources. current_messages.sysid = message.sysid; current_messages.compid = message.compid; // Handle Message ID switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { //printf("MAVLINK_MSG_ID_HEARTBEAT\n"); mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat)); current_messages.time_stamps.heartbeat = get_time_usec(); this_timestamps.heartbeat = current_messages.time_stamps.heartbeat; break; } case MAVLINK_MSG_ID_SYS_STATUS: { //printf("MAVLINK_MSG_ID_SYS_STATUS\n"); mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status)); current_messages.time_stamps.sys_status = get_time_usec(); this_timestamps.sys_status = current_messages.time_stamps.sys_status; break; } case MAVLINK_MSG_ID_BATTERY_STATUS: { //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n"); mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status)); current_messages.time_stamps.battery_status = get_time_usec(); this_timestamps.battery_status = current_messages.time_stamps.battery_status; break; } case MAVLINK_MSG_ID_RADIO_STATUS: { //printf("MAVLINK_MSG_ID_RADIO_STATUS\n"); mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status)); current_messages.time_stamps.radio_status = get_time_usec(); this_timestamps.radio_status = current_messages.time_stamps.radio_status; break; } case MAVLINK_MSG_ID_LOCAL_POSITION_NED: { //printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n"); mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned)); current_messages.time_stamps.local_position_ned = get_time_usec(); this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned; break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { //printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n"); mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int)); current_messages.time_stamps.global_position_int = get_time_usec(); this_timestamps.global_position_int = current_messages.time_stamps.global_position_int; break; } case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NE