Line data    Source code

       1             : /*
       2             :  * Copyright (C) 2015-2016  Intel Corporation. All rights reserved.
       3             :  *
       4             :  * This file is free software: you can redistribute it and/or modify it
       5             :  * under the terms of the GNU General Public License as published by the
       6             :  * Free Software Foundation, either version 3 of the License, or
       7             :  * (at your option) any later version.
       8             :  *
       9             :  * This file is distributed in the hope that it will be useful, but
      10             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      12             :  * See the GNU General Public License for more details.
      13             :  *
      14             :  * You should have received a copy of the GNU General Public License along
      15             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : #pragma once
      18             : 
      19             : #include "SIM_Aircraft.h"
      20             : 
      21             : #include <AP_Math/AP_Math.h>
      22             : 
      23             : namespace SITL {
      24             : 
      25             : /**
      26             :  * Simulation model to simulate calibration of accelerometers and compasses.
      27             :  *
      28             :  * The vehicle rotation can be controlled by sending PWM values to the servos
      29             :  * input, denoted by PWM[i] for the i-th channel (starting by zero). All PWM
      30             :  * values must be in [1000, 2000], otherwise that will cause undefined
      31             :  * behavior.
      32             :  *
      33             :  * There are four control modes, that are set with PWM[4]:
      34             :  *
      35             :  *  1) Stop (1000 <= PWM[4] < 1100):
      36             :  *    Stop the vehicle, i.e., stop the actuation of the other modes.
      37             :  *
      38             :  *  2) Attitude (1100 <= PWM[4] < 1200):
      39             :  *    Rotate the vehicle to the specified attitude. The attitude is defined
      40             :  *    with the PWM channels 5, 6 and 7 for roll, pitch and yaw angles,
      41             :  *    respectively. The PWM value for a desired angle in radians is given by:
      42             :  *
      43             :  *        pwm(theta) = 1500 + 500 * round(theta / pi)
      44             :  *        where -pi <= theta <= pi
      45             :  *
      46             :  *  3) Simple autonomous compass calibration (1200 <= PWM[4] < 1300):
      47             :  *    Move continuously the vehicle through six calibration poses and do a
      48             :  *    rotation about each pose over a short period of time.
      49             :  *
      50             :  *  4) Angular Velocity (1300 <= PWM[4] <= 2000):
      51             :  *    Rotate the vehicle at a desired angular velocity. The angular velocity is
      52             :  *    specified by a rotation axis and an angular speed.
      53             :  *
      54             :  *    The x, y and z components of the rotation axis is given, respectively, by
      55             :  *    the PWM channels 5, 6 and 7 with an offset of 1500. The rotation axis is
      56             :  *    normalized internally, so that PWM[5,6,7] = [1600, 1300, 0] and
      57             :  *    PWM[5,6,7] = [1700, 1100, 0] means the same normalized rotation axis.
      58             :  *
      59             :  *    The angular speed value is specified by PWM[4]. The PWM value for a
      60             :  *    desired angular speed in radians/s is given by:
      61             :  *
      62             :  *        pwm(theta) = 1300 + 700 * round(theta / (2 * pi)),
      63             :  *        where 0 <= theta <= 2 * pi
      64             :  */
      65             : class Calibration : public Aircraft {
      66             : public:
      67             :     Calibration(const char *frame_str);
      68             : 
      69             :     void update(const struct sitl_input &input) override;
      70             : 
      71          11 :     static Aircraft *create(const char *frame_str) {
      72          11 :         return NEW_NOTHROW Calibration(frame_str);
      73             :     }
      74             : 
      75             : private:
      76             :     void _stop_control(const struct sitl_input &input, Vector3f& rot_accel);
      77             : 
      78             :     void _attitude_set(float desired_roll, float desired_pitch, float desired_yaw,
      79             :                        Vector3f& rot_accel);
      80             : 
      81             :     void _attitude_control(const struct sitl_input &input,
      82             :                            Vector3f& rot_accel);
      83             : 
      84             :     void _angular_velocity_control(const struct sitl_input &input,
      85             :                                    Vector3f& rot_accel);
      86             : 
      87             :     void _calibration_poses(Vector3f& rot_accel);
      88             : };
      89             : }