diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index c2686c062e906..f561cf8ce8c81 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9331,6 +9331,34 @@ def GPSForYaw(self): if ex is not None: raise ex + def GPSForYawCompassLearn(self): + '''Moving baseline GPS yaw - with compass learning''' + self.context_push() + self.load_default_params_file("copter-gps-for-yaw.parm") + self.set_parameter("EK3_SRC1_YAW", 3) # GPS with compass fallback + self.reboot_sitl() + + self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) + + self.wait_ready_to_arm() + + self.takeoff(10, mode='GUIDED') + tstart = self.get_sim_time() + compass_learn_set = False + while True: + delta_t = self.get_sim_time_cached() - tstart + if delta_t > 30: + break + if not compass_learn_set and delta_t > 10: + self.set_parameter("COMPASS_LEARN", 3) + compass_learn_set = True + + self.check_attitudes_match() + self.delay_sim_time(1) + + self.context_pop() + self.reboot_sitl() + def AP_Avoidance(self): '''ADSB-based avoidance''' self.set_parameters({ @@ -10831,6 +10859,7 @@ def tests2b(self): # this block currently around 9.5mins here self.MAV_CMD_NAV_TAKEOFF_command_int, self.Ch6TuningWPSpeed, self.PILOT_THR_BHV, + self.GPSForYawCompassLearn, ]) return ret @@ -10860,6 +10889,7 @@ def disabled_tests(self): "GroundEffectCompensation_takeOffExpected": "Flapping", "GroundEffectCompensation_touchDownExpected": "Flapping", "FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561", + "GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion", } diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2787be202135e..e0382402528d0 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -11,7 +11,6 @@ import time from pymavlink import quaternion -from pymavlink import mavextra from pymavlink import mavutil from pymavlink.rotmat import Vector3 @@ -2005,70 +2004,6 @@ def run_subtest(self, desc, func): self.start_subtest(desc) func() - def check_attitudes_match(self, a, b): - '''make sure ahrs2 and simstate and ATTTIUDE_QUATERNION all match''' - - # these are ordered to bookend the list with timestamps (which - # both attitude messages have): - get_names = ['ATTITUDE', 'SIMSTATE', 'AHRS2', 'ATTITUDE_QUATERNION'] - msgs = self.get_messages_frame(get_names) - - for get_name in get_names: - self.progress("%s: %s" % (get_name, msgs[get_name])) - - simstate = msgs['SIMSTATE'] - attitude = msgs['ATTITUDE'] - ahrs2 = msgs['AHRS2'] - attitude_quaternion = msgs['ATTITUDE_QUATERNION'] - - # check ATTITUDE - want = math.degrees(simstate.roll) - got = math.degrees(attitude.roll) - if abs(mavextra.angle_diff(want, got)) > 20: - raise NotAchievedException("ATTITUDE.Roll looks bad (want=%f got=%f)" % - (want, got)) - want = math.degrees(simstate.pitch) - got = math.degrees(attitude.pitch) - if abs(mavextra.angle_diff(want, got)) > 20: - raise NotAchievedException("ATTITUDE.Pitch looks bad (want=%f got=%f)" % - (want, got)) - - # check AHRS2 - want = math.degrees(simstate.roll) - got = math.degrees(ahrs2.roll) - if abs(mavextra.angle_diff(want, got)) > 20: - raise NotAchievedException("AHRS2.Roll looks bad (want=%f got=%f)" % - (want, got)) - - want = math.degrees(simstate.pitch) - got = math.degrees(ahrs2.pitch) - if abs(mavextra.angle_diff(want, got)) > 20: - raise NotAchievedException("AHRS2.Pitch looks bad (want=%f got=%f)" % - (want, got)) - - # check ATTITUDE_QUATERNION - q = quaternion.Quaternion([ - attitude_quaternion.q1, - attitude_quaternion.q2, - attitude_quaternion.q3, - attitude_quaternion.q4 - ]) - euler = q.euler - self.progress("attquat:%s q:%s euler:%s" % ( - str(attitude_quaternion), q, euler)) - - want = math.degrees(simstate.roll) - got = math.degrees(euler[0]) - if mavextra.angle_diff(want, got) > 20: - raise NotAchievedException("quat roll differs from attitude roll; want=%f got=%f" % - (want, got)) - - want = math.degrees(simstate.pitch) - got = math.degrees(euler[1]) - if mavextra.angle_diff(want, got) > 20: - raise NotAchievedException("quat pitch differs from attitude pitch; want=%f got=%f" % - (want, got)) - def fly_ahrs2_test(self): '''check secondary estimator is looking OK''' @@ -2089,7 +2024,7 @@ def fly_ahrs2_test(self): if self.get_distance_int(gpi, ahrs2) > 10: raise NotAchievedException("Secondary location looks bad") - self.check_attitudes_match(1, 2) + self.check_attitudes_match() def MainFlight(self): '''Lots of things in one flight''' diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 848179176f985..d223290addb1e 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -13675,6 +13675,70 @@ def assert_gps_satellite_count(self, messagename, count): raise NotAchievedException("Expected %u sats, got %u" % (count, m.satellites_visible)) + def check_attitudes_match(self): + '''make sure ahrs2 and simstate and ATTTIUDE_QUATERNION all match''' + + # these are ordered to bookend the list with timestamps (which + # both attitude messages have): + get_names = ['ATTITUDE', 'SIMSTATE', 'AHRS2', 'ATTITUDE_QUATERNION'] + msgs = self.get_messages_frame(get_names) + + for get_name in get_names: + self.progress("%s: %s" % (get_name, msgs[get_name])) + + simstate = msgs['SIMSTATE'] + attitude = msgs['ATTITUDE'] + ahrs2 = msgs['AHRS2'] + attitude_quaternion = msgs['ATTITUDE_QUATERNION'] + + # check ATTITUDE + want = math.degrees(simstate.roll) + got = math.degrees(attitude.roll) + if abs(mavextra.angle_diff(want, got)) > 20: + raise NotAchievedException("ATTITUDE.Roll looks bad (want=%f got=%f)" % + (want, got)) + want = math.degrees(simstate.pitch) + got = math.degrees(attitude.pitch) + if abs(mavextra.angle_diff(want, got)) > 20: + raise NotAchievedException("ATTITUDE.Pitch looks bad (want=%f got=%f)" % + (want, got)) + + # check AHRS2 + want = math.degrees(simstate.roll) + got = math.degrees(ahrs2.roll) + if abs(mavextra.angle_diff(want, got)) > 20: + raise NotAchievedException("AHRS2.Roll looks bad (want=%f got=%f)" % + (want, got)) + + want = math.degrees(simstate.pitch) + got = math.degrees(ahrs2.pitch) + if abs(mavextra.angle_diff(want, got)) > 20: + raise NotAchievedException("AHRS2.Pitch looks bad (want=%f got=%f)" % + (want, got)) + + # check ATTITUDE_QUATERNION + q = quaternion.Quaternion([ + attitude_quaternion.q1, + attitude_quaternion.q2, + attitude_quaternion.q3, + attitude_quaternion.q4 + ]) + euler = q.euler + self.progress("attquat:%s q:%s euler:%s" % ( + str(attitude_quaternion), q, euler)) + + want = math.degrees(simstate.roll) + got = math.degrees(euler[0]) + if mavextra.angle_diff(want, got) > 20: + raise NotAchievedException("quat roll differs from attitude roll; want=%f got=%f" % + (want, got)) + + want = math.degrees(simstate.pitch) + got = math.degrees(euler[1]) + if mavextra.angle_diff(want, got) > 20: + raise NotAchievedException("quat pitch differs from attitude pitch; want=%f got=%f" % + (want, got)) + def MultipleGPS(self): '''check ArduPilot behaviour across multiple GPS units''' self.assert_message_rate_hz('GPS2_RAW', 0)