mavlink协议，c++语言版本，用于px4飞控通信

/**************************************************************************** * * Copyright (c) 2014 MAVlink Development Team. All rights reserved. * Author: Trent Lukaczyk, <[email protected]> * Jaycee Lock, <[email protected]> * Lorenz Meier, <[email protected]> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file autopilot_interface.cpp * * @brief Autopilot interface functions * * Functions for sending and recieving commands to an autopilot via MAVlink * * @author Trent Lukaczyk, <[email protected]> * @author Jaycee Lock, <[email protected]> * @author Lorenz Meier, <[email protected]> * */ // ------------------------------------------------------------------------------ // Includes // ------------------------------------------------------------------------------ #include <iostream> #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; std::cout<< sp.type_mask << std::endl; 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; std::cout<< sp.type_mask << std::endl; 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(Generic_Port *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; port = port_; // port management object } Autopilot_Interface:: ~Autopilot_Interface() {} // ------------------------------------------------------------------------------ // Update Setpoint // ------------------------------------------------------------------------------ void Autopilot_Interface:: update_setpoint(mavlink_set_position_target_local_ned_t setpoint) { std::lock_guard<std::mutex> lock(current_setpoint.mutex); current_setpoint.data = 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 = port->read_message(message); // ---------------------------------------------------------------------- // HANDLE MESSAGE // ---------------------------------------------------------------------- if( success ) { // Store message sysid and compid. // Note this