Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   gimbal simulator class
      17             : 
      18             : ./Tools/autotest/sim_vehicle.py -v Copter --gdb --debug --console --map --mavproxy-args=--load-module=gimbal --mavlink-gimbal
      19             : param load Tools/autotest/default_params/copter-solo-gimbal.parm
      20             : 
      21             : graph degrees(Quaternion(GIMBAL_DEVICE_ATTITUDE_STATUS.q).euler[0]) degrees(Quaternion(GIMBAL_DEVICE_ATTITUDE_STATUS.q).euler[1]) degrees(Quaternion(GIMBAL_DEVICE_ATTITUDE_STATUS.q).euler[2])
      22             : 
      23             : reboot
      24             : 
      25             : rc 6 1400
      26             : */
      27             : 
      28             : #pragma once
      29             : 
      30             : #include "SIM_config.h"
      31             : 
      32             : #if AP_SIM_GIMBAL_ENABLED
      33             : 
      34             : #include <AP_Math/AP_Math.h>
      35             : #include <GCS_MAVLink/GCS.h>
      36             : 
      37             : namespace SITL {
      38             : 
      39             : class Gimbal {
      40             : public:
      41             : 
      42             :     void update(const class Aircraft &aircraft);
      43      838479 :     void set_demanded_rates(const Vector3f &rates) {
      44      838479 :         demanded_angular_rate = rates;
      45      838479 :     }
      46             : 
      47             :     void get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us);
      48      530245 :     void get_joint_angles(Vector3f &_angles) const { _angles = joint_angles; }
      49             : 
      50             :     // copy the gimbal body-to-earth rotation matrix
      51      841072 :     void get_dcm(Matrix3f &out) const { out = dcm; }
      52             : 
      53             :     // override the physical joint angle limits (roll, pitch, azimuth) in radians
      54           5 :     void set_joint_limits(const Vector3f &lower_limits, const Vector3f &upper_limits) {
      55           5 :         lower_joint_limits = lower_limits;
      56           5 :         upper_joint_limits = upper_limits;
      57           5 :     }
      58             : 
      59             :     // force the gimbal body-to-earth rotation matrix to a specific value
      60      278506 :     void set_dcm(const Matrix3f &m) { dcm = m; init_done = true; }
      61             : 
      62             : private:
      63             : 
      64             :     // rotation matrix (gimbal body -> earth)
      65             :     Matrix3f dcm;
      66             :     bool init_done;
      67             : 
      68             :     // time of last update
      69             :     uint32_t last_update_us;
      70             : 
      71             :     // true angular rate of gimbal in body frame (rad/s)
      72             :     Vector3f gimbal_angular_rate;
      73             : 
      74             :     // observed angular rate (including biases)
      75             :     Vector3f gyro;
      76             : 
      77             :     /* joint angles, in radians. in yaw/roll/pitch order. Relative to fwd.
      78             :        So 0,0,0 points forward.
      79             :        Pi/2,0,0 means pointing right
      80             :        0, Pi/2, 0 means pointing fwd, but rolled 90 degrees to right
      81             :        0, 0, -Pi/2, means pointing down
      82             :     */
      83             :     Vector3f joint_angles;
      84             : 
      85             :     // physical constraints on joint angles in (roll, pitch, azimuth) order
      86             :     Vector3f lower_joint_limits{
      87             :         float(radians(-40)),
      88             :         float(radians(-135)),
      89             :         float(radians(-7.5))
      90             :     };
      91             :     Vector3f upper_joint_limits{
      92             :         float(radians(40)),
      93             :         float(radians(45)),
      94             :         float(radians(7.5))
      95             :     };
      96             : 
      97             :     const float travelLimitGain = 20;
      98             : 
      99             :     // true gyro bias
     100             :     Vector3f true_gyro_bias;
     101             : 
     102             :     // time since delta angles/velocities returned
     103             :     uint32_t delta_start_us;
     104             : 
     105             :     // integral of gyro vector over last time interval. In radians
     106             :     Vector3f delta_angle;
     107             : 
     108             :     // integral of accel vector over last time interval. In m/s
     109             :     Vector3f delta_velocity;
     110             : 
     111             :     /*
     112             :       control variables from the vehicle
     113             :     */
     114             :     // angular rate in rad/s. In body frame of gimbal
     115             :     Vector3f demanded_angular_rate;
     116             : 
     117             :     // gyro bias provided by EKF on vehicle. In rad/s.
     118             :     // Should be subtracted from the gyro readings to get true body
     119             :     // rotatation rates
     120             :     // Vector3f supplied_gyro_bias;
     121             : };
     122             : 
     123             : }  // namespace SITL
     124             : 
     125             : #endif  // AP_SIM_GIMBAL_ENABLED