From 32f68147de5ec50814b579577245b0ed58f6840f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 30 Aug 2019 23:00:52 +1000 Subject: [PATCH 01/11] mavwp: set initial change time to zero This helps us detect whether we've actually legitimately got an item list --- mavwp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavwp.py b/mavwp.py index 4007b6b12..0f0011365 100644 --- a/mavwp.py +++ b/mavwp.py @@ -32,7 +32,7 @@ def __init__(self, target_system=0, target_component=0): self.wpoints = [] self.target_system = target_system self.target_component = target_component - self.last_change = time.time() + self.last_change = 0 self.colour_for_polygon = { mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION : (255,0,0), mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION : (0,255,0) From 227a3dbe47bdb191fdd871da9494ff07012613c8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 28 Aug 2019 17:30:30 +1000 Subject: [PATCH 02/11] mavwp: allow remove to remove a list of waypoints --- mavwp.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mavwp.py b/mavwp.py index 0f0011365..2a35839f7 100644 --- a/mavwp.py +++ b/mavwp.py @@ -121,7 +121,11 @@ def set(self, w, idx): def remove(self, w): '''remove a waypoint''' - self.wpoints.remove(w) + if isinstance(w, list): + for point in w: + self.wpoints.remove(point) + else: + self.wpoints.remove(w) self.last_change = time.time() self.reindex() From bc3307825ab4edb393845a0396e6dc2edef7c1fa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 30 Aug 2019 23:01:35 +1000 Subject: [PATCH 03/11] mavwp: allow add to add a list of waypoints --- mavwp.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/mavwp.py b/mavwp.py index 2a35839f7..47ed50a0c 100644 --- a/mavwp.py +++ b/mavwp.py @@ -65,11 +65,19 @@ def wp_is_loiter(self, i): def add(self, w, comment=''): '''add a waypoint''' - w = copy.copy(w) - if comment: - w.comment = comment - w.seq = self.count() - self.wpoints.append(w) + if type(w) == list: + w = copy.deepcopy(w) + n = self.count() + for p in w: + p.seq = n + n += 1 + self.wpoints.extend(w) + else: + w = copy.copy(w) + if comment: + w.comment = comment + w.seq = self.count() + self.wpoints.append(w) self.last_change = time.time() def insert(self, idx, w, comment=''): From 506f0b34988a20e31f498de644a813346859f61b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 30 Jul 2019 14:50:38 +1000 Subject: [PATCH 04/11] mavwp: create mission-item-protocol-based class for rally upload --- mavwp.py | 242 +++++++++++++++++++++++++++++++------------------------ 1 file changed, 137 insertions(+), 105 deletions(-) diff --git a/mavwp.py b/mavwp.py index 47ed50a0c..f9651513b 100644 --- a/mavwp.py +++ b/mavwp.py @@ -26,8 +26,8 @@ def __init__(self, msg): self.message = msg -class MAVWPLoader(object): - '''MAVLink waypoint loader''' +class MissionItemProtocol(object): + '''Base class for transfering items based on the MISSION_ITEM protocol''' def __init__(self, target_system=0, target_component=0): self.wpoints = [] self.target_system = target_system @@ -43,7 +43,11 @@ def count(self): return len(self.wpoints) def wp(self, i): - '''return a waypoint''' + '''alias for backwards compatability''' + return self.item(i) + + def item(self, i): + '''return an item''' try: the_wp = self.wpoints[i] except: @@ -51,18 +55,6 @@ def wp(self, i): return the_wp - def wp_is_loiter(self, i): - '''return true if waypoint is a loiter waypoint''' - loiter_cmds = [mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, - mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, - mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, - mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT] - - if (self.wpoints[i].command in loiter_cmds): - return True - - return False - def add(self, w, comment=''): '''add a waypoint''' if type(w) == list: @@ -102,21 +94,6 @@ def reindex(self): w.seq = i self.last_change = time.time() - def add_latlonalt(self, lat, lon, altitude, terrain_alt=False): - '''add a point via latitude/longitude/altitude''' - if terrain_alt: - frame = mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT - else: - frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT - p = mavutil.mavlink.MAVLink_mission_item_message(self.target_system, - self.target_component, - 0, - frame, - mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, - 0, 0, 0, 0, 0, 0, - lat, lon, altitude) - self.add(p) - def set(self, w, idx): '''set a waypoint''' w.seq = idx @@ -142,53 +119,6 @@ def clear(self): self.wpoints = [] self.last_change = time.time() - def _read_waypoints_v100(self, file): - '''read a version 100 waypoint''' - cmdmap = { - 2 : mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 3 : mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, - 4 : mavutil.mavlink.MAV_CMD_NAV_LAND, - 24: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 26: mavutil.mavlink.MAV_CMD_NAV_LAND, - 25: mavutil.mavlink.MAV_CMD_NAV_WAYPOINT , - 27: mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM - } - comment = '' - for line in file: - if line.startswith('#'): - comment = line[1:].lstrip() - continue - line = line.strip() - if not line: - continue - a = line.split() - if len(a) != 13: - raise MAVWPError("invalid waypoint line with %u values" % len(a)) - if mavutil.mavlink10(): - fn = mavutil.mavlink.MAVLink_mission_item_message - else: - fn = mavutil.mavlink.MAVLink_waypoint_message - w = fn(self.target_system, self.target_component, - int(a[0]), # seq - int(a[1]), # frame - int(a[2]), # action - int(a[7]), # current - int(a[12]), # autocontinue - float(a[5]), # param1, - float(a[6]), # param2, - float(a[3]), # param3 - float(a[4]), # param4 - float(a[9]), # x, latitude - float(a[8]), # y, longitude - float(a[10]) # z - ) - if not w.command in cmdmap: - raise MAVWPError("Unknown v100 waypoint action %u" % w.command) - - w.command = cmdmap[w.command] - self.add(w, comment) - comment = '' - def _read_waypoints_v110(self, file): '''read a version 110 waypoint''' comment = '' @@ -202,24 +132,32 @@ def _read_waypoints_v110(self, file): a = line.split() if len(a) != 12: raise MAVWPError("invalid waypoint line with %u values" % len(a)) - if mavutil.mavlink10(): + args = [ + self.target_system, + self.target_component, + int(a[0]), # seq + int(a[2]), # frame + int(a[3]), # command + int(a[1]), # current + int(a[11]), # autocontinue + float(a[4]), # param1, + float(a[5]), # param2, + float(a[6]), # param3 + float(a[7]), # param4 + float(a[8]), # x (latitude) + float(a[9]), # y (longitude) + float(a[10]), # z (altitude) + ] + if mavutil.mavlink20(): + fn = mavutil.mavlink.MAVLink_mission_item_message + args.append(self.mav_mission_type()) + elif mavutil.mavlink10(): fn = mavutil.mavlink.MAVLink_mission_item_message + if self.mav_mission_type() != mavutil.mavlink.MAV_MISSION_TYPE_MISSION: + raise ValueError("Not using mavlink2") else: fn = mavutil.mavlink.MAVLink_waypoint_message - w = fn(self.target_system, self.target_component, - int(a[0]), # seq - int(a[2]), # frame - int(a[3]), # command - int(a[1]), # current - int(a[11]), # autocontinue - float(a[4]), # param1, - float(a[5]), # param2, - float(a[6]), # param3 - float(a[7]), # param4 - float(a[8]), # x (latitude) - float(a[9]), # y (longitude) - float(a[10]) # z (altitude) - ) + w = fn(*args) if w.command == 0 and w.seq == 0 and self.count() == 0: # special handling for Mission Planner created home wp w.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT @@ -336,19 +274,6 @@ def save(self, filename): w.x, w.y, w.z, w.autocontinue)) f.close() - def is_location_command(self, cmd): - '''see if cmd is a MAV_CMD with a latitude/longitude''' - mav_cmd = mavutil.mavlink.enums['MAV_CMD'] - if not cmd in mav_cmd: - return False - return getattr(mav_cmd[cmd],'has_location',True) - - def is_location_wp(self, w): - '''see if w.command is a MAV_CMD with a latitude/longitude''' - if w.x == 0 and w.y == 0: - return False - return self.is_location_command(w.command) - def view_indexes(self, done=None): '''return a list waypoint indexes in view order''' ret = [] @@ -443,12 +368,119 @@ def view_list(self): ret.append(p) return ret +class MAVWPLoader(MissionItemProtocol): + '''MAVLink waypoint loader''' + def mav_mission_type(self): + '''returns type of mission this object transfers''' + return mavutil.mavlink.MAV_MISSION_TYPE_MISSION + + def wp_is_loiter(self, i): + '''return true if waypoint is a loiter waypoint''' + loiter_cmds = [mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT] + + if (self.wpoints[i].command in loiter_cmds): + return True + + return False + + def add_latlonalt(self, lat, lon, altitude, terrain_alt=False): + '''add a point via latitude/longitude/altitude''' + if terrain_alt: + frame = mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT + else: + frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + p = mavutil.mavlink.MAVLink_mission_item_message(self.target_system, + self.target_component, + 0, + frame, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, 0, 0, 0, 0, 0, + lat, lon, altitude) + self.add(p) + + def _read_waypoints_v100(self, file): + '''read a version 100 waypoint''' + cmdmap = { + 2 : mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 3 : mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + 4 : mavutil.mavlink.MAV_CMD_NAV_LAND, + 24: mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 26: mavutil.mavlink.MAV_CMD_NAV_LAND, + 25: mavutil.mavlink.MAV_CMD_NAV_WAYPOINT , + 27: mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM + } + comment = '' + for line in file: + if line.startswith('#'): + comment = line[1:].lstrip() + continue + line = line.strip() + if not line: + continue + a = line.split() + if len(a) != 13: + raise MAVWPError("invalid waypoint line with %u values" % len(a)) + if mavutil.mavlink10(): + fn = mavutil.mavlink.MAVLink_mission_item_message + else: + fn = mavutil.mavlink.MAVLink_waypoint_message + w = fn(self.target_system, self.target_component, + int(a[0]), # seq + int(a[1]), # frame + int(a[2]), # action + int(a[7]), # current + int(a[12]), # autocontinue + float(a[5]), # param1, + float(a[6]), # param2, + float(a[3]), # param3 + float(a[4]), # param4 + float(a[9]), # x, latitude + float(a[8]), # y, longitude + float(a[10]) # z + ) + if not w.command in cmdmap: + raise MAVWPError("Unknown v100 waypoint action %u" % w.command) + + w.command = cmdmap[w.command] + self.add(w, comment) + comment = '' + + def is_location_command(self, cmd): + '''see if cmd is a MAV_CMD with a latitude/longitude''' + mav_cmd = mavutil.mavlink.enums['MAV_CMD'] + if not cmd in mav_cmd: + return False + return getattr(mav_cmd[cmd],'has_location',True) + + def is_location_wp(self, w): + '''see if w.command is a MAV_CMD with a latitude/longitude''' + if w.x == 0 and w.y == 0: + return False + return self.is_location_command(w.command) + + +class MissionItemProtocol_Rally(MissionItemProtocol): + '''New mission-item-protocol-based class for sending rally points to + autopilot''' + + def mav_mission_type(self): + '''returns type of mission this object transfers''' + return mavutil.mavlink.MAV_MISSION_TYPE_RALLY + + def is_location_command(self, cmd): + '''returns true if cmd nominates a location in param5/param6/param7''' + return True + class MAVRallyError(Exception): '''MAVLink rally point error class''' def __init__(self, msg): Exception.__init__(self, msg) self.message = msg + class MAVRallyLoader(object): '''MAVLink Rally points and Rally Land points loader''' def __init__(self, target_system=0, target_component=0): From 18eb7d066f8602f69060d734a8a2bdcbae9debda Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 22 Aug 2019 12:28:03 +1000 Subject: [PATCH 05/11] mavwp: make MissionItemProtocol_Rally load old rally file format --- mavwp.py | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/mavwp.py b/mavwp.py index f9651513b..201554fe0 100644 --- a/mavwp.py +++ b/mavwp.py @@ -10,6 +10,8 @@ import time, copy import logging +import re + from . import mavutil try: from google.protobuf import text_format @@ -474,6 +476,61 @@ def is_location_command(self, cmd): '''returns true if cmd nominates a location in param5/param6/param7''' return True + def load(self, filename): + '''attempts to load from legacy rally file''' + f = open(filename, mode='r') + version_line = f.readline().strip() + f.close() + if not re.match("^RALLY ", version_line): + return super(MissionItemProtocol_Rally, self).load(filename) + + points = [] + # this code shamelessly copy-and-pasted from the old + # MAVWPLoader, below + seq = 0 + f = open(filename, mode='r') + for line in f: + if line.startswith('#'): + continue + line = line.strip() + if not line: + continue + a = line.split() + if len(a) != 7: + raise MAVRallyError("invalid rally file line: %s" % line) + + if (a[0].lower() == "rally"): + lat_deg = float(a[1]) + lng_deg = float(a[2]) + alt = float(a[3]) +# break_alt = float(a[4]) +# land_dir = float(a[5]) +# flags = int(a[6]) + w = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, self.target_component, + seq, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame + mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT, # command + 0, # current + 0, # autocontinue + 0, # param1, + 0, # param2, + 0, # param3 + 0, # param4 + int(lat_deg * 1e7), # x (latitude) + int(lng_deg * 1e7), # y (longitude) + alt * 1e3, # z (altitude) + self.mav_mission_type(), + ) + points.append(w) + seq += 1 + f.close() + + self.clear() + for point in points: + self.add(point) + + class MAVRallyError(Exception): '''MAVLink rally point error class''' def __init__(self, msg): From 0b0f19d7373b6d1070b4f5b150a94d9fdfcf8560 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 23 Aug 2019 14:52:36 +1000 Subject: [PATCH 06/11] mavwp: create mission-item-protocol-based class for fence upload --- mavwp.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/mavwp.py b/mavwp.py index 201554fe0..3d702fec1 100644 --- a/mavwp.py +++ b/mavwp.py @@ -464,6 +464,18 @@ def is_location_wp(self, w): return self.is_location_command(w.command) +class MissionItemProtocol_Fence(MissionItemProtocol): + '''New mission-item-protocol-based class for sending fence points to + autopilot''' + + def mav_mission_type(self): + '''returns type of mission this object transfers''' + return mavutil.mavlink.MAV_MISSION_TYPE_FENCE + + def is_location_command(self, cmd): + '''returns true if cmd nominates a location in param5/param6/param7''' + return True + class MissionItemProtocol_Rally(MissionItemProtocol): '''New mission-item-protocol-based class for sending rally points to autopilot''' From 02609070ffa36122e5d7e6828f34901f9e475660 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 30 Aug 2019 23:02:24 +1000 Subject: [PATCH 07/11] mavwp: add ability to read from old fence-point files --- mavwp.py | 96 +++++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 92 insertions(+), 4 deletions(-) diff --git a/mavwp.py b/mavwp.py index 3d702fec1..1efeb013d 100644 --- a/mavwp.py +++ b/mavwp.py @@ -464,6 +464,19 @@ def is_location_wp(self, w): return self.is_location_command(w.command) +def get_first_line_from_file(filename): + '''return line from filename that doesn't start with a #''' + f = open(filename, mode='r') + while True: + line = f.readline().strip() + if len(line) == 0: + return None + if not line.startswith("#"): + break + + f.close() + return line + class MissionItemProtocol_Fence(MissionItemProtocol): '''New mission-item-protocol-based class for sending fence points to autopilot''' @@ -476,6 +489,83 @@ def is_location_command(self, cmd): '''returns true if cmd nominates a location in param5/param6/param7''' return True + def load(self, filename): + '''try to load from an old "FENCE_POINT" format file (just a list of + latlon pairs - or fall back to QGC formats''' + + version_line = get_first_line_from_file(filename) + + if (version_line is None or + not re.match("[-0-9.]+\s+[-0-9.]+", version_line)): + return super(MissionItemProtocol_Fence, self).load(filename) + + # shamelessly copy-and-pasted from traditional loader, below + f = open(filename, mode='r') + points = [] + for line in f: + if line.startswith('#'): + continue + line = line.strip() + if not line: + continue + a = line.split() + if len(a) != 2: + raise MAVFenceError("invalid fence point line: %s" % line) + points.append((float(a[0]),float(a[1]))) + f.close() + + # 1 return point + # at least 3 vertex points + # 1 closing point + if len(points) < 5: + print("Insufficient points in file") + return + + items = [] + # return point: + (ret_lat, ret_lng) = points[0] + items.append( + mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, # frame + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, # command + 0, # current + 0, # autocontinue + 0, # param1, + 0, # param2, + 0, # param3 + 0, # param4 + int(ret_lat * 1e7), # x (latitude) + int(ret_lng * 1e7), # y (longitude) + 0, # z (altitude) + self.mav_mission_type(), + )) + for i in range(1, len(points)-1): + (lat, lng) = points[i] + item = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, # frame + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, # command + 0, # current + 0, # autocontinue + len(points)-2, # param1, + 0, # param2, + 0, # param3 + 0, # param4 + int(lat * 1e7), # x (latitude) + int(lng * 1e7), # y (longitude) + 0, # z (altitude) + self.mav_mission_type(), + ) + items.append(item) + + self.clear() + self.add(items) + class MissionItemProtocol_Rally(MissionItemProtocol): '''New mission-item-protocol-based class for sending rally points to autopilot''' @@ -490,10 +580,8 @@ def is_location_command(self, cmd): def load(self, filename): '''attempts to load from legacy rally file''' - f = open(filename, mode='r') - version_line = f.readline().strip() - f.close() - if not re.match("^RALLY ", version_line): + version_line = get_first_line_from_file(filename) + if version_line is None or not re.match("^RALLY ", version_line): return super(MissionItemProtocol_Rally, self).load(filename) points = [] From 5756528187a4b06f0b5aaa01dd9b072c6b082ec9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 4 Jun 2022 15:37:37 +1000 Subject: [PATCH 08/11] mavwp: add explicit checks for mavlink20 in new loaders --- mavwp.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/mavwp.py b/mavwp.py index 1efeb013d..dd82b6430 100644 --- a/mavwp.py +++ b/mavwp.py @@ -493,6 +493,9 @@ def load(self, filename): '''try to load from an old "FENCE_POINT" format file (just a list of latlon pairs - or fall back to QGC formats''' + if not mavutil.mavlink20(): + raise ValueError("Must be using mavlink2") + version_line = get_first_line_from_file(filename) if (version_line is None or @@ -580,6 +583,9 @@ def is_location_command(self, cmd): def load(self, filename): '''attempts to load from legacy rally file''' + if not mavutil.mavlink20(): + raise ValueError("Must be using mavlink2") + version_line = get_first_line_from_file(filename) if version_line is None or not re.match("^RALLY ", version_line): return super(MissionItemProtocol_Rally, self).load(filename) @@ -620,7 +626,7 @@ def load(self, filename): int(lat_deg * 1e7), # x (latitude) int(lng_deg * 1e7), # y (longitude) alt * 1e3, # z (altitude) - self.mav_mission_type(), + self.mav_mission_type() ) points.append(w) seq += 1 From 8b75d1e2d3d99dbea20f4fbce50e4f6936658b58 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 4 Jun 2022 15:38:07 +1000 Subject: [PATCH 09/11] tests: add tests for new rally/fence loader classes --- tests/fence-110.txt | 11 +++++++++ tests/fence.txt | 6 +++++ tests/rally-110.txt | 3 +++ tests/rally.txt | 2 ++ tests/test_wp.py | 60 +++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 82 insertions(+) create mode 100644 tests/fence-110.txt create mode 100644 tests/fence.txt create mode 100644 tests/rally-110.txt create mode 100644 tests/rally.txt diff --git a/tests/fence-110.txt b/tests/fence-110.txt new file mode 100644 index 000000000..ace97c223 --- /dev/null +++ b/tests/fence-110.txt @@ -0,0 +1,11 @@ +QGC WPL 110 +0 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071766 -105.230202 0.000000 0 +1 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.230247 0.000000 0 +2 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.228821 0.000000 0 +3 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071609 -105.228867 0.000000 0 +4 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071602 -105.228172 0.000000 0 +5 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070858 -105.227982 0.000000 0 +6 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070789 -105.226219 0.000000 0 +7 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.072453 -105.226379 0.000000 0 +8 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071609 -105.228172 0.000000 0 +9 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071625 -105.227982 0.000000 0 diff --git a/tests/fence.txt b/tests/fence.txt new file mode 100644 index 000000000..705fc6d0b --- /dev/null +++ b/tests/fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358795 149.164734 +-35.359211 149.160767 +-35.368622 149.162750 +-35.368279 149.166870 +-35.358795 149.164734 diff --git a/tests/rally-110.txt b/tests/rally-110.txt new file mode 100644 index 000000000..ce1e842a6 --- /dev/null +++ b/tests/rally-110.txt @@ -0,0 +1,3 @@ +QGC WPL 110 +0 0 0 5100 8.000000 0.000000 0.000000 0.000000 40.071766 -105.230202 0.000000 0 +1 0 0 5100 8.000000 0.000000 0.000000 0.000000 40.071014 -105.230247 0.000000 0 diff --git a/tests/rally.txt b/tests/rally.txt new file mode 100644 index 000000000..5b6677395 --- /dev/null +++ b/tests/rally.txt @@ -0,0 +1,2 @@ +RALLY 40.071553 -105.229401 0.000000 40.000000 0.000000 0 +RALLY 40.072265 -105.231136 0.000000 40.000000 0.000000 0 diff --git a/tests/test_wp.py b/tests/test_wp.py index 2beef1ddd..55d6df773 100755 --- a/tests/test_wp.py +++ b/tests/test_wp.py @@ -9,6 +9,9 @@ import os import pkg_resources import sys + +os.environ["MAVLINK20"] = "1" + from pymavlink import mavwp from pymavlink import mavutil @@ -52,6 +55,8 @@ def make_wps(self): def test_add_remove(self): """Test we can add/remove waypoints to/from mavwp""" loader = mavwp.MAVWPLoader() + self.assertEqual(loader.mav_mission_type(), + mavutil.mavlink.MAV_MISSION_TYPE_MISSION) waypoints = self.make_wps() @@ -67,6 +72,11 @@ def test_add_remove(self): self.assertEqual(loader.wp(0).z, 0) self.assertEqual(loader.wp(1).z, 1) self.assertEqual(loader.wp(2).z, 2) + # short test for the new item() method: + self.assertEqual(loader.item(0).z, 0) + self.assertEqual(loader.item(1).z, 1) + self.assertEqual(loader.item(2).z, 2) + # remove a middle one, make sure things get renumbered loader.remove(waypoints[0]) @@ -185,11 +195,61 @@ def test_wp_is_loiter(self): self.assertFalse(loader.wp_is_loiter(1)) self.assertFalse(loader.wp_is_loiter(2)) + assert True + def test_is_location_command(self): loader = mavwp.MAVWPLoader() self.assertFalse(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH)) self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT)) self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS)) +class RallyTest(unittest.TestCase): + '''tests functions related to loading waypoints and transfering them + via the mission-item-protocol''' + def test_rally_load(self): + '''test loading rally points from old RALLY style file''' + loader = mavwp.MissionItemProtocol_Rally() + self.assertEqual(loader.mav_mission_type(), + mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT)) + + # test loading a QGC WPL 110 file: + rally_filepath = pkg_resources.resource_filename(__name__, "rally-110.txt") + loader.load(rally_filepath) + self.assertEqual(loader.count(), 2) + self.assertEqual(loader.wp(0).command, mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT) + + # test loading tradition "RALLY" style format: + rally_filepath = pkg_resources.resource_filename(__name__, "rally.txt") + loader.load(rally_filepath) + self.assertEqual(loader.count(), 2) + self.assertEqual(loader.wp(0).command, mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT) + +class FenceTest(unittest.TestCase): + def test_fence_load(self): + '''test loading rally points from old RALLY style file''' + loader = mavwp.MissionItemProtocol_Fence() + self.assertEqual(loader.mav_mission_type(), + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.assertTrue(loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION)) + + # test loading a QGC WPL 110 file: + fence_filepath = pkg_resources.resource_filename(__name__, + "fence-110.txt") + loader.load(fence_filepath) + self.assertEqual(loader.count(), 10) + self.assertEqual(loader.wp(3).command, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) + + # test loading tradition lat/lng-pair style format: + fence_filepath = pkg_resources.resource_filename(__name__, + "fence.txt") + loader.load(fence_filepath) + # there are 6 lines in the file - one return point, four fence + # points and a "fence closing point". We don't store the + # fence closing point. + self.assertEqual(loader.count(), 5) + self.assertEqual(loader.wp(0).command, mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT) + self.assertEqual(loader.wp(3).command, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) + if __name__ == '__main__': unittest.main() From c3f572b51d36bb1758c25e4016a678480729e263 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 22 Jul 2023 14:01:20 +1000 Subject: [PATCH 10/11] mavwp: set mission type when loading --- mavwp.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mavwp.py b/mavwp.py index dd82b6430..6906afaad 100644 --- a/mavwp.py +++ b/mavwp.py @@ -446,6 +446,9 @@ def _read_waypoints_v100(self, file): if not w.command in cmdmap: raise MAVWPError("Unknown v100 waypoint action %u" % w.command) + if self.mav_mission_type() != mavutil.mavlink.MAV_MISSION_TYPE_MISSION: + w.mission_type = self.mav_mission_type() + w.command = cmdmap[w.command] self.add(w, comment) comment = '' From d233da10fd2b0f60499ebf9072dce5df0520f525 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 22 Jul 2023 14:01:36 +1000 Subject: [PATCH 11/11] pytest: set mavlink20 in environment that way everything is imported under a consistent regime --- tests/__init__.py | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 tests/__init__.py diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 000000000..afcbe108f --- /dev/null +++ b/tests/__init__.py @@ -0,0 +1,3 @@ +import os + +os.environ["MAVLINK20"] = "1"