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 for gimbal
      17             : */
      18             : 
      19             : #include "SIM_Gimbal.h"
      20             : 
      21             : #if AP_SIM_GIMBAL_ENABLED
      22             : 
      23             : #include <stdio.h>
      24             : 
      25             : #include "SIM_Aircraft.h"
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : 
      29             : #define GIMBAL_DEBUG 0
      30             : 
      31             : #if GIMBAL_DEBUG
      32             : #define debug(fmt, args...)  do { printf("GIMBAL: " fmt, ##args); } while(0)
      33             : #else
      34             : #define debug(fmt, args...)  do { } while(0)
      35             : #endif
      36             : 
      37             : namespace SITL {
      38             : 
      39             : /*
      40             :   update the gimbal state
      41             : */
      42     1092939 : void Gimbal::update(const class Aircraft &aircraft)
      43             : {
      44             :     // calculate delta time in seconds
      45     1092939 :     uint32_t now_us = AP_HAL::micros();
      46             : 
      47     1092939 :     float delta_t = (now_us - last_update_us) * 1.0e-6f;
      48     1092939 :     last_update_us = now_us;
      49             : 
      50     1092939 :     const Matrix3f &vehicle_dcm = aircraft.get_dcm();
      51     1092939 :     if (!init_done) {
      52          10 :         dcm = vehicle_dcm;
      53          10 :         init_done = true;
      54             :     }
      55             : 
      56     1092939 :     const Vector3f &vehicle_gyro = AP::ins().get_gyro();
      57     1092939 :     const Vector3f &vehicle_accel_body = AP::ins().get_accel();
      58             : 
      59             :     // take a copy of the demanded rates to bypass the limiter function for testing
      60     1092939 :     Vector3f demRateRaw = demanded_angular_rate;
      61             : 
      62             :     // 1)  Rotate the copters rotation rates into the gimbals frame of reference
      63             :     // copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate
      64     1092939 :     Vector3f copterAngRate_G = dcm.transposed()*vehicle_dcm*vehicle_gyro;
      65             : 
      66             :     // 2) Subtract the copters body rates to obtain a copter relative rotational
      67             :     // rate vector (X,Y,Z) in gimbal sensor frame
      68             :     // relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G
      69     1092939 :     Vector3f relativeGimbalRate = demanded_angular_rate - copterAngRate_G;
      70             : 
      71             :     // calculate joint angles (euler312 order)
      72             :     // calculate copter -> gimbal rotation matrix
      73     1092939 :     Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
      74             : 
      75     1092939 :     joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
      76             : 
      77             :     /* 4)  For each of the three joints, calculate upper and lower rate limits
      78             :        from the corresponding angle limits and current joint angles
      79             : 
      80             :        upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain
      81             :        lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain
      82             : 
      83             :        travelLimitGain is equal to the inverse of the bump stop time constant and
      84             :        should be set to something like 20 initially. If set too high it can cause
      85             :        the rates to 'ring' when they the limiter is in force, particularly given
      86             :        we are using a first order numerical integration.
      87             :     */
      88     1092939 :     Vector3f upperRatelimit = -(joint_angles - upper_joint_limits) * travelLimitGain;
      89     1092939 :     Vector3f lowerRatelimit = -(joint_angles - lower_joint_limits) * travelLimitGain;
      90             : 
      91             :     /*
      92             :       5) Calculate the gimbal joint rates (roll, elevation, azimuth)
      93             : 
      94             :       gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z)
      95             : 
      96             :       where matrix =
      97             :       +-                                                                  -+
      98             :       |          cos(elevAngle),        0,         sin(elevAngle)          |
      99             :       |                                                                    |
     100             :       |  sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle)  |
     101             :       |                                                                    |
     102             :       |           sin(elevAngle)                   cos(elevAngle)          |
     103             :       |         - --------------,       0,         --------------          |
     104             :       |           cos(rollAngle)                   cos(rollAngle)          |
     105             :       +-                                                                  -+
     106             :     */
     107     1092939 :     float rollAngle = joint_angles.x;
     108     1092939 :     float elevAngle = joint_angles.y;
     109     1092939 :     Matrix3f matrix = Matrix3f(Vector3f(cosf(elevAngle),                  0,  sinf(elevAngle)),
     110     1092939 :                                Vector3f(sinf(elevAngle)*tanf(rollAngle),  1, -cosf(elevAngle)*tanf(rollAngle)),
     111     2185878 :                                Vector3f(-sinf(elevAngle)/cosf(rollAngle), 0,  cosf(elevAngle)/cosf(rollAngle)));
     112     1092939 :     Vector3f gimbalJointRates = matrix * relativeGimbalRate;
     113             : 
     114             :     // 6) Apply the rate limits from 4)
     115     1092939 :     gimbalJointRates.x = constrain_float(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x);
     116     1092939 :     gimbalJointRates.y = constrain_float(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y);
     117     1092939 :     gimbalJointRates.z = constrain_float(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z);
     118             :     /*
     119             :       7) Convert the modified gimbal joint rates to body rates (still copter
     120             :       relative)
     121             :       relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth)
     122             : 
     123             :       where Matrix =
     124             : 
     125             :       +-                                                   -+
     126             :       |  cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle)  |
     127             :       |                                                     |
     128             :       |         0,       1,         sin(rollAngle)          |
     129             :       |                                                     |
     130             :       |  sin(elevAngle), 0,  cos(elevAngle) cos(rollAngle)  |
     131             :       +-                                                   -+
     132             :     */
     133     1092939 :     matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, -cosf(rollAngle)*sinf(elevAngle)),
     134     1092939 :                       Vector3f(0,               1,  sinf(rollAngle)),
     135     1092939 :                       Vector3f(sinf(elevAngle), 0,  cosf(elevAngle)*cosf(rollAngle)));
     136     1092939 :     relativeGimbalRate = matrix * gimbalJointRates;
     137             : 
     138             :     // 8) Add to the result from step 1) to obtain the demanded gimbal body rates
     139             :     //    in an inertial frame of reference
     140             :     // demandedGimbalRatesInertial(X,Y,Z)  = relativeGimbalRate(X,Y,Z) + copterAngRate_G
     141             :     // Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G;
     142             : 
     143             :     // for the moment we will set gyros equal to demanded_angular_rate
     144     1092939 :     gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias
     145             : 
     146             :     // update rotation of the gimbal
     147     1092939 :     dcm.rotate(gimbal_angular_rate*delta_t);
     148     1092939 :     dcm.normalize();
     149             : 
     150             :     // calculate copter -> gimbal rotation matrix
     151     1092939 :     rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
     152             : 
     153             :     // calculate joint angles (euler312 order)
     154     1092939 :     joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
     155             : 
     156             :     // update observed gyro
     157     1092939 :     gyro = gimbal_angular_rate + true_gyro_bias;
     158             : 
     159             :     // update delta_angle (integrate)
     160     1092939 :     delta_angle += gyro * delta_t;
     161             : 
     162             :     // calculate accel in gimbal body frame
     163     1092939 :     Vector3f copter_accel_earth = vehicle_dcm * vehicle_accel_body;
     164     1092939 :     Vector3f accel = dcm.transposed() * copter_accel_earth;
     165             : 
     166             :     // integrate velocity
     167     1092939 :     delta_velocity += accel * delta_t;
     168     1092939 : }
     169             : 
     170       18238 : void Gimbal::get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us)
     171             : {
     172       18238 :     const uint32_t now_us = AP_HAL::micros();
     173             : 
     174       18238 :     _delta_angle = delta_angle;
     175       18238 :     _delta_velocity = delta_velocity;
     176       18238 :     _delta_time_us = now_us - delta_start_us;
     177             : 
     178       18238 :     delta_angle.zero();
     179       18238 :     delta_velocity.zero();
     180       18238 :     delta_start_us = now_us;
     181       18238 : }
     182             : 
     183             : } // namespace SITL
     184             : 
     185             : #endif  // AP_SIM_GIMBAL_ENABLED