Skip to content

Commit 9fb72e2

Browse files
committed
mavproxy_wp: skip DO_LAND_START waypoints when splitting
1 parent f665668 commit 9fb72e2

File tree

1 file changed

+17
-12
lines changed

1 file changed

+17
-12
lines changed

Diff for: MAVProxy/modules/mavproxy_wp.py

+17-12
Original file line numberDiff line numberDiff line change
@@ -556,18 +556,23 @@ def cmd_split(self, args):
556556
print("wp is not a location command")
557557
return
558558

559-
prev = num - 1
560-
if prev < 1 or prev > self.wploader.count():
561-
print("Bad item %u" % num)
562-
return
563-
prev_wp = self.wploader.wp(prev)
564-
if prev_wp is None:
565-
print("Could not get previous wp %u" % prev)
566-
return
567-
prev_loc = self.get_loc(prev_wp)
568-
if prev_loc is None:
569-
print("previous wp is not a location command")
570-
return
559+
prev = num
560+
while True:
561+
prev = prev - 1
562+
if prev < 1 or prev > self.wploader.count():
563+
print("Bad item %u" % num)
564+
return
565+
prev_wp = self.wploader.wp(prev)
566+
if prev_wp is None:
567+
print("Could not get previous wp %u" % prev)
568+
return
569+
if prev_wp.command == mavutil.mavlink.MAV_CMD_DO_LAND_START:
570+
continue
571+
prev_loc = self.get_loc(prev_wp)
572+
if prev_loc is None:
573+
print("previous wp is not a location command")
574+
return
575+
break
571576

572577
if wp.frame != prev_wp.frame:
573578
print("waypoints differ in frame (%u vs %u)" %

0 commit comments

Comments
 (0)