From 9fb72e2fa5655f5ab129ca8c4fceb9ed8bd84010 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 3 Oct 2024 19:20:35 +1000 Subject: [PATCH] mavproxy_wp: skip DO_LAND_START waypoints when splitting --- MAVProxy/modules/mavproxy_wp.py | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/MAVProxy/modules/mavproxy_wp.py b/MAVProxy/modules/mavproxy_wp.py index cc28b7ee76..95bbced0e4 100644 --- a/MAVProxy/modules/mavproxy_wp.py +++ b/MAVProxy/modules/mavproxy_wp.py @@ -556,18 +556,23 @@ def cmd_split(self, args): print("wp is not a location command") return - prev = num - 1 - if prev < 1 or prev > self.wploader.count(): - print("Bad item %u" % num) - return - prev_wp = self.wploader.wp(prev) - if prev_wp is None: - print("Could not get previous wp %u" % prev) - return - prev_loc = self.get_loc(prev_wp) - if prev_loc is None: - print("previous wp is not a location command") - return + prev = num + while True: + prev = prev - 1 + if prev < 1 or prev > self.wploader.count(): + print("Bad item %u" % num) + return + prev_wp = self.wploader.wp(prev) + if prev_wp is None: + print("Could not get previous wp %u" % prev) + return + if prev_wp.command == mavutil.mavlink.MAV_CMD_DO_LAND_START: + continue + prev_loc = self.get_loc(prev_wp) + if prev_loc is None: + print("previous wp is not a location command") + return + break if wp.frame != prev_wp.frame: print("waypoints differ in frame (%u vs %u)" %