Tools: rover: implement test_gcs_failsafe

This commit is contained in:
Pierre Kancir 2023-02-01 00:56:29 +01:00 committed by Tom Pittenger
parent 0b7ef942b4
commit 468356d513
2 changed files with 106 additions and 0 deletions

View File

@ -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

View File

@ -1719,6 +1719,107 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
if statustext_fragment in m.text: if statustext_fragment in m.text:
break 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): 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") self.start_subtest("Ensuring if we don't have a centroid it gets calculated")
items = self.test_gcs_fence_need_centroid( 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.StickMixingAuto,
self.AutoDock, self.AutoDock,
self.PrivateChannel, self.PrivateChannel,
self.GCSFailsafe,
]) ])
return ret return ret