SITL: split MAVLink and physical gimbal simulations

libraries/SITL/SIM_Gimbal.cpp

@@ -0,0 +1,184 @@
+/*
+   This program 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 program 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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  gimbal simulator class for gimbal
+*/
+
+#include "SIM_Gimbal.h"
+
+#if AP_SIM_GIMBAL_ENABLED
+
+#include <stdio.h>
+
+#include "SIM_Aircraft.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define GIMBAL_DEBUG 0
+
+#if GIMBAL_DEBUG
+#define debug(fmt, args...)  do { printf("GIMBAL: " fmt, ##args); } while(0)
+#else
+#define debug(fmt, args...)  do { } while(0)
+#endif
+
+namespace SITL {
+
+/*
+  update the gimbal state
+*/
+void Gimbal::update(const class Aircraft &aircraft)
+{
+    // calculate delta time in seconds
+    uint32_t now_us = AP_HAL::micros();
+
+    float delta_t = (now_us - last_update_us) * 1.0e-6f;
+    last_update_us = now_us;
+
+    const Matrix3f &vehicle_dcm = aircraft.get_dcm();
+    if (!init_done) {
+        dcm = vehicle_dcm;
+    }
+
+    const Vector3f &vehicle_gyro = AP::ins().get_gyro();
+    const Vector3f &vehicle_accel_body = AP::ins().get_accel();
+
+    // take a copy of the demanded rates to bypass the limiter function for testing
+    Vector3f demRateRaw = demanded_angular_rate;
+
+    // 1)  Rotate the copters rotation rates into the gimbals frame of reference
+    // copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate
+    Vector3f copterAngRate_G = dcm.transposed()*vehicle_dcm*vehicle_gyro;
+
+    // 2) Subtract the copters body rates to obtain a copter relative rotational
+    // rate vector (X,Y,Z) in gimbal sensor frame
+    // relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G
+    Vector3f relativeGimbalRate = demanded_angular_rate - copterAngRate_G;
+
+    // calculate joint angles (euler312 order)
+    // calculate copter -> gimbal rotation matrix
+    Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
+
+    joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
+
+    /* 4)  For each of the three joints, calculate upper and lower rate limits
+       from the corresponding angle limits and current joint angles
+
+       upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain
+       lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain
+
+       travelLimitGain is equal to the inverse of the bump stop time constant and
+       should be set to something like 20 initially. If set too high it can cause
+       the rates to 'ring' when they the limiter is in force, particularly given
+       we are using a first order numerical integration.
+    */
+    Vector3f upperRatelimit = -(joint_angles - upper_joint_limits) * travelLimitGain;
+    Vector3f lowerRatelimit = -(joint_angles - lower_joint_limits) * travelLimitGain;
+
+    /*
+      5) Calculate the gimbal joint rates (roll, elevation, azimuth)
+
+      gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z)
+
+      where matrix =
+      +-                                                                  -+
+      |          cos(elevAngle),        0,         sin(elevAngle)          |
+      |                                                                    |
+      |  sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle)  |
+      |                                                                    |
+      |           sin(elevAngle)                   cos(elevAngle)          |
+      |         - --------------,       0,         --------------          |
+      |           cos(rollAngle)                   cos(rollAngle)          |
+      +-                                                                  -+
+    */
+    float rollAngle = joint_angles.x;
+    float elevAngle = joint_angles.y;
+    Matrix3f matrix = Matrix3f(Vector3f(cosf(elevAngle),                  0,  sinf(elevAngle)),
+                               Vector3f(sinf(elevAngle)*tanf(rollAngle),  1, -cosf(elevAngle)*tanf(rollAngle)),
+                               Vector3f(-sinf(elevAngle)/cosf(rollAngle), 0,  cosf(elevAngle)/cosf(rollAngle)));
+    Vector3f gimbalJointRates = matrix * relativeGimbalRate;
+
+    // 6) Apply the rate limits from 4)
+    gimbalJointRates.x = constrain_float(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x);
+    gimbalJointRates.y = constrain_float(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y);
+    gimbalJointRates.z = constrain_float(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z);
+    /*
+      7) Convert the modified gimbal joint rates to body rates (still copter
+      relative)
+      relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth)
+
+      where Matrix =
+
+      +-                                                   -+
+      |  cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle)  |
+      |                                                     |
+      |         0,       1,         sin(rollAngle)          |
+      |                                                     |
+      |  sin(elevAngle), 0,  cos(elevAngle) cos(rollAngle)  |
+      +-                                                   -+
+    */
+    matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, -cosf(rollAngle)*sinf(elevAngle)),
+                      Vector3f(0,               1,  sinf(rollAngle)),
+                      Vector3f(sinf(elevAngle), 0,  cosf(elevAngle)*cosf(rollAngle)));
+    relativeGimbalRate = matrix * gimbalJointRates;
+
+    // 8) Add to the result from step 1) to obtain the demanded gimbal body rates
+    //    in an inertial frame of reference
+    // demandedGimbalRatesInertial(X,Y,Z)  = relativeGimbalRate(X,Y,Z) + copterAngRate_G
+    // Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G;
+
+    // for the moment we will set gyros equal to demanded_angular_rate
+    gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias
+
+    // update rotation of the gimbal
+    dcm.rotate(gimbal_angular_rate*delta_t);
+    dcm.normalize();
+
+    // calculate copter -> gimbal rotation matrix
+    rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
+
+    // calculate joint angles (euler312 order)
+    joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
+
+    // update observed gyro
+    gyro = gimbal_angular_rate + true_gyro_bias;
+
+    // update delta_angle (integrate)
+    delta_angle += gyro * delta_t;
+
+    // calculate accel in gimbal body frame
+    Vector3f copter_accel_earth = vehicle_dcm * vehicle_accel_body;
+    Vector3f accel = dcm.transposed() * copter_accel_earth;
+
+    // integrate velocity
+    delta_velocity += accel * delta_t;
+}
+
+void Gimbal::get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us)
+{
+    const uint32_t now_us = AP_HAL::micros();
+
+    _delta_angle = delta_angle;
+    _delta_velocity = delta_velocity;
+    _delta_time_us = now_us - delta_start_us;
+
+    delta_angle.zero();
+    delta_velocity.zero();
+    delta_start_us = now_us;
+}
+
+} // namespace SITL
+
+#endif  // AP_SIM_GIMBAL_ENABLED

