mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
autotest: add test for compass_learn=3 when using gps-for-yaw
This commit is contained in:
Peter Barker
Peter Barker
parent
563d31b1ea
commit
c578a18b7f
@ -9331,6 +9331,34 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
||||
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 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
||||
self.MAV_CMD_NAV_TAKEOFF_command_int,
|
||||
self.Ch6TuningWPSpeed,
|
||||
self.PILOT_THR_BHV,
|
||||
self.GPSForYawCompassLearn,
|
||||
])
|
||||
return ret
|
||||
|
||||
@ -10860,6 +10889,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
||||
"GroundEffectCompensation_takeOffExpected": "Flapping",
|
||||
"GroundEffectCompensation_touchDownExpected": "Flapping",
|
||||
"FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561",
|
||||
"GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion",
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -11,7 +11,6 @@ import signal
|
||||
import time
|
||||
|
||||
from pymavlink import quaternion
|
||||
from pymavlink import mavextra
|
||||
from pymavlink import mavutil
|
||||
|
||||
from pymavlink.rotmat import Vector3
|
||||
@ -2005,70 +2004,6 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
|
||||
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 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
|
||||
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'''
|
||||
|
||||
@ -13675,6 +13675,70 @@ switch value'''
|
||||
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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user