Tools: autotest: add test for Copter onboard mag calibration

This commit is contained in:
Peter Barker 2019-04-11 11:53:35 +10:00 committed by Andrew Tridgell
parent aae9dc55e3
commit 27f642014c
1 changed files with 74 additions and 0 deletions

View File

@ -2846,6 +2846,76 @@ class AutoTestCopter(AutoTest):
(tdelta, max_good_tdelta))
self.progress("Vehicle returned")
def test_onboard_compass_calibration(self, timeout=240):
twist_x = 2.1
twist_y = 2.2
twist_z = 2.3
ex = None
self.context_push()
try:
self.set_parameter("SIM_GND_BEHAV", 0)
self.set_parameter("AHRS_EKF_TYPE", 10)
self.reboot_sitl()
report = self.mav.messages.get("MAG_CAL_REPORT", None)
if report is not None:
raise PreconditionFailedException("MAG_CAL_REPORT found")
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL,
1, # bitmask of compasses to calibrate
0,
0,
0,
0,
0,
0,
timeout=1)
tstart = self.get_sim_time()
last_twist_time = 0
while True:
now = self.get_sim_time_cached()
if now - tstart > timeout:
raise NotAchievedException("timeout before cal complete")
report = self.mav.messages.get("MAG_CAL_REPORT", None)
if report is not None:
print("Report: %s" % str(report))
break
if now - last_twist_time > 5:
last_twist_time = now
twist_x *= 1.1
twist_y *= 1.2
twist_z *= 1.3
if abs(twist_x) > 10:
twist_x /= -2
if abs(twist_y) > 10:
twist_y /= -2
if abs(twist_z) > 10:
twist_z /= -2
self.set_parameter("SIM_TWIST_X", twist_x)
self.set_parameter("SIM_TWIST_Y", twist_y)
self.set_parameter("SIM_TWIST_Z", twist_z)
try:
self.set_parameter("SIM_TWIST_TIME", 100)
except ValueError as e:
# the shove resets this to zero
pass
m = self.mav.recv_match(type="MAG_CAL_PROGRESS", timeout=1)
self.progress("progress: %s" % str(m))
if m is None:
continue
att = self.mav.messages.get("ATTITUDE", None)
self.progress("Attitude: %f %f %f" %
(math.degrees(att.roll), math.degrees(att.pitch), math.degrees(att.yaw)))
except Exception as e:
print("Exception caught: %s" % str(e))
ex = e
self.context_pop()
self.reboot_sitl()
if ex is not None:
raise ex
def fly_brake_mode(self):
# test brake mode
self.progress("Testing brake mode")
@ -3237,6 +3307,10 @@ class AutoTestCopter(AutoTest):
"Fly POSHOLD takeoff",
self.fly_poshold_takeoff),
("OnboardCompassCalibration",
"Test onboard compass calibration",
self.test_onboard_compass_calibration),
("LogDownLoad",
"Log download",
lambda: self.log_download(