1 // AISim.cxx -- interface to the AI Sim
2 //
3 // Written by Erik Hofman, started November 2016
4 //
5 // Copyright (C) 2016,2017  Erik Hofman <erik@ehofman.com>
6 //
7 //
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License as
10 // published by the Free Software Foundation; either version 2 of the
11 // License, or (at your option) any later version.
12 //
13 // This program is distributed in the hope that it will be useful, but
14 // WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16 // General Public License for more details.
17 //
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
21 //
22 
23 
24 #ifndef _FGAISim_HXX
25 #define _FGAISim_HXX
26 
27 #ifdef HAVE_CONFIG_H
28 #  include <config.h>
29 #endif
30 
31 #include <string>
32 #include <cmath>
33 
34 #ifdef ENABLE_SP_FDM
35 # include <simgear/math/simd.hxx>
36 # include <simgear/constants.h>
37 # include <FDM/flight.hxx>
38 #else
39 # include "simd.hxx"
40 # include "simd4x4.hxx"
41 # define SG_METER_TO_FEET               3.2808399
42 # define SG_FEET_TO_METER               (1/SG_METER_TO_FEET)
43 # define SGD_DEGREES_TO_RADIANS         0.0174532925
44 # define SGD_RADIANS_TO_DEGREES         (1/SGD_DEGREES_TO_RADIANS)
45 # define SGD_PI                         3.1415926535
46 #endif
47 
48 // #define SG_DEGREES_TO_RADIANS 0.0174532925f
49 
50 // max. no. gears, maxi. no. engines
51 #define AISIM_MAX       4
52 #define AISIM_G         32.174f
53 
54 class FGAISim
55 #ifdef ENABLE_SP_FDM
56         : public FGInterface
57 #endif
58 {
59 private:
60     enum { X=0, Y=1, Z=2 };
61     enum { XX=0, YY=1, ZZ=2, XZ=3 };
62     enum { LATITUDE=0, LONGITUDE=1, ALTITUDE=2 };
63     enum { NORTH=0, EAST=1, DOWN=2 };
64     enum { LEFT=0, RIGHT=1, UP=2 };
65     enum { MAX=0, VELOCITY=1, PROPULSION=2 };
66     enum { FLAPS=2, RUDDER=2, MIN=3, AILERON=3, ELEVATOR=3 };
67     enum { DRAG=0, SIDE=1, LIFT=2 };
68     enum { ROLL=0, PITCH=1, YAW=2, THRUST=3 };
69     enum { ALPHA=0, BETA=1 };
70     enum { PHI, THETA, PSI };
71     enum { P=0, Q=1, R=2 };
72     enum { U=0, V=1, W=2 };
73 
74 public:
75     FGAISim(double dt);
76     ~FGAISim();
77 
78     // Subsystem API.
79     void init() override;
80     void update(double dt) override;
81 
82     // Subsystem identification.
staticSubsystemClassId()83     static const char* staticSubsystemClassId() { return "aisim"; }
84 
85     bool load(std::string path);
86 
87 #ifdef ENABLE_SP_FDM
88     // copy FDM state to AISim structures
89     bool copy_to_AISim();
90 
91     // copy AISim structures to FDM state
92     bool copy_from_AISim();
93 #endif
94 
95     /* controls */
set_rudder_norm(float f)96     inline void set_rudder_norm(float f) {
97         xCDYLT.ptr()[RUDDER][SIDE] = CYdr_n*f;
98         xClmnT.ptr()[RUDDER][ROLL] = Cldr_n*f;
99         xClmnT.ptr()[RUDDER][YAW] = Cndr_n*f;
100     }
set_elevator_norm(float f)101     inline void set_elevator_norm(float f) {
102         xClmnT.ptr()[ELEVATOR][PITCH] = Cmde_n*f;
103     }
set_aileron_norm(float f)104     inline void set_aileron_norm(float f) {
105         xClmnT.ptr()[AILERON][ROLL] = Clda_n*f;
106         xClmnT.ptr()[AILERON][YAW] = Cnda_n*f;
107     }
set_flaps_norm(float f)108     inline void set_flaps_norm(float f) {
109         xCDYLT.ptr()[FLAPS][LIFT] = CLdf_n*f;
110         xCDYLT.ptr()[FLAPS][DRAG] = CDdf_n*std::abs(f);
111         xClmnT.ptr()[FLAPS][PITCH] = Cmdf_n*f;
112     }
set_throttle_norm(float f)113     inline void set_throttle_norm(float f) { th = f; }
set_brake_norm(float f)114     inline void set_brake_norm(float f) { br = f; }
115 
116     /* (initial) state, local frame */
set_location_geod(const simd4_t<double,3> & p)117     inline void set_location_geod(const simd4_t<double,3>& p) {
118         location_geod = p;
119     }
set_location_geod(double lat,double lon,double alt)120     inline void set_location_geod(double lat, double lon, double alt) {
121         location_geod = simd4_t<double,3>(lat, lon, alt);
122     }
set_altitude_asl_ft(float f)123     inline void set_altitude_asl_ft(float f) { location_geod[ALTITUDE] = f; };
set_altitude_agl_ft(float f)124     inline void set_altitude_agl_ft(float f) { agl = f; }
125 
set_euler_angles_rad(const simd4_t<float,3> & e)126     inline void set_euler_angles_rad(const simd4_t<float,3>& e) {
127         euler = e;
128      }
set_euler_angles_rad(float phi,float theta,float psi)129     inline void set_euler_angles_rad(float phi, float theta, float psi) {
130         euler = simd4_t<float,3>(phi, theta, psi);
131     }
set_pitch_rad(float f)132     inline void set_pitch_rad(float f) { euler[PHI] = f; }
set_roll_rad(float f)133     inline void set_roll_rad(float f) { euler[THETA] = f; }
set_heading_rad(float f)134     inline void set_heading_rad(float f) { euler[PSI] = f; }
135 
set_velocity_fps(const simd4_t<float,3> & v)136     void set_velocity_fps(const simd4_t<float,3>& v) { vUVW = v; }
set_velocity_fps(float u,float v,float w)137     void set_velocity_fps(float u, float v, float w) {
138         vUVW = simd4_t<float,3>(u, v, w);
139     }
set_velocity_fps(float u)140     void set_velocity_fps(float u) {
141         vUVW = simd4_t<float,3>(u, 0.0f, 0.0f);
142     }
143 
set_wind_ned_fps(const simd4_t<float,3> & w)144     inline void set_wind_ned_fps(const simd4_t<float,3>& w) { wind_ned = w; }
set_wind_ned_fps(float n,float e,float d)145     inline void set_wind_ned_fps(float n, float e, float d) {
146         wind_ned = simd4_t<float,3>(n, e, d);
147     }
148 
set_alpha_rad(float f)149     inline void set_alpha_rad(float f) {
150         xCDYLT.ptr()[ALPHA][DRAG] = CDa*std::abs(f);
151         xCDYLT.ptr()[ALPHA][LIFT] = -CLa*f;
152         xClmnT.ptr()[ALPHA][PITCH] = Cma*f;
153         AOA[ALPHA] = f;
154     }
set_beta_rad(float f)155     inline void set_beta_rad(float f) {
156         xCDYLT.ptr()[BETA][DRAG] = CDb*std::abs(f);
157         xCDYLT.ptr()[BETA][SIDE] = CYb*f;
158         xClmnT.ptr()[BETA][ROLL] = Clb*f;
159         xClmnT.ptr()[BETA][YAW] = Cnb*f;
160         AOA[BETA] = f;
161     }
get_alpha_rad()162     inline float get_alpha_rad() {
163         return AOA[ALPHA];
164     }
get_beta_rad()165     inline float get_beta_rad() {
166         return AOA[BETA];
167     }
168 
169 private:
170     void update_velocity(float v);
171     simd4x4_t<float,4> matrix_inverse(simd4x4_t<float,4> mtx);
172     simd4x4_t<float,4> invert_inertia(simd4x4_t<float,4> mtx);
173 
174     /* aircraft normalized controls */
175     float th;                           /* throttle command             */
176     float br;                           /* brake command                */
177 
178     /* aircraft state */
179     simd4_t<double,3> location_geod;    /* lat, lon, altitude           */
180     simd4_t<float,3> aXYZ;              /* local body accelrations      */
181     simd4_t<float,3> NEDdot;            /* North, East, Down velocity   */
182     simd4_t<float,3> vUVW;              /* fwd, side, down velocity     */
183     simd4_t<float,3> vUVWdot;           /* fwd, side, down accel.       */
184     simd4_t<float,3> vPQR;              /* roll, pitch, yaw rate        */
185     simd4_t<float,3> vPQRdot;           /* roll, pitch, yaw accel.      */
186     simd4_t<float,3> AOA;               /* alpha, beta                  */
187     simd4_t<float,3> AOAdot;            /* adot, bdot                   */
188     simd4_t<float,3> euler;             /* phi, theta, psi              */
189     simd4_t<float,3> euler_dot;         /* change in phi, theta, psi    */
190     simd4_t<float,3> wind_ned;          /* wind north, east, down       */
191 
192     /* ---------------------------------------------------------------- */
193     /* This should reduce the time spent in update() since controls     */
194     /* change less often than the update function runs which  might     */
195     /* run 20 to 60 times (or more) per second                          */
196 
197     /* cache */
198     simd4_t<float,3> vUVWaero;          /* airmass relative to the body */
199     simd4_t<float,3> FT[AISIM_MAX];     /* thrust force                 */
200     simd4_t<float,3> FTM[AISIM_MAX];    /* thrust due to mach force     */
201     simd4_t<float,3> MT[AISIM_MAX];     /* thrust moment                */
202     simd4_t<float,3> b_2U, cbar_2U;
203     simd4_t<float,3> inv_m;
204     float velocity, mach;
205     float agl;
206     bool WoW;
207 
208     /* dynamic coefficients (already multiplied with their value) */
209     simd4_t<float,3> xCq, xCadot, xCp, xCr;
210     simd4x4_t<float,4> xCDYLT;
211     simd4x4_t<float,4> xClmnT;
212     simd4_t<float,4> Coef2Force;
213     simd4_t<float,4> Coef2Moment;
214 
215     /* ---------------------------------------------------------------- */
216     /* aircraft static data */
217     int no_engines, no_gears;
218     simd4x4_t<float,4> mI, mIinv;       /* inertia matrix               */
219     simd4_t<float,3> gear_pos[AISIM_MAX]; /* pos in structural frame    */
220     simd4_t<float,3> cg;        /* center of gravity                    */
221     simd4_t<float,4> I;         /* inertia                              */
222     float S, cbar, b;           /* wing area, mean average chord, span  */
223     float m;                    /* mass                                 */
224 
225     /* static coefficients, *_n is for normalized surface deflection    */
226     float Cg_spring[AISIM_MAX]; /* gear spring coeffients               */
227     float Cg_damp[AISIM_MAX];   /* gear damping coefficients            */
228     float CTmax, CTu;           /* thrust max, due to speed             */
229     float CLmin, CLa, CLadot, CLq, CLdf_n;
230     float CDmin, CDa, CDb, CDi, CDdf_n;
231     float CYb, CYp, CYr, CYdr_n;
232     float Clb, Clp, Clr, Clda_n, Cldr_n;
233     float Cma, Cmadot, Cmq, Cmde_n, Cmdf_n;
234     float Cnb, Cnp, Cnr, Cnda_n, Cndr_n;
235 
236     /* environment data */
237     static float density[101], vsound[101];
238     simd4_t<float,3> gravity_ned;
239     float rho, qbar, sigma;
240 };
241 
242 #endif // _FGAISim_HXX
243 
244