libraries/SITL/SIM_Gimbal.h

@@ -0,0 +1,96 @@
+/*
+   This program 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 program 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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  gimbal simulator class
+*/
+
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_GIMBAL_ENABLED
+
+#include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
+
+namespace SITL {
+
+class Gimbal {
+public:
+
+    void update(const class Aircraft &aircraft);
+    void set_demanded_rates(const Vector3f &rates) {
+        demanded_angular_rate = rates;
+    }
+
+    void get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us);
+    void get_joint_angles(Vector3f &_angles) { _angles = joint_angles; }
+
+private:
+
+    // rotation matrix (gimbal body -> earth)
+    Matrix3f dcm;
+    bool init_done;
+
+    // time of last update
+    uint32_t last_update_us;
+
+    // true angular rate of gimbal in body frame (rad/s)
+    Vector3f gimbal_angular_rate;
+
+    // observed angular rate (including biases)
+    Vector3f gyro;
+
+    /* joint angles, in radians. in yaw/roll/pitch order. Relative to fwd.
+       So 0,0,0 points forward.
+       Pi/2,0,0 means pointing right
+       0, Pi/2, 0 means pointing fwd, but rolled 90 degrees to right
+       0, 0, -Pi/2, means pointing down
+    */
+    Vector3f joint_angles;
+
+    // physical constraints on joint angles in (roll, pitch, azimuth) order
+    Vector3f lower_joint_limits{radians(-40), radians(-135), radians(-7.5)};
+    Vector3f upper_joint_limits{radians(40),  radians(45),   radians(7.5)};
+
+    const float travelLimitGain = 20;
+
+    // true gyro bias
+    Vector3f true_gyro_bias;
+
+    // time since delta angles/velocities returned
+    uint32_t delta_start_us;
+
+    // integral of gyro vector over last time interval. In radians
+    Vector3f delta_angle;
+
+    // integral of accel vector over last time interval. In m/s
+    Vector3f delta_velocity;
+
+    /*
+      control variables from the vehicle
+    */
+    // angular rate in rad/s. In body frame of gimbal
+    Vector3f demanded_angular_rate;
+
+    // gyro bias provided by EKF on vehicle. In rad/s.
+    // Should be subtracted from the gyro readings to get true body
+    // rotatation rates
+    // Vector3f supplied_gyro_bias;
+};
+
+}  // namespace SITL
+
+#endif  // AP_SIM_GIMBAL_ENABLED