diff --git a/Tools/mavproxy_modules/README.md b/Tools/mavproxy_modules/README.md
new file mode 100644
index 0000000000..eb2a60709f
--- /dev/null
+++ b/Tools/mavproxy_modules/README.md
@@ -0,0 +1,33 @@
+# MAVProxy modules #
+
+This folder contains modules for MAVProxy specifically for ArduPilot. Add the
+path to this folder to your `PYTHONPATH` in order to use it.
+
+# Modules #
+
+## `sitl_calibration` ##
+This module interfaces with the `calibration` model of SITL. It provides
+commands to actuate on the vehicle's rotation to simulate a calibration
+process.
+
+### Accelerometer Calibration ###
+The command `sitl_accelcal` listens to the accelerometer calibration status
+texts and set the vehicle in the desired attitude. Example:
+```
+accelcal
+sitl_accelcal
+```
+
+### Compass Calibration ###
+The command `sitl_magcal` applies angular velocity on the vehicle in order to
+get the compasses calibrated. Example:
+```
+magcal start
+sitl_magcal
+```
+
+### Other commands ###
+There are other commands you can use with this module:
+ - `sitl_attitude`: set vehicle at a desired attitude
+ - `sitl_angvel`: apply angular velocity on the vehicle
+ - `sitl_stop`: stop any of this module's currently active command
diff --git a/Tools/mavproxy_modules/sitl_calibration.py b/Tools/mavproxy_modules/sitl_calibration.py
new file mode 100644
index 0000000000..4d65a776fc
--- /dev/null
+++ b/Tools/mavproxy_modules/sitl_calibration.py
@@ -0,0 +1,201 @@
+# Copyright (C) 2015-2016 Intel Corporation. All rights reserved.
+#
+# This file is free software: you can redistribute it and/or modify it
+# under the terms of the GNU General Public License as published by the
+# Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This file is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program. If not, see .
+'''calibration simulation command handling'''
+
+from __future__ import division, print_function
+import math
+from pymavlink import quaternion
+
+from MAVProxy.modules.lib import mp_module
+
+class CalController(object):
+ def __init__(self, mpstate):
+ self.mpstate = mpstate
+ self.active = False
+ self.reset()
+
+ def reset(self):
+ self.desired_quaternion = None
+ self.general_state = 'idle'
+ self.attitude_callback = None
+ self.desired_quaternion_close_count = 0
+
+ def start(self):
+ self.active = True
+
+ def stop(self):
+ self.reset()
+ self.mpstate.functions.process_stdin('servo set 5 1000')
+ self.active = False
+
+ def normalize_attitude_angle(self, angle):
+ if angle < 0:
+ angle = 2 * math.pi + angle % (-2 * math.pi)
+ angle %= 2 * math.pi
+ if angle > math.pi:
+ return angle % -math.pi
+ return angle
+
+ def set_attitute(self, roll, pitch, yaw, callback=None):
+ roll = self.normalize_attitude_angle(roll)
+ pitch = self.normalize_attitude_angle(pitch)
+ yaw = self.normalize_attitude_angle(yaw)
+
+ self.desired_quaternion = quaternion.Quaternion((roll, pitch, yaw))
+ self.desired_quaternion.normalize()
+
+ scale = 500.0 / math.pi
+
+ roll_pwm = 1500 + int(roll * scale)
+ pitch_pwm = 1500 + int(pitch * scale)
+ yaw_pwm = 1500 + int(yaw * scale)
+
+ self.mpstate.functions.process_stdin('servo set 5 1150')
+ self.mpstate.functions.process_stdin('servo set 6 %d' % roll_pwm)
+ self.mpstate.functions.process_stdin('servo set 7 %d' % pitch_pwm)
+ self.mpstate.functions.process_stdin('servo set 8 %d' % yaw_pwm)
+
+ self.general_state = 'attitude'
+ self.desired_quaternion_close_count = 0
+
+ if callback:
+ self.attitude_callback = callback
+
+ def angvel(self, x, y, z, theta):
+ m = max(abs(x), abs(y), abs(z))
+ if not m:
+ x_pwm = y_pwm = z_pwm = 1500
+ else:
+ x_pwm = 1500 + round((x / m) * 500)
+ y_pwm = 1500 + round((y / m) * 500)
+ z_pwm = 1500 + round((z / m) * 500)
+
+ max_theta = 2 * math.pi
+ if theta < 0:
+ theta = 0
+ elif theta > max_theta:
+ theta = max_theta
+ theta_pwm = 1200 + round((theta / max_theta) * 800)
+
+ self.mpstate.functions.process_stdin('servo set 5 %d' % theta_pwm)
+ self.mpstate.functions.process_stdin('servo set 6 %d' % x_pwm)
+ self.mpstate.functions.process_stdin('servo set 7 %d' % y_pwm)
+ self.mpstate.functions.process_stdin('servo set 8 %d' % z_pwm)
+
+ self.general_state = 'angvel'
+
+ def handle_simstate(self, m):
+ if self.general_state == 'attitude':
+ q = quaternion.Quaternion((m.roll, m.pitch, m.yaw))
+ q.normalize()
+ d1 = abs(self.desired_quaternion.q - q.q)
+ d2 = abs(self.desired_quaternion.q + q.q)
+ if (d1 <= 1e-2).all() or (d2 <= 1e-2).all():
+ self.desired_quaternion_close_count += 1
+ else:
+ self.desired_quaternion_close_count = 0
+
+ if self.desired_quaternion_close_count == 5:
+ self.general_state = 'idle'
+ if callable(self.attitude_callback):
+ self.attitude_callback()
+ self.attitude_callback = None
+
+ def mavlink_packet(self, m):
+ if not self.active:
+ return
+ if m.get_type() == 'SIMSTATE':
+ self.handle_simstate(m)
+
+
+class SitlCalibrationModule(mp_module.MPModule):
+ def __init__(self, mpstate):
+ super(SitlCalibrationModule, self).__init__(mpstate, "sitl_calibration")
+ self.add_command(
+ 'sitl_attitude',
+ self.cmd_sitl_attitude,
+ 'set the vehicle at the inclination given by ROLL, PITCH and YAW' +
+ ' in degrees',
+ )
+ self.add_command(
+ 'sitl_angvel',
+ self.cmd_angvel,
+ 'apply angular velocity on the vehicle with a rotation axis and a '+
+ 'magnitude in degrees/s',
+ )
+ self.add_command(
+ 'sitl_stop',
+ self.cmd_sitl_stop,
+ 'stop the current calibration control',
+ )
+
+ self.controllers = dict(
+ generic_controller=CalController(mpstate),
+ )
+
+ self.current_controller = None
+
+ def set_controller(self, controller):
+ if self.current_controller:
+ self.current_controller.stop()
+
+ controller = self.controllers.get(controller, None)
+ if controller:
+ controller.start()
+ self.current_controller = controller
+
+ def cmd_sitl_attitude(self, args):
+ if len(args) != 3:
+ print('Usage: sitl_attitude ')
+ return
+
+ try:
+ roll, pitch, yaw = args
+ roll = math.radians(float(roll))
+ pitch = math.radians(float(pitch))
+ yaw = math.radians(float(yaw))
+ except ValueError:
+ print('Invalid arguments')
+
+ self.set_controller('generic_controller')
+ self.current_controller.set_attitute(roll, pitch, yaw)
+
+ def cmd_angvel(self, args):
+ if len(args) != 4:
+ print('Usage: sitl_angvel ')
+ return
+
+ try:
+ x, y, z, theta = args
+ x = float(x)
+ y = float(y)
+ z = float(z)
+ theta = math.radians(float(theta))
+ except ValueError:
+ print('Invalid arguments')
+
+ self.set_controller('generic_controller')
+ self.current_controller.angvel(x, y, z, theta)
+
+ def cmd_sitl_stop(self, args):
+ self.set_controller('generic_controller')
+
+ def mavlink_packet(self, m):
+ for c in self.controllers.values():
+ c.mavlink_packet(m)
+
+def init(mpstate):
+ '''initialise module'''
+ return SitlCalibrationModule(mpstate)