ardupilot之mavlink消息--飞控接收--单向

由于项目需要，完成一个测试demo, 本次从dronekit中发送mavlink消息给飞控，飞控接收发来的wp信息，然后进行修改供程序使用。

首先祭出测试视频 dronekit_arudpilot_test

dronekit

1. 首先从copter.cpp中找到到对应的函数，如下图所示：
2. 跳转进去
   
3. 再次套娃，可以得到下图：
   
4. 该函数中的最后一个函数 handleMessage();
5. 涉及的文件主要是 GCS_mavlink.cpp , 找到其中 headleMessage() 即可。里面是通过switch语句来判断对应的消息ID的, 如下面代码所示：

case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
    {
        // decode packet
        mavlink_set_position_target_global_int_t packet;
        mavlink_msg_set_position_target_global_int_decode(&msg, &packet);

        // exit if vehicle is not in Guided mode or Auto-Guided mode
        if (!copter.flightmode->in_guided_mode()) {
            break;
        }

        bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
        bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
        bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
        bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
        bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;

        // exit immediately if acceleration provided
        if (!acc_ignore) {
            break;
        }

        // extract location from message
        Location loc;
        if (!pos_ignore) {
            // sanity check location
            if (!check_latlng(packet.lat_int, packet.lon_int)) {
                break;
            }
            Location::AltFrame frame;
            if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
                // unknown coordinate frame
                break;
            }
            loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
        }

        // prepare yaw
        float yaw_cd = 0.0f;
        bool yaw_relative = false;
        float yaw_rate_cds = 0.0f;
        if (!yaw_ignore) {
            yaw_cd = ToDeg(packet.yaw) * 100.0f;
            yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
        }
        if (!yaw_rate_ignore) {
            yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
        }

        // send targets to the appropriate guided mode controller
        if (!pos_ignore && !vel_ignore) {
            // convert Location to vector from ekf origin for posvel controller
            if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
                // posvel controller does not support alt-above-terrain
                break;
            }
            Vector3f pos_neu_cm;
            if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
                break;
            }
            copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        } else if (pos_ignore && !vel_ignore) {
            copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        } else if (!pos_ignore && vel_ignore) {
            copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
        }

        break;

大概流程就是判断接收到的数据是什么类型，然后进行相应的控制（控制速度，控制航点，还是速度和位置都控制）。需要修改的地方就显而易见了。

需要注意的点：

1. 本次实验采用的是dronekit脚本的形式完成mavnlink消息的解析。在dronekit中 不要采用simple_go函数，该函数好像用的是manvlink_command, 而不是mavlink_message，导致捕获不到消息。
2. dronekit脚本在处理 goto_position_target_global_int的时候，pymavlink解析出错，报错为python中的struct.pack需要整形的数据，但是发送的不是。最后采用强制数据类型转换完成。
3. dronekit脚本附赠如下：

import time
import random
import dronekit
import math
from pymavlink import mavutil
import threading
vehicle = dronekit.connect('127.0.0.1:14550',wait_ready=True)
master =  mavutil.mavlink_connection('udp:127.0.0.1:{}'.format(14551))
master.wait_heartbeat()

def prearm_check():
    if vehicle.mode.name != "STABILIZE":
        vehicle.mode = "STABILIZE"
        print("waiting for changing the mode to stabilize...")
        vehicle.wait_for_mode("STABILIZE")
        print("changing mode to stabilize successfully...")
    while not vehicle.gps_0.fix_type:
        time.sleep(0.5)
    print("gps check passed...")
    while not vehicle.ekf_ok:
        time.sleep(0.5)
    print("ekf check passed...")

def goto_position_target_global_int(alocation):
    """
    Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified location.
    """
    msg = vehicle.message_factory.set_position_target_global_int_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
        0b0000111111111000, # type_mask (only speeds enabled)
        alocation.lat*1e7, # lat_int - X Position in WGS84 frame in 1e7 * meters
        alocation.lon*1e7, # lon_int - Y Position in WGS84 frame in 1e7 * meters
        alocation.alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
        0, # X velocity in NED frame in m/s
        0, # Y velocity in NED frame in m/s
        0, # Z velocity in NED frame in m/s
        0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
        0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
    # send command to vehicle
    vehicle.send_mavlink(msg)

def goto_position_target_local_ned(vehicle, north, east, down):
    """
    Send SET_POSITION_TARGET_LOCAL_NED command to request the vehicle fly to a specified
    location in the North, East, Down frame.
    """
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # frame
        0b0000111111111000,  # type_mask (only positions enabled)
        north, east, down,
        0, 0, 0,  # x, y, z velocity in m/s  (not used)
        0, 0, 0,  # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
        0, 0)  # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
    # send command to vehicle
    vehicle.send_mavlink(msg)

def is_fly_land_reset():
    if vehicle.armed == True:
        while vehicle.mode != "RTL":
            vehicle.wait_for_mode("RTL")
        print("Set mode to RTL...")
    while vehicle.armed != False:
        time.sleep(1)
    vehicle.wait_for_mode("STABILIZE")
    print("Vehicle reset OK...")


def get_location_metres(original_location, dNorth, dEast):
    earth_radius = 6378137.0  # Radius of "spherical" earth
    # Coordinate offsets in radians
    dLat = dNorth / earth_radius
    dLon = dEast / (earth_radius * math.cos(math.pi * original_location.lat / 180))

    # New position in decimal degrees
    newlat = original_location.lat + (dLat * 180 / math.pi)
    newlon = original_location.lon + (dLon * 180 / math.pi)
    if type(original_location) is dronekit.LocationGlobal:
        targetlocation = dronekit.LocationGlobal(newlat, newlon, original_location.alt)
    elif type(original_location) is dronekit.LocationGlobalRelative:
        targetlocation = dronekit.LocationGlobalRelative(newlat, newlon, original_location.alt)
    else:
        raise Exception("Invalid Location object passed")

    return targetlocation

def goto(dNorth, dEast, gotoFunction=vehicle.simple_goto):

    currentLocation = vehicle.location.global_relative_frame
    targetLocation = get_location_metres(currentLocation, dNorth, dEast)
    # targetDistance = get_distance_metres(currentLocation, targetLocation)
    gotoFunction(targetLocation)

delay_time = 60
def time_schedule():
    while True:
        if 'i' == input():
            delay_time  = delay_time + 0.1
        elif 'k' == input():
            delay_time = delay_time - 0.1
        else:
            print("Wrong input... Nothing changed...")
        if delay_time < 0:
            delay_time = 0.1
        elif delay_time > 60:
            delay_time = 60
        print(delay_time)
        time.sleep(0.1)

if __name__ == '__main__':
    is_fly_land_reset()
    prearm_check()
    vehicle.mode = "GUIDED"
    vehicle.wait_for_mode("GUIDED")

    while vehicle.armed == False:
        vehicle.arm(True)
    print("vehicle armed...")

    # vehicle.simple_takeoff(alt=12)
    vehicle.wait_simple_takeoff(alt= 12,epsilon=2)
    while vehicle.location.local_frame.down > -8:
        print(vehicle.location.local_frame.down)
        time.sleep(0.5)
    print("Begin to navigation...")
    while True:
        lat = 39.1036 + random.random() * 32 * 1e-4
        lon = 117.16 + random.random() * 66 * 1e-4
        alt = 30
        location_send = dronekit.LocationGlobalRelative(lat, lon, alt)
        # vehicle.simple_goto(location=location_send)
        # goto(-100, -130, goto_position_target_global_int)
        goto_position_target_global_int(location_send)
        # goto_position_target_local_ned(vehicle,5,2,-20)
        # master.mav.set_position_target_global_int_send(
        #     0,
        #     master.target_system,
        #     master.target_component,
        #     6,
        #     lat*1e7,
        #     lon*1e7,
        #     alt,
        #     0,0,0,
        #     0,0,0,
        #     0,0,0, force_mavlink1=True)
        print("target lat=%f,\t lon=%f,\t current lat=%f,\t lon=%f"
              %(lat, lon, vehicle.location.global_frame.lat, vehicle.location.global_frame.lon))
        random_time = random.random()*9.9+0.1
        time.sleep(random_time)