Tools: autotest: add a test for SYSID_ENFORCE

Tools/autotest/apmrover2.py

@@ -796,6 +796,45 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.mavproxy_do_set_mode_via_command_long("HOLD")
         self.mavproxy_do_set_mode_via_command_long("MANUAL")
 
+    def test_sysid_enforce(self):
+        '''Run the same arming code with correct then incorrect SYSID'''
+        self.context_push()
+        ex = None
+        try:
+            # if set_parameter is ever changed to not use MAVProxy
+            # this test is going to break horribly.  Sorry.
+            self.set_parameter("SYSID_MYGCS", 255) # assume MAVProxy does this!
+            self.set_parameter("SYSID_ENFORCE", 1) # assume MAVProxy does this!
+
+            self.change_mode('MANUAL')
+
+            self.progress("make sure I can arm ATM")
+            self.wait_ready_to_arm()
+            self.arm_vehicle(timeout=5)
+            self.disarm_vehicle()
+
+            # temporarily set a different system ID than MAVProxy:
+            self.progress("Attempting to arm vehicle myself")
+            old_srcSystem = self.mav.mav.srcSystem
+            try:
+                self.mav.mav.srcSystem = 243
+                self.arm_vehicle(timeout=5)
+                self.disarm_vehicle()
+                success = False
+            except NotAchievedException as e:
+                success = True
+                pass
+            self.mav.srcSystem = old_srcSystem
+            if not success:
+                raise NotAchievedException(
+                    "Managed to arm with SYSID_ENFORCE set")
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+        self.context_pop()
+        if ex is not None:
+            raise ex
+
     def autotest(self):
         """Autotest APMrover2 in SITL."""
         self.check_test_syntax(test_file=os.path.realpath(__file__))
@@ -872,6 +911,8 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             self.run_test("Test MAV_CMD_SET_MESSAGE_INTERVAL",
                           self.test_set_message_interval)
 
+            self.run_test("SYSID_ENFORCE", self.test_sysid_enforce)
+
             self.run_test("Download logs", lambda:
                           self.log_download(
                               self.buildlogs_path("APMrover2-log.bin"),

Tools/autotest/common.py

@@ -633,7 +633,7 @@ class AutoTest(ABC):
                      0,
                      0,
                      0,
-                     )
+                     timeout=timeout)
         tstart = self.get_sim_time()
         while self.get_sim_time() - tstart < timeout:
             self.wait_heartbeat()
@@ -653,7 +653,7 @@ class AutoTest(ABC):
                      0,
                      0,
                      0,
-                     )
+                     timeout=timeout)
         tstart = self.get_sim_time()
         while self.get_sim_time() - tstart < timeout:
             self.wait_heartbeat()
@@ -832,7 +832,8 @@ class AutoTest(ABC):
                 p5,
                 p6,
                 p7,
-                want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
+                want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED,
+                timeout=10):
         """Send a MAVLink command long."""
         self.mav.mav.command_long_send(1,
                                        1,
@@ -845,8 +846,15 @@ class AutoTest(ABC):
                                        p5,
                                        p6,
                                        p7)
+        tstart = self.get_sim_time_cached()
         while True:
-            m = self.mav.recv_match(type='COMMAND_ACK', blocking=True)
+            if self.get_sim_time_cached() - tstart > timeout:
+                raise NotAchievedException("Did not get good COMMAND_ACK")
+            m = self.mav.recv_match(type='COMMAND_ACK',
+                                    blocking=True,
+                                    timeout=1)
+            if m is None:
+                continue
             self.progress("ACK received: %s" % str(m))
             if m.command == command:
                 if m.result != want_result: