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