From 5c952285e329dc86445f39a4f3b19c416364f758 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 19 Aug 2024 20:49:30 +1000 Subject: [PATCH] autotest: augment ClearMission to test clearing from current uploader in the same way that we allow a link to re-start an upload by sending mission_count, allow a GCS to clear a mission and cancel current upload if it was the one doing the transfer --- Tools/autotest/rover.py | 45 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 03de9268c4f1b9..65041324a94330 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -3617,6 +3617,51 @@ def ClearMission(self, target_system=1, target_component=1): self.assert_current_waypoint(0) + self.drain_mav() + + self.start_subtest("No clear mission while it is being uploaded by a different node") + mav2 = mavutil.mavlink_connection("tcp:localhost:5763", + robust_parsing=True, + source_system=7, + source_component=7) + self.context_push() + self.context_collect("MISSION_REQUEST") + mav2.mav.mission_count_send(target_system, + target_component, + 17, + mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + ack = self.assert_receive_message('MISSION_REQUEST', check_context=True, mav=mav2) + self.context_pop() + + self.context_push() + self.context_collect("MISSION_ACK") + self.mav.mav.mission_clear_all_send( + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_MISSION + ) + ack = self.assert_receive_message('MISSION_ACK', check_context=True) + self.assert_message_field_values(ack, { + "type": mavutil.mavlink.MAV_MISSION_DENIED, + }) + self.context_pop() + + self.progress("Test cancel upload from second connection") + self.context_push() + self.context_collect("MISSION_ACK") + mav2.mav.mission_clear_all_send( + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_MISSION + ) + ack = self.assert_receive_message('MISSION_ACK', mav=mav2, check_context=True) + self.assert_message_field_values(ack, { + "type": mavutil.mavlink.MAV_MISSION_ACCEPTED, + }) + self.context_pop() + mav2.close() + del mav2 + def GCSMission(self): '''check MAVProxy's waypoint handling of missions'''