Tools: Autotest: Test for EAHRS misconfiguration prearm failure

* And test for single GPS reporting on Microstrain7

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-03-25 22:17:30 -06:00 committed by Andrew Tridgell
parent b81a5deefe
commit 2c803e365e
1 changed files with 51 additions and 1 deletions

View File

@ -3104,7 +3104,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
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({
@ -3228,6 +3228,55 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
'''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
@ -5345,6 +5394,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.MicroStrainEAHRS5,
self.MicroStrainEAHRS7,
self.InertialLabsEAHRS,
self.GpsSensorPreArmEAHRS,
self.Deadreckoning,
self.DeadreckoningNoAirSpeed,
self.EKFlaneswitch,