From 1a84591eb7e8115d74cc8649b52e603f492f0317 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 20 Jun 2024 19:51:06 -0300 Subject: [PATCH] autotest: test mag fusion and origin backup Co-authored-by: Clyde McQueen --- Tools/autotest/ardusub.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 44beefaed3..9dba24991e 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -645,6 +645,37 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.disarm_vehicle() self.progress("Mission OK") + def backup_home(self): + """Test ORIGIN_LAT and ORIGIN_LON parameters""" + + self.context_push() + self.set_parameters({ + 'GPS1_TYPE': 0, # Disable GPS + 'EK3_SRC1_POSXY': 0, # Make sure EK3_SRC parameters do not refer to GPS + 'EK3_SRC1_VELXY': 0, # Make sure EK3_SRC parameters do not refer to GPS + 'ORIGIN_LAT': 47.607584, + 'ORIGIN_LON': -122.343911, + }) + self.reboot_sitl() + self.context_collect('STATUSTEXT') + + # Wait for the EKF to be happy in constant position mode + self.wait_ready_to_arm_const_pos() + + if self.current_onboard_log_contains_message('ORGN'): + raise NotAchievedException("Found unexpected ORGN message") + + # This should set the origin and write a record to ORGN + self.arm_vehicle() + + self.wait_statustext('Using backup location', check_context=True) + + if not self.current_onboard_log_contains_message('ORGN'): + raise NotAchievedException("Did not find expected ORGN message") + + self.disarm_vehicle() + self.context_pop() + def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -669,6 +700,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.MAV_CMD_DO_CHANGE_SPEED, self.MAV_CMD_CONDITION_YAW, self.TerrainMission, + self.backup_home, ]) return ret