ardupilot/libraries/SITL/SIM_Plane.h

111 lines
3.4 KiB
C
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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/>.
*/
/*
simple plane simulator class
*/
#pragma once
#include "SIM_Aircraft.h"
2016-07-15 05:29:12 -03:00
#include "SIM_ICEngine.h"
namespace SITL {
/*
a very simple plane simulator
*/
class Plane : public Aircraft {
public:
Plane(const char *home_str, const char *frame_str);
/* update model by one time step */
virtual void update(const struct sitl_input &input);
/* static object creator */
static Aircraft *create(const char *home_str, const char *frame_str) {
return new Plane(home_str, frame_str);
}
protected:
const float hover_throttle = 0.7f;
const float air_density = 1.225; // kg/m^3 at sea level, ISA conditions
float angle_of_attack;
float beta;
struct {
// from last_letter skywalker_2013/aerodynamics.yaml
// thanks to Georacer!
float s = 0.45;
float b = 1.88;
float c = 0.24;
float c_lift_0 = 0.56;
float c_lift_deltae = 0;
float c_lift_a = 6.9;
float c_lift_q = 0;
float mcoeff = 50;
float oswald = 0.9;
float alpha_stall = 0.4712;
float c_drag_q = 0;
float c_drag_deltae = 0.0;
float c_drag_p = 0.1;
float c_y_0 = 0;
float c_y_b = -0.98;
float c_y_p = 0;
float c_y_r = 0;
float c_y_deltaa = 0;
float c_y_deltar = -0.2;
float c_l_0 = 0;
float c_l_p = -1.0;
float c_l_b = -0.12;
float c_l_r = 0.14;
float c_l_deltaa = 0.25;
float c_l_deltar = -0.037;
float c_m_0 = 0.045;
float c_m_a = -0.7;
float c_m_q = -20;
float c_m_deltae = 1.0;
float c_n_0 = 0;
float c_n_b = 0.25;
float c_n_p = 0.022;
float c_n_r = -1;
float c_n_deltaa = 0.00;
float c_n_deltar = 0.1;
float deltaa_max = 0.3491;
float deltae_max = 0.3491;
float deltar_max = 0.3491;
// the X CoG offset should be -0.02, but that makes the plane too tail heavy
// in manual flight. Adjusted to -0.15 gives reasonable flight
Vector3f CGOffset{-0.15, 0, -0.05};
} coefficient;
float thrust_scale;
2016-01-30 04:40:45 -04:00
bool reverse_thrust;
bool elevons;
bool vtail;
bool reverse_elevator_rudder;
2016-07-15 05:29:12 -03:00
bool ice_engine;
2016-07-23 04:36:10 -03:00
ICEngine icengine{2, 14, 12, 13, 100};
float liftCoeff(float alpha) const;
float dragCoeff(float alpha) const;
Vector3f getForce(float inputAileron, float inputElevator, float inputRudder) const;
Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const;
void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
};
} // namespace SITL