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             :   simple plane simulator class
      17             : */
      18             : 
      19             : #pragma once
      20             : 
      21             : #include "SIM_Aircraft.h"
      22             : #include "SIM_ICEngine.h"
      23             : #include <Filter/LowPassFilter.h>
      24             : #include <AP_InternalError/AP_InternalError.h>
      25             : #include <AP_JSON/AP_JSON.h>
      26             : 
      27             : namespace SITL {
      28             : 
      29             : /*
      30             :   a very simple plane simulator
      31             :  */
      32             : class Plane : public Aircraft {
      33             : public:
      34             :     Plane(const char *frame_str);
      35             : 
      36             :     /* update model by one time step */
      37             :     virtual void update(const struct sitl_input &input) override;
      38             : 
      39             :     /* static object creator */
      40         490 :     static Aircraft *create(const char *frame_str) {
      41         490 :         return NEW_NOTHROW Plane(frame_str);
      42             :     }
      43             : 
      44             : protected:
      45             :     const float hover_throttle = 0.7f;
      46             :     float angle_of_attack;
      47             :     float beta;
      48             : 
      49             :     const struct Coefficients {
      50             :         // from last_letter skywalker_2013/aerodynamics.yaml
      51             :         // thanks to Georacer!
      52             :         float s = 0.45;
      53             :         float b = 1.88;
      54             :         float c = 0.24;
      55             :         float c_lift_0 = 0.56;
      56             :         float c_lift_deltae = 0;
      57             :         float c_lift_a = 6.9;
      58             :         float c_lift_q = 0;
      59             :         float mcoeff = 50;
      60             :         float oswald = 0.9;
      61             :         float alpha_stall = 0.4712;
      62             :         float c_drag_q = 0;
      63             :         float c_drag_deltae = 0.0;
      64             :         float c_drag_p = 0.1;
      65             :         float c_y_0 = 0;
      66             :         float c_y_b = -0.98;
      67             :         float c_y_p = 0;
      68             :         float c_y_r = 0;
      69             :         float c_y_deltaa = 0;
      70             :         float c_y_deltar = -0.2;
      71             :         float c_l_0 = 0;
      72             :         float c_l_p = -1.0;
      73             :         float c_l_b = -0.12;
      74             :         float c_l_r = 0.14;
      75             :         float c_l_deltaa = 0.25;
      76             :         float c_l_deltar = -0.037;
      77             :         float c_m_0 = 0.045;
      78             :         float c_m_a = -0.7;
      79             :         float c_m_q = -20;
      80             :         float c_m_deltae = 1.0;
      81             :         float c_n_0 = 0;
      82             :         float c_n_b = 0.25;
      83             :         float c_n_p = 0.022;
      84             :         float c_n_r = -1;
      85             :         float c_n_deltaa = 0.00;
      86             :         float c_n_deltar = 0.1;
      87             :         float deltaa_max = 0.3491;
      88             :         float deltae_max = 0.3491;
      89             :         float deltar_max = 0.3491;
      90             :         // the X CoG offset should be -0.02, but that makes the plane too tail heavy
      91             :         // in manual flight. Adjusted to -0.15 gives reasonable flight
      92             :         Vector3f CGOffset{-0.15, 0, -0.05};
      93             :     } default_coefficients;
      94             : 
      95             :     struct Coefficients coefficient;
      96             : 
      97             :     float thrust_scale;
      98             :     bool reverse_thrust;
      99             :     bool elevons;
     100             :     bool vtail;
     101             :     bool dspoilers;
     102             :     bool redundant;
     103             :     bool reverse_elevator_rudder;
     104             :     bool ice_engine;
     105             :     bool tailsitter;
     106             :     bool aerobatic;
     107             :     bool copter_tailsitter;
     108             :     bool have_launcher;
     109             :     bool have_steering;
     110             :     float launch_accel;
     111             :     float launch_time;
     112             :     uint64_t launch_start_ms;
     113             : 
     114             :     const uint8_t throttle_servo = 2;
     115             :     const int8_t choke_servo = 14;
     116             :     const int8_t ignition_servo = 12;
     117             :     const int8_t starter_servo = 13;
     118             :     const float slewrate = 100;
     119             :     ICEngine icengine{
     120             :         throttle_servo,
     121             :         choke_servo,
     122             :         ignition_servo,
     123             :         starter_servo,
     124             :         slewrate,
     125             :         true
     126             :     };
     127             : 
     128             :     // load aero coefficients from a json model file
     129             :     void load_coeffs(const char *model_json);
     130             :     float liftCoeff(float alpha) const;
     131             :     float dragCoeff(float alpha) const;
     132             :     Vector3f getForce(float inputAileron, float inputElevator, float inputRudder) const;
     133             :     Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, float inputThrust, const Vector3f &force) const;
     134             :     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel);
     135             : 
     136             : private:
     137             :     // json parsing helpers (TODO reduce code duplication)
     138             :     void parse_float(AP_JSON::value val, const char* label, float &param);
     139             :     void parse_vector3(AP_JSON::value val, const char* label, Vector3f &param);
     140             : 
     141             :     void update_battery(const struct sitl_input &input) override;
     142             :     // Plane needs the sitl_input, should never use the parent's input-free version.
     143             :     // (Using it would be using the wrong battery model, but it would silently work.)
     144           0 :     void update_battery() override { INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); };
     145    64559893 :     bool battery_is_empty() { return battery_voltage < 0.5f; };
     146             : };
     147             : 
     148             : } // namespace SITL