From fb5a482113f61ec7664f64b743b1e6ba39ff05e2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 30 Sep 2024 12:40:00 +1000 Subject: [PATCH] mavproxy_wp: add option to reset mission counters when setting waypoint e.g "wp set 2 reset" --- MAVProxy/modules/mavproxy_wp.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/MAVProxy/modules/mavproxy_wp.py b/MAVProxy/modules/mavproxy_wp.py index b894ae5459..cc28b7ee76 100644 --- a/MAVProxy/modules/mavproxy_wp.py +++ b/MAVProxy/modules/mavproxy_wp.py @@ -307,8 +307,15 @@ def cmd_editor(self, args): self.mpstate.functions.process_stdin("module load misseditor", immediate=True) def cmd_set(self, args): - if len(args) != 1: - print("usage: wp set ") + usage = "usage: wp set [reset]" + reset = False + if len(args) == 2: + if args[1] != "reset": + print(usage) + return + reset = True + elif len(args) != 1: + print(usage) return wp_num = int(args[0]) @@ -324,14 +331,20 @@ def cmd_set(self, args): # we "know" because we hook receipt of COMMAND_ACK. if self.settings.wp_use_waypoint_set_current or supports is False: + if reset: + print("Reset unavailable, wp command NOT executed") + return self.master.waypoint_set_current_send(wp_num) else: + p2 = 0 + if reset: + p2 = 1 self.master.mav.command_long_send( self.target_system, self.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, 0, - wp_num, 0, 0, 0, 0, 0, 0 + wp_num, p2, 0, 0, 0, 0, 0 ) def cmd_add(self, args):