From 468356d51340db6aae8738ea3f7ff0a06d4b0488 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Wed, 1 Feb 2023 00:56:29 +0100 Subject: [PATCH] Tools: rover: implement test_gcs_failsafe --- .../GCSFailsafe/test_arming.txt | 4 + Tools/autotest/rover.py | 102 ++++++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 Tools/autotest/ArduRover_Tests/GCSFailsafe/test_arming.txt diff --git a/Tools/autotest/ArduRover_Tests/GCSFailsafe/test_arming.txt b/Tools/autotest/ArduRover_Tests/GCSFailsafe/test_arming.txt new file mode 100644 index 0000000000..d4efe03d78 --- /dev/null +++ b/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 diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 3a6f89e449..77db59b146 100644 --- a/Tools/autotest/rover.py +++ b/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