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

libraries/SITL/SIM_SoloGimbal.cpp

@@ -36,149 +36,12 @@ extern const AP_HAL::HAL& hal;
 
 namespace SITL {
 
-SoloGimbal::SoloGimbal(const struct sitl_fdm &_fdm) :
-    fdm(_fdm),
-    target_address("127.0.0.1"),
-    target_port(5762),
-    lower_joint_limits(radians(-40), radians(-135), radians(-7.5)),
-    upper_joint_limits(radians(40),  radians(45),   radians(7.5)),
-    travelLimitGain(20),
-    reporting_period_ms(10),
-    seen_heartbeat(false),
-    seen_gimbal_control(false),
-    mav_socket(false)
-{
-    memset(&mavlink, 0, sizeof(mavlink));
-    fdm.quaternion.rotation_matrix(dcm);
-}
-
-
 /*
   update the gimbal state
 */
-void SoloGimbal::update(void)
+void SoloGimbal::update(const Aircraft &aircraft)
 {
-    // calculate delta time in seconds
+    gimbal.update(aircraft);
-    uint32_t now_us = AP_HAL::micros();
-
-    float delta_t = (now_us - last_update_us) * 1.0e-6f;
-    last_update_us = now_us;
-
-    Matrix3f vehicle_dcm;
-    fdm.quaternion.rotation_matrix(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;
 
     // see if we should do a report
     send_report();
@@ -306,11 +169,9 @@ void SoloGimbal::send_report(void)
                     }
                     mavlink_gimbal_control_t pkt;
                     mavlink_msg_gimbal_control_decode(&msg, &pkt);
-                    demanded_angular_rate = Vector3f(pkt.demanded_rate_x,
+                    gimbal.set_demanded_rates(Vector3f(pkt.demanded_rate_x,
                                                        pkt.demanded_rate_y,
-                                                     pkt.demanded_rate_z);
+                                                       pkt.demanded_rate_z));
-                    // no longer supply a bias
-                    supplied_gyro_bias.zero();
                     seen_gimbal_control = true;
                     break;
                 }
@@ -375,12 +236,20 @@ void SoloGimbal::send_report(void)
      */
     uint32_t now_us = AP_HAL::micros();
     if (now_us - last_report_us > reporting_period_ms*1000UL) {
-        mavlink_gimbal_report_t gimbal_report;
-        float delta_time = (now_us - last_report_us) * 1.0e-6f;
         last_report_us = now_us;
+
+        uint32_t delta_time_us;
+        Vector3f delta_angle;
+        Vector3f delta_velocity;
+        gimbal.get_deltas(delta_angle, delta_velocity, delta_time_us);
+
+        Vector3f joint_angles;
+        gimbal.get_joint_angles(joint_angles);
+
+        mavlink_gimbal_report_t gimbal_report;
         gimbal_report.target_system = vehicle_system_id;
         gimbal_report.target_component = vehicle_component_id;
-        gimbal_report.delta_time = delta_time;
+        gimbal_report.delta_time = delta_time_us * 1e-6;
         gimbal_report.delta_angle_x = delta_angle.x;
         gimbal_report.delta_angle_y = delta_angle.y;
         gimbal_report.delta_angle_z = delta_angle.z;

libraries/SITL/SIM_SoloGimbal.h

@@ -27,6 +27,8 @@ rc 6 1818  # for neutral pitch input
 
 #if AP_SIM_SOLOGIMBAL_ENABLED
 
+#include "SIM_Gimbal.h"
+
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_HAL/utility/Socket_native.h>
@@ -35,65 +37,23 @@ namespace SITL {
 
 class SoloGimbal {
 public:
-    SoloGimbal(const struct sitl_fdm &_fdm);
+
-    void update(void);
+    SoloGimbal() {}
+    void update(const Aircraft &aicraft);
 
 private:
-    const struct sitl_fdm &fdm;
-    const char *target_address;
-    const uint16_t target_port;
 
-    // rotation matrix (gimbal body -> earth)
+    const char *target_address = "127.0.0.1";
-    Matrix3f dcm;
+    const uint16_t target_port = 5762;
 
-    // time of last update
+    // physic simulation of gimbal:
-    uint32_t last_update_us;
+    Gimbal gimbal;
-
-    // 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;
-    Vector3f upper_joint_limits;
-
-    const float travelLimitGain;
-
-    // true gyro bias
-    Vector3f true_gyro_bias;
 
     // reporting variables. gimbal pushes these to vehicle code over
     // MAVLink at approx 100Hz
 
     // reporting period in ms
-    const float reporting_period_ms;
+    const float reporting_period_ms = 10;
-
-    // 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;
 
     uint32_t last_report_us;
     uint32_t last_heartbeat_ms;
@@ -102,7 +62,7 @@ private:
     uint8_t vehicle_system_id;
     uint8_t vehicle_component_id;
 
-    SocketAPM_native mav_socket;
+    SocketAPM_native mav_socket{false};
     struct {
         // socket to telem2 on aircraft
         bool connected;

libraries/SITL/SIM_config.h

@@ -123,5 +123,10 @@
 #endif
 
 #ifndef AP_SIM_SOLOGIMBAL_ENABLED
-#define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED)
 #endif
+
+#ifndef AP_SIM_GIMBAL_ENABLED
+#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED)
+#endif
+