From 27f642014c3af7c62cd654faa9cba38cbadae672 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 11 Apr 2019 11:53:35 +1000
Subject: [PATCH] Tools: autotest: add test for Copter onboard mag calibration

---
 Tools/autotest/arducopter.py | 74 ++++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 7ea92c7b7c..a173d77f17 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -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(