Tools: rover: implement test_gcs_failsafe

Tools/autotest/ArduRover_Tests/GCSFailsafe/test_arming.txt

@@ -0,0 +1,4 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	-0.110000	1
+1	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+2	0	3	17	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1

Tools/autotest/rover.py

@@ -1719,6 +1719,107 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             if statustext_fragment in m.text:
                 break
 
+    def GCSFailsafe(self, side=60, timeout=360):
+        """Test GCS Failsafe"""
+        try:
+            self.test_gcs_failsafe(side=side, timeout=timeout)
+        except Exception as ex:
+            self.setGCSfailsafe(0)
+            self.set_parameter('FS_ACTION', 0)
+            self.disarm_vehicle(force=True)
+            self.reboot_sitl()
+            raise ex
+
+    def test_gcs_failsafe(self, side=60, timeout=360):
+        self.set_parameter("SYSID_MYGCS", self.mav.source_system)
+        self.set_parameter("FS_ACTION", 1)
+        self.set_parameter("FS_THR_ENABLE", 0)  # disable radio FS as it inhibt GCS one's
+
+        def go_somewhere():
+            self.change_mode("MANUAL")
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            self.set_rc(3, 2000)
+            self.delay_sim_time(5)
+            self.set_rc(3, 1500)
+        # Trigger telemetry loss with failsafe disabled. Verify no action taken.
+        self.start_subtest("GCS failsafe disabled test: FS_GCS_ENABLE=0 should take no failsafe action")
+        self.setGCSfailsafe(0)
+        go_somewhere()
+        self.set_heartbeat_rate(0)
+        self.delay_sim_time(5)
+        self.wait_mode("MANUAL")
+        self.set_heartbeat_rate(self.speedup)
+        self.delay_sim_time(5)
+        self.wait_mode("MANUAL")
+        self.end_subtest("Completed GCS failsafe disabled test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify
+        # failsafe triggers to RTL. Restore telemetry, verify failsafe
+        # clears, and change modes.
+        # TODO not implemented
+        # self.start_subtest("GCS failsafe recovery test: FS_GCS_ENABLE=1")
+        # self.setGCSfailsafe(1)
+        # self.set_heartbeat_rate(0)
+        # self.wait_mode("RTL")
+        # self.set_heartbeat_rate(self.speedup)
+        # self.wait_statustext("GCS Failsafe Cleared", timeout=60)
+        # self.change_mode("MANUAL")
+        # self.end_subtest("Completed GCS failsafe recovery test")
+
+        # Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and RTL completes
+        self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1")
+        self.setGCSfailsafe(1)
+        self.set_heartbeat_rate(0)
+        self.wait_mode("RTL")
+        self.wait_statustext("Reached destination", timeout=60)
+        self.set_heartbeat_rate(self.speedup)
+        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
+        self.end_subtest("Completed GCS failsafe RTL")
+
+        # Trigger telemetry loss with an invalid failsafe value. Verify failsafe triggers and RTL completes
+        self.start_subtest("GCS failsafe invalid value with no options test: FS_GCS_ENABLE=99")
+        self.setGCSfailsafe(99)
+        go_somewhere()
+        self.set_heartbeat_rate(0)
+        self.wait_mode("RTL")
+        self.wait_statustext("Reached destination", timeout=60)
+        self.set_heartbeat_rate(self.speedup)
+        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
+        self.end_subtest("Completed GCS failsafe invalid value")
+
+        self.start_subtest("Testing continue in auto mission")
+        self.disarm_vehicle()
+        self.setGCSfailsafe(2)
+        self.load_mission("test_arming.txt")
+        self.change_mode("AUTO")
+        self.delay_sim_time(5)
+        self.set_heartbeat_rate(0)
+        self.wait_statustext("Failsafe - Continuing Auto Mode", timeout=60)
+        self.delay_sim_time(5)
+        self.wait_mode("AUTO")
+        self.set_heartbeat_rate(self.speedup)
+        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
+
+        self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1 and FS_GCS_TIMEOUT=10")
+        self.setGCSfailsafe(1)
+        old_gcs_timeout = self.get_parameter("FS_GCS_TIMEOUT")
+        new_gcs_timeout = old_gcs_timeout * 2
+        self.set_parameter("FS_GCS_TIMEOUT", new_gcs_timeout)
+        go_somewhere()
+        self.set_heartbeat_rate(0)
+        self.delay_sim_time(old_gcs_timeout + (new_gcs_timeout - old_gcs_timeout) / 2)
+        self.assert_mode("MANUAL")
+        self.wait_mode("RTL")
+        self.wait_statustext("Reached destination", timeout=60)
+        self.set_heartbeat_rate(self.speedup)
+        self.wait_statustext("GCS Failsafe Cleared", timeout=60)
+        self.disarm_vehicle()
+        self.end_subtest("Completed GCS failsafe RTL")
+
+        self.setGCSfailsafe(0)
+        self.progress("All GCS failsafe tests complete")
+
     def test_gcs_fence_centroid(self, target_system=1, target_component=1):
         self.start_subtest("Ensuring if we don't have a centroid it gets calculated")
         items = self.test_gcs_fence_need_centroid(
@@ -6214,6 +6315,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             self.StickMixingAuto,
             self.AutoDock,
             self.PrivateChannel,
+            self.GCSFailsafe,
         ])
         return ret