From 4b4ed67a339958014a4b818d26e13b53815fde60 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 25 Mar 2024 22:17:30 -0600 Subject: [PATCH] Tools: Autotest: Test for EAHRS misconfiguration prearm failure * And test for single GPS reporting on Microstrain7 Signed-off-by: Ryan Friedman --- Tools/autotest/arduplane.py | 52 ++++++++++++++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2787be202135e..51218ab3000ec 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3169,7 +3169,7 @@ def TerrainLoiter(self): self.fly_home_land_and_disarm(240) def fly_external_AHRS(self, sim, eahrs_type, mission): - """Fly with external AHRS (VectorNav)""" + """Fly with external AHRS""" self.customise_SITL_commandline(["--serial4=sim:%s" % sim]) self.set_parameters({ @@ -3293,6 +3293,55 @@ def InertialLabsEAHRS(self): '''Test InertialLabs EAHRS support''' self.fly_external_AHRS("ILabs", 5, "ap1.txt") + def GpsSensorPreArmEAHRS(self): + '''Test pre-arm checks related to EAHRS_SENSORS using the MicroStrain7 driver''' + self.customise_SITL_commandline(["--serial4=sim:MicroStrain7"]) + + self.set_parameters({ + "EAHRS_TYPE": 7, + "SERIAL4_PROTOCOL": 36, + "SERIAL4_BAUD": 230400, + "GPS1_TYPE": 0, # Disabled (simulate user setup error) + "GPS2_TYPE": 0, # Disabled (simulate user setup error) + "AHRS_EKF_TYPE": 11, + "INS_GYR_CAL": 1, + "EAHRS_SENSORS": 13, # GPS is enabled + }) + self.reboot_sitl() + self.delay_sim_time(5) + self.progress("Running accelcal") + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) + + self.assert_prearm_failure("ExternalAHRS: Incorrect number", # Cut short due to message limits. + timeout=30, + other_prearm_failures_fatal=False) + + self.set_parameters({ + "EAHRS_TYPE": 7, + "SERIAL4_PROTOCOL": 36, + "SERIAL4_BAUD": 230400, + "GPS1_TYPE": 1, # Auto + "GPS2_TYPE": 21, # EARHS + "AHRS_EKF_TYPE": 11, + "INS_GYR_CAL": 1, + "EAHRS_SENSORS": 13, # GPS is enabled + }) + self.reboot_sitl() + self.delay_sim_time(5) + self.progress("Running accelcal") + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) + # Check prearm success with MicroStrain when the first GPS is occupied by another GPS. + # This supports the use case of comparing MicroStrain dual antenna to another GPS. + self.wait_ready_to_arm() + def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -5408,6 +5457,7 @@ def tests(self): self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, self.InertialLabsEAHRS, + self.GpsSensorPreArmEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch,