diff --git a/libraries/SITL/SIM_Calibration.cpp b/libraries/SITL/SIM_Calibration.cpp
new file mode 100644
index 0000000000..7dd6b1c54c
--- /dev/null
+++ b/libraries/SITL/SIM_Calibration.cpp
@@ -0,0 +1,109 @@
+/*
+ * 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 .
+ */
+#include
+
+#include
+
+#include "SIM_Calibration.h"
+
+#define MAX_ANGULAR_SPEED (2 * M_PI)
+
+#include
+
+SITL::Calibration::Calibration(const char *home_str, const char *frame_str)
+ : Aircraft(home_str, frame_str)
+{
+ mass = 1.5f;
+}
+
+void SITL::Calibration::update(const struct sitl_input& input)
+{
+ Vector3f rot_accel{0, 0, 0};
+
+ float switcher_pwm = input.servos[4];
+ if (switcher_pwm < 1100) {
+ _stop_control(input, rot_accel);
+ } else if (switcher_pwm < 1200) {
+ _attitude_control(input, rot_accel);
+ } else {
+ _angular_velocity_control(input, rot_accel);
+ }
+
+ accel_body(0, 0, 0);
+ update_dynamics(rot_accel);
+ update_position();
+}
+
+void SITL::Calibration::_stop_control(const struct sitl_input& input,
+ Vector3f& rot_accel)
+{
+ Vector3f desired_angvel{0, 0, 0};
+ Vector3f error = desired_angvel - gyro;
+ float dt = frame_time_us * 1.0e-6f;
+ rot_accel = error * (1.0f / dt);
+ /* Provide a somewhat "smooth" transition */
+ rot_accel *= 0.002f;
+}
+
+void SITL::Calibration::_attitude_control(const struct sitl_input& input,
+ Vector3f& rot_accel)
+{
+ float desired_roll = -M_PI + 2 * M_PI * (input.servos[5] - 1000) / 1000.f;
+ float desired_pitch = -M_PI + 2 * M_PI * (input.servos[6] - 1000) / 1000.f;
+ float desired_yaw = -M_PI + 2 * M_PI * (input.servos[7] - 1000) / 1000.f;
+ float dt = frame_time_us * 1.0e-6f;
+
+ Quaternion desired_q;
+ desired_q.from_euler(desired_roll, desired_pitch, desired_yaw);
+ desired_q.normalize();
+
+ Quaternion current_q;
+ current_q.from_rotation_matrix(dcm);
+ current_q.normalize();
+
+ Quaternion error_q = desired_q / current_q;
+
+ Vector3f angle_differential;
+ error_q.normalize();
+ error_q.to_axis_angle(angle_differential);
+
+ Vector3f desired_angvel = angle_differential * (1 / dt);
+ /* Provide a somewhat "smooth" transition */
+ desired_angvel *= .005f;
+
+ Vector3f error = desired_angvel - gyro;
+ rot_accel = error * (1.0f / dt);
+}
+
+void SITL::Calibration::_angular_velocity_control(const struct sitl_input& in,
+ Vector3f& rot_accel)
+{
+ Vector3f axis{(float)(in.servos[5] - 1500),
+ (float)(in.servos[6] - 1500),
+ (float)(in.servos[7] - 1500)};
+ float theta = MAX_ANGULAR_SPEED * (in.servos[4] - 1200) / 800.f;
+ float dt = frame_time_us * 1.0e-6f;
+
+ axis.normalize();
+
+ Vector3f desired_angvel = axis * theta;
+ Vector3f error = desired_angvel - gyro;
+
+ rot_accel = error * (1.0f / dt);
+ /* Provide a somewhat "smooth" transition */
+ rot_accel *= .05f;
+}
diff --git a/libraries/SITL/SIM_Calibration.h b/libraries/SITL/SIM_Calibration.h
new file mode 100644
index 0000000000..225a2b266a
--- /dev/null
+++ b/libraries/SITL/SIM_Calibration.h
@@ -0,0 +1,80 @@
+/*
+ * 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 .
+ */
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+#include
+
+namespace SITL {
+
+/**
+ * Simulation model to simulate calibration of accelerometers and compasses.
+ *
+ * The vehicle rotation can be controlled by sending PWM values to the servos
+ * input, denoted by PWM[i] for the i-th channel (starting by zero). All PWM
+ * values must be in [1000, 2000], otherwise that will cause undefined
+ * behavior.
+ *
+ * There are three control modes, that are set with PWM[4]:
+ *
+ * 1) Stop (1000 <= PWM[4] < 1100):
+ * Stop the vehicle, i.e., stop the actuation of the other modes.
+ *
+ * 2) Attitude (1100 <= PWM[4] < 1200):
+ * Rotate the vehicle to the specified attitude. The attitude is defined
+ * with the PWM channels 5, 6 and 7 for roll, pitch and yaw angles,
+ * respectively. The PWM value for a desired angle in radians is given by:
+ *
+ * pwm(theta) = 1500 + 500 * round(theta / pi)
+ * where -pi <= theta <= pi
+ *
+ * 3) Angular Velocity (1200 <= PWM[4] <= 2000):
+ * Rotate the vehicle at a desired angular velocity. The angular velocity is
+ * specified by a rotation axis and an angular speed.
+ *
+ * The x, y and z components of the rotation axis is given, respectively, by
+ * the PWM channels 5, 6 and 7 with an offset of 1500. The rotation axis is
+ * normalized internally, so that PWM[5,6,7] = [1600, 1300, 0] and
+ * PWM[5,6,7] = [1700, 1100, 0] means the same normalized rotation axis.
+ *
+ * The angular speed value is specified by PWM[4]. The PWM value for a
+ * desired angular speed in radians/s is given by:
+ *
+ * pwm(theta) = 1200 + 800 * round(theta / (2 * pi)),
+ * where 0 <= theta <= 2 * pi
+ */
+class Calibration : public Aircraft {
+public:
+ Calibration(const char *home_str, const char *frame_str);
+
+ void update(const struct sitl_input& input);
+
+ static Aircraft *create(const char *home_str, const char *frame_str) {
+ return new Calibration(home_str, frame_str);
+ }
+
+private:
+ void _stop_control(const struct sitl_input& input, Vector3f& rot_accel);
+
+ void _attitude_control(const struct sitl_input& input,
+ Vector3f& rot_accel);
+
+ void _angular_velocity_control(const struct sitl_input& input,
+ Vector3f& rot_accel);
+};
+}