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             :   Glider model for high altitude balloon drop
      17             : */
      18             : 
      19             : #pragma once
      20             : 
      21             : #include "SIM_config.h"
      22             : 
      23             : #if AP_SIM_GLIDER_ENABLED
      24             : 
      25             : #include "SIM_Aircraft.h"
      26             : #include <AP_Param/AP_Param.h>
      27             : 
      28             : namespace SITL {
      29             : 
      30             : /*
      31             :   a very simple plane simulator
      32             :  */
      33             : class Glider : public Aircraft {
      34             : public:
      35             :     Glider(const char *frame_str);
      36             : 
      37             :     /* update model by one time step */
      38             :     virtual void update(const struct sitl_input &input) override;
      39             : 
      40             :     /* static object creator */
      41           1 :     static Aircraft *create(const char *frame_str) {
      42           1 :         return NEW_NOTHROW Glider(frame_str);
      43             :     }
      44             : 
      45             :     bool on_ground() const override;
      46             : 
      47             :     static const struct AP_Param::GroupInfo var_info[];
      48             : 
      49             : protected:
      50             :     float alpharad;
      51             :     float betarad;
      52             : 
      53             :     AP_Float balloon_burst_amsl;
      54             :     AP_Float balloon_rate;
      55             : 
      56             :     /*
      57             :       parameters that define the glider model
      58             :      */
      59             :     const struct Model {
      60             :         // total vehicle mass
      61             :         float mass = 9.07441; // kg
      62             : 
      63             :         // reference area
      64             :         float Sref = 0.92762; // m^2
      65             : 
      66             :         float refSpan = 1.827411; // m
      67             :         float refChord = 0.507614; // m
      68             :         float IXX = 0.234; // kg-m^2
      69             :         float IYY = 1.85; // kg-m^2
      70             :         float IZZ = 2.04; // kg-m^2
      71             : 
      72             :         // CN is coefficients for forces on +Z axis
      73             :         // quadratic in alpharad
      74             :         float CN2 = -0.5771;
      75             :         float CN1 = 3.9496;
      76             :         float CN0 = 0;
      77             : 
      78             :         // CA is the coefficients for forces on +X axis
      79             :         // quadratic in alpharad
      80             :         float CA2 = -1.6809;
      81             :         float CA1 = -0.0057;
      82             :         float CA0 = 0.0150;
      83             : 
      84             :         // CY is the coefficients for forces on the +Y axis
      85             :         // quadratic in alpharad, with betarad factor
      86             :         float CY2 = -3.342;
      87             :         float CY1 = 0.0227;
      88             :         float CY0 = -0.4608;
      89             : 
      90             :         // Cl is the coefficients for moments on X axis
      91             :         // quadratic in alpharad, with betarad factor
      92             :         float Cl2 = 0.2888;
      93             :         float Cl1 = -0.8518;
      94             :         float Cl0 = -0.0491;
      95             : 
      96             :         // Cm is the coefficients for moments on Y axis
      97             :         // quadratic in alpharad
      98             :         float Cm2 = 0.099;
      99             :         float Cm1 = -0.6506;
     100             :         float Cm0 = -0.0005;
     101             : 
     102             :         // Cn is the coefficients for moments on Z axis
     103             :         // quadratic in alpharad, with betarad factor
     104             :         float Cn2 = 0.0057;
     105             :         float Cn1 = -0.0101;
     106             :         float Cn0 = 0.1744;
     107             : 
     108             :         // controls neutral dynamic derivatives
     109             :         // p, q, r are gyro rates
     110             :         float Cmq = -6.1866;
     111             : 
     112             :         float Clp2 = 0.156;
     113             :         float Clp1 = 0.0129;
     114             :         float Clp0 = -0.315;
     115             : 
     116             :         float Clr2 = -0.0284;
     117             :         float Clr1 = 0.2641;
     118             :         float Clr0 = 0.0343;
     119             : 
     120             :         float Cnp2 = 0.0199;
     121             :         float Cnp1 = -0.315;
     122             :         float Cnp0 = -0.013;
     123             : 
     124             :         float Cnr2 = 0.1297;
     125             :         float Cnr1 = 0.0343;
     126             :         float Cnr0 = -0.264;
     127             : 
     128             :         // elevator
     129             :         float elevatorDeflectionLimitDeg = -12.5;
     130             :         float deltaCNperRadianElev = -0.7;
     131             :         float deltaCAperRadianElev = 0.12;
     132             :         float deltaCmperRadianElev = 1.39;
     133             :         float deltaCYperRadianElev = 0;
     134             :         float deltaClperRadianElev = 0;
     135             :         float deltaCnperRadianElev = 0;
     136             : 
     137             :         // rudder
     138             :         float rudderDeflectionLimitDeg = 18.0;
     139             :         float deltaCNperRadianRud = 0;
     140             :         float deltaCAperRadianRud = 0.058;
     141             :         float deltaCmperRadianRud = 0;
     142             :         float deltaCYperRadianRud = 0.31;
     143             :         float deltaClperRadianRud = 0.038;
     144             :         float deltaCnperRadianRud = -0.174;
     145             : 
     146             :         // aileron
     147             :         float aileronDeflectionLimitDeg = 15.5;
     148             :         float deltaCNperRadianAil = 0;
     149             :         float deltaCAperRadianAil = 0.016;
     150             :         float deltaCmperRadianAil = 0;
     151             :         float deltaCYperRadianAil = -0.015;
     152             : 
     153             :         // quadratic in alpharad
     154             :         float deltaClperRadianAil0 = 0.09191;
     155             :         float deltaClperRadianAil1 = 0.0001;
     156             :         float deltaClperRadianAil2 = -0.08645;
     157             : 
     158             :         // quadratic in alpharad
     159             :         float deltaCnperRadianAil0 = 0.00789;
     160             :         float deltaCnperRadianAil1 = 0.00773;
     161             :         float deltaCnperRadianAil2 = -0.01162;
     162             : 
     163             :         // Forces in the +X direction are –CA * q * Sref
     164             :         // Forces in the +Y direction are  +CY * q * Sref
     165             :         // Forces in the +Z direction are  –CN * q *Sref
     166             :         // Moments about the X axis are +Cl * q * Sref * RefSpan
     167             :         // Moments about the Y axis are +Cm * q * Sref * RefChord
     168             :         // Moments about the Z axis are +Cn * q * Sref * RefSpan
     169             : 
     170             :         // low altitude
     171             :         float alphaRadMax = 0.209;
     172             :         float betaRadMax = 0.209;
     173             : 
     174             :         // balloon launch parameters
     175             :         float tetherLength = 50.0f;       // length of tether from balloon to aircraft (m)
     176             :         float tetherPogoFreq = 2.0f;      // measured vertical frequency of on tether (Hz)
     177             : 
     178             :     } model;
     179             : 
     180             :     Vector3f getForce(float inputAileron, float inputElevator, float inputRudder);
     181             :     Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const;
     182             :     bool update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel);
     183             :     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
     184             : 
     185             :     Vector3f balloon_velocity;           // balloon velocity NED
     186             :     Vector3f balloon_position{0.0f, 0.0f, -45.0f}; // balloon position NED from origin
     187             : 
     188             :     enum class carriageState {
     189             :         NONE = 0, // no carriage option available
     190             :         WAITING_FOR_PICKUP = 1, // in launch cradle waiting to be picked up by launch vehicle
     191             :         WAITING_FOR_RELEASE = 2, // being carried by luanch vehicle waitng to be released
     192             :         PRE_RELEASE = 3, // had been released by launch vehicle
     193             :         RELEASED = 4 // had been released by launch vehicle
     194             :     } carriage_state;
     195             :     bool plane_air_release;    // true when plane has separated from the airborne launching platform
     196             : 
     197             :     uint32_t last_drag_ms;
     198             :     float sim_LD;
     199             : };
     200             : 
     201             : } // namespace SITL
     202             : 
     203             : #endif // AP_SIM_GLIDER_ENABLED