From c77e763eca48a7d81ddc11fd8b60c0b3f011fdbb Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Mon, 9 May 2016 18:32:35 -0300
Subject: [PATCH] Tools: sitl_calibration: add sitl_magcal command

---
 Tools/mavproxy_modules/sitl_calibration.py | 105 +++++++++++++++++++++
 1 file changed, 105 insertions(+)

diff --git a/Tools/mavproxy_modules/sitl_calibration.py b/Tools/mavproxy_modules/sitl_calibration.py
index f666e368ef..417b44cf6e 100644
--- a/Tools/mavproxy_modules/sitl_calibration.py
+++ b/Tools/mavproxy_modules/sitl_calibration.py
@@ -17,6 +17,8 @@
 from __future__ import division, print_function
 import math
 from pymavlink import quaternion
+import random
+import time
 
 from MAVProxy.modules.lib import mp_module
 
@@ -237,6 +239,100 @@ class AccelcalController(CalController):
                 )
 
 
+class MagcalController(CalController):
+    yaw_increment = math.radians(45)
+    yaw_noise_range = math.radians(5)
+
+    rotation_angspeed = math.pi / 4
+    '''rotation angular speed in rad/s'''
+    rotation_angspeed_noise = math.radians(2)
+    rotation_axes = (
+        (1, 0, 0),
+        (0, 1, 0),
+        (1, 1, 0),
+    )
+
+    full_turn_time = 2 * math.pi / rotation_angspeed
+
+    max_full_turns = 3
+    '''maximum number of full turns to be performed for each initial attitude'''
+
+    def reset(self):
+        super(MagcalController, self).reset()
+        self.yaw = 0
+        self.rotation_start_time = 0
+        self.last_progress = {}
+        self.rotation_axis_idx = 0
+
+    def start(self):
+        super(MagcalController, self).start()
+        self.set_attitute(0, 0, 0, callback=self.next_rot_att_callback)
+
+    def next_rot_att_callback(self):
+        x, y, z = self.rotation_axes[self.rotation_axis_idx]
+        angspeed = self.rotation_angspeed
+        angspeed += random.uniform(-1, 1) * self.rotation_angspeed_noise
+        self.angvel(x, y, z, angspeed)
+        self.rotation_start_time = time.time()
+
+    def next_rotation(self):
+        self.rotation_axis_idx += 1
+        self.rotation_axis_idx %= len(self.rotation_axes)
+
+        if self.rotation_axis_idx == 0:
+            yaw_inc = self.yaw_increment
+            yaw_inc += random.uniform(-1, 1) * self.yaw_noise_range
+            self.yaw = (self.yaw + yaw_inc) % (2 * math.pi)
+
+        self.rotation_start_time = 0
+        self.last_progress = {}
+        self.set_attitute(0, 0, self.yaw, callback=self.next_rot_att_callback)
+
+    def mavlink_packet(self, m):
+        super(MagcalController, self).mavlink_packet(m)
+
+        if m.get_type() == 'MAG_CAL_REPORT':
+            # NOTE: This may be not the ideal way to handle it
+            if m.compass_id in self.last_progress:
+                del self.last_progress[m.compass_id]
+            if not self.last_progress:
+                self.stop()
+            return
+
+        if m.get_type() != 'MAG_CAL_PROGRESS':
+            return
+
+        if not self.rotation_start_time:
+            return
+
+        t = time.time()
+        m.time = t
+
+        if m.compass_id not in self.last_progress:
+            self.last_progress[m.compass_id] = m
+            m.stuck = False
+            return
+
+        last = self.last_progress[m.compass_id]
+
+        dt = t - self.rotation_start_time
+        if dt > self.max_full_turns * self.full_turn_time:
+            self.next_rotation()
+            return
+
+        if m.completion_pct == last.completion_pct:
+            if m.time - last.time > self.full_turn_time / 2:
+                last.stuck = True
+        else:
+            self.last_progress[m.compass_id] = m
+            m.stuck = False
+
+        for p in self.last_progress.values():
+            if not p.stuck:
+                break
+        else:
+            self.next_rotation()
+
 
 class SitlCalibrationModule(mp_module.MPModule):
     def __init__(self, mpstate):
@@ -258,6 +354,11 @@ class SitlCalibrationModule(mp_module.MPModule):
             self.cmd_sitl_accelcal,
             'actuate on the simulator vehicle for accelerometer calibration',
         )
+        self.add_command(
+            'sitl_magcal',
+            self.cmd_sitl_magcal,
+            'actuate on the simulator vehicle for magnetometer calibration',
+        )
         self.add_command(
             'sitl_stop',
             self.cmd_sitl_stop,
@@ -267,6 +368,7 @@ class SitlCalibrationModule(mp_module.MPModule):
         self.controllers = dict(
             generic_controller=CalController(mpstate),
             accelcal_controller=AccelcalController(mpstate),
+            magcal_controller=MagcalController(mpstate),
         )
 
         self.current_controller = None
@@ -319,6 +421,9 @@ class SitlCalibrationModule(mp_module.MPModule):
     def cmd_sitl_accelcal(self, args):
         self.set_controller('accelcal_controller')
 
+    def cmd_sitl_magcal(self, args):
+        self.set_controller('magcal_controller')
+
     def mavlink_packet(self, m):
         for c in self.controllers.values():
             c.mavlink_packet(m)