From a405c674520b51415e64bd4c7ef9f771c6c6de25 Mon Sep 17 00:00:00 2001 From: Peter Barker <pbarker@barker.dropbear.id.au> Date: Mon, 27 Jan 2025 20:19:51 +1100 Subject: [PATCH] autotest: add a test for in-flight compass learning --- Tools/autotest/arduplane.py | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 172a4a1c904d6..8c6721d8162e4 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -6445,6 +6445,28 @@ def ForceArm(self): ) self.disarm_vehicle() + def CompassLearnInFlight(self): + '''check we can learn compass offsets in flight''' + self.context_push() + self.set_parameters({ + "COMPASS_OFS_X": 1100, + }) + self.assert_prearm_failure("Check mag field", other_prearm_failures_fatal=False) + self.context_pop() + self.wait_ready_to_arm() + self.takeoff(30, mode='TAKEOFF') + self.assert_parameter_value("COMPASS_OFS_X", 20, epsilon=30) + old_compass_ofs_x = self.get_parameter('COMPASS_OFS_X') + self.set_parameters({ + "COMPASS_OFS_X": 1100, + }) + self.set_parameter("COMPASS_LEARN", 3) # 3 is in-flight learning + self.wait_parameter_value("COMPASS_LEARN", 0) + self.assert_parameter_value("COMPASS_OFS_X", old_compass_ofs_x, epsilon=30) + self.fly_home_land_and_disarm() + self.reboot_sitl() + self.assert_parameter_value("COMPASS_OFS_X", old_compass_ofs_x, epsilon=30) + def _MAV_CMD_EXTERNAL_WIND_ESTIMATE(self, command): self.reboot_sitl() @@ -6839,6 +6861,7 @@ def tests1b(self): self.MAV_CMD_DO_LAND_START, self.MAV_CMD_NAV_ALTITUDE_WAIT, self.InteractTest, + self.CompassLearnInFlight, self.MAV_CMD_MISSION_START, self.TerrainRally, self.MAV_CMD_NAV_LOITER_UNLIM,