ardupilot之mavlink消息--飞控接收--单向
由于项目需要，完成一个测试demo, 本次从中发送消息给飞控，飞控接收发来的wp信息，然后进行修改供程序使用。 首先祭出测试视频 est
跳转进去
再次套娃，可以得到下图：
该函数中的最后一个函数 ();
涉及的文件主要是 .cpp , 找到其中 () 即可。里面是通过语句来判断对应的消息ID的, 如下面代码所示：
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86{// decode packetmavlink_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 modeif (!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 providedif (!acc_ignore) {break;}// extract location from messageLocation loc;if (!pos_ignore) {// sanity check locationif (!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 framebreak;}loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};}// prepare yawfloat 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 controllerif (!pos_ignore && !vel_ignore) {// convert Location to vector from ekf origin for posvel controllerif (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {// posvel controller does not support alt-above-terrainbreak;}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;
大概流程就是判断接收到的数据是什么类型，然后进行相应的控制（控制速度，控制航点，还是速度和位置都控制）。需要修改的地方就显而易见了。
需要注意的点：
本次实验采用的是脚本的形式完成消息的解析。在中 不要采用函数，该函数好像用的是, 而不是，导致捕获不到消息。脚本在处理 的时候，解析出错，报错为中的.pack需要整形的数据，但是发送的不是。最后采用强制数据类型转换完成。脚本附赠如下：
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 componentmavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame0b0000111111111000, # type_mask (only speeds enabled)alocation.lat*1e7, # lat_int - X Position in WGS84 frame in 1e7 * metersalocation.lon*1e7, # lon_int - Y Position in WGS84 frame in 1e7 * metersalocation.alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT0, # X velocity in NED frame in m/s0, # Y velocity in NED frame in m/s0, # Z velocity in NED frame in m/s0, 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 vehiclevehicle.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 specifiedlocation 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 componentmavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame0b0000111111111000, # 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 vehiclevehicle.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 radiansdLat = dNorth / earth_radiusdLon = dEast / (earth_radius * math.cos(math.pi * original_location.lat / 180))# New position in decimal degreesnewlat = 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 targetlocationdef goto(dNorth, dEast, gotoFunction=vehicle.simple_goto):currentLocation = vehicle.location.global_relative_frametargetLocation = 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.1elif 'k' == input():delay_time = delay_time - 0.1else:print("Wrong input... Nothing changed...")if delay_time < 0:delay_time = 0.1elif delay_time > 60:delay_time = 60print(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-4lon = 117.16 + random.random() * 66 * 1e-4alt = 30location_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.1time.sleep(random_time)
