dronekit
handleMessage();
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;
大概流程就是判断接收到的数据是什么类型，然后进行相应的控制（控制速度，控制航点，还是速度和位置都控制）。需要修改的地方就显而易见了。
需要注意的点：
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)