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 #include "AISim.hpp"
24
25 #include <cmath>
26 #include <limits>
27 #include <stdio.h>
28
29 #ifdef ENABLE_SP_FDM
30 # include <simgear/constants.h>
31 # include <simgear/math/simd.hxx>
32 # include <simgear/math/simd4x4.hxx>
33 # include <simgear/math/sg_geodesy.hxx>
34
35 # include <Aircraft/controls.hxx>
36 # include <Main/fg_props.hxx>
37 # include <Main/globals.hxx>
38 # include <FDM/flight.hxx>
39 #else
40 # include "simd.hxx"
41 # include "simd4x4.hxx"
42 #endif
43
44 #define INCHES_TO_FEET 0.08333333333f
45 #ifndef _MINMAX
46 # define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
47 #endif
48
49
FGAISim(double dt)50 FGAISim::FGAISim(double dt) :
51 br(0),
52 location_geod(0.0),
53 NEDdot(0.0f),
54 vUVW(0.0f),
55 vUVWdot(0.0f),
56 vPQR(0.0f),
57 vPQRdot(0.0f),
58 AOA(0.0f),
59 AOAdot(0.0f),
60 euler(0.0f),
61 euler_dot(0.0f),
62 wind_ned(0.0f),
63 vUVWaero(0.0f),
64 b_2U(0.0f),
65 cbar_2U(0.0f),
66 velocity(0.0f),
67 mach(0.0f),
68 agl(0.0f),
69 WoW(true),
70 no_engines(0),
71 no_gears(0),
72 cg(0.0f),
73 I(0.0f),
74 S(0.0f),
75 cbar(0.0f),
76 b(0.0f),
77 m(0.0f),
78 gravity_ned(0.0f, 0.0f, AISIM_G),
79 rho(0.0f),
80 qbar(0.0f),
81 sigma(0.0f)
82 {
83 simd4x4::zeros(xCDYLT);
84 simd4x4::zeros(xClmnT);
85
86 xCq = xCadot = 0.0f;
87 xCp = xCr = 0.0f;
88
89 for (int i=0; i<AISIM_MAX; ++i) {
90 FT[i] = FTM[i] = MT[i] = 0.0f;
91
92 gear_pos[i] = 0.0f;
93 Cg_spring[i] = Cg_damp[i] = 0.0f;
94 }
95
96
97 #ifdef ENABLE_SP_FDM
98 SGPropertyNode_ptr aero = fgGetNode("sim/aero", true);
99 load(aero->getStringValue());
100 #else
101 load("");
102 #endif
103
104 set_rudder_norm(0.0f);
105 set_elevator_norm(0.0f);
106 set_aileron_norm(0.0f);
107 set_throttle_norm(0.0f);
108 set_flaps_norm(0.0f);
109 set_brake_norm(0.0f);
110
111 set_velocity_fps(0.0f);
112 set_alpha_rad(0.0f);
113 set_beta_rad(0.0f);
114
115 /* useful constants */
116 xCp[SIDE] = CYp;
117 xCr[SIDE] = CYr;
118 xCp[ROLL] = Clp;
119 xCr[ROLL] = Clr;
120 xCp[YAW] = Cnp;
121 xCr[YAW] = Cnr;
122
123 xCq[LIFT] = CLq;
124 xCadot[LIFT] = CLadot;
125
126 xCq[PITCH] = Cmq;
127 xCadot[PITCH] = Cmadot;
128
129 xCDYLT.ptr()[MIN][LIFT] = CLmin;
130 xCDYLT.ptr()[MIN][DRAG] = CDmin;
131
132 inv_m = 1.0f/m;
133
134 // calculate the initial c.g. position above ground level to position
135 // the aircraft on it's wheels.
136 for (int i=0; i<no_gears; ++i) {
137 // convert from structural frame to body frame
138 gear_pos[i] = simd4_t<float,3>(cg[X]-gear_pos[i][X], gear_pos[i][Y]-cg[Y], cg[Z]-gear_pos[i][Z])*INCHES_TO_FEET;
139 if (gear_pos[i][Z] < agl) agl = gear_pos[i][Z];
140 }
141 // Contact point at the Genter of Gravity
142 if (no_gears > (AISIM_MAX-1)) no_gears = AISIM_MAX-1;
143 gear_pos[no_gears] = cg*INCHES_TO_FEET;
144 Cg_spring[no_gears] = 20000.0f;
145 Cg_damp[no_gears] = 2000.0f;
146 no_gears++;
147
148 simd4x4_t<float,4> mcg;
149 simd4x4::unit(mcg);
150 simd4x4::translate(mcg, cg);
151
152 mI = simd4x4_t<float,4>( I[XX], 0.0f, -I[XZ], 0.0f,
153 0.0f, I[YY], 0.0f, 0.0f,
154 -I[XZ], 0.0f, I[ZZ], 0.0f,
155 0.0f, 0.0f, 0.0f, 0.0f);
156 mIinv = invert_inertia(mI);
157 // mI *= mcg;
158 // mIinv *= matrix_inverse(mcg);
159 }
160
~FGAISim()161 FGAISim::~FGAISim()
162 {
163 }
164
165 // Initialize the AISim flight model, dt is the time increment for
166 // each subsequent iteration through the EOM
167 void
init()168 FGAISim::init() {
169 #ifdef ENABLE_SP_FDM
170 // do init common to all the FDM's
171 common_init();
172
173 // now do init specific to the AISim
174 SG_LOG( SG_FLIGHT, SG_INFO, "Starting initializing AISim" );
175
176 // right now agl holds the lowest contact point of the aircraft
177 set_Altitude( get_Altitude()-agl );
178 set_location_geod( get_Latitude(), get_Longitude(), get_Altitude() );
179 set_altitude_agl_ft( get_Altitude_AGL() );
180 set_altitude_agl_ft( 0.0f );
181
182 set_euler_angles_rad( get_Phi(), get_Theta(), get_Psi() );
183
184 set_velocity_fps( fgGetFloat("sim/presets/uBody-fps"),
185 fgGetFloat("sim/presets/vBody-fps"),
186 fgGetFloat("sim/presets/wBody-fps"));
187 #endif
188 }
189
190 void
update(double ddt)191 FGAISim::update(double ddt)
192 {
193 #ifdef ENABLE_SP_FDM
194 if (is_suspended() || ddt == 0)
195 return;
196 #endif
197
198 // initialize all of AISim vars
199 float dt = float(ddt);
200 float inv_dt = 1.0f/dt;
201
202 #ifdef ENABLE_SP_FDM
203 copy_to_AISim();
204 #endif
205
206 /* Earth-to-Body-Axis Transformation Matrix */
207 /* matrices to compensate for pitch, roll and heading */
208 simd4_t<float,3> vector = euler;
209 float angle = simd4::normalize(vector);
210 simd4x4_t<float,4> mNed2Body = simd4x4::rotation_matrix(angle, vector);
211 simd4x4_t<float,4> mBody2Ned = simd4x4::transpose(mNed2Body);
212 simd4_t<float,3> gravity_body = mNed2Body*gravity_ned;
213
214 /* Air-Relative velocity vector */
215 vUVWaero = vUVW + mNed2Body*wind_ned;
216 update_velocity( simd4::magnitude( simd4_t<float,2>(vUVWaero) ) );
217
218 simd4_t<float,3> WindAxis = AOA;
219 float alpha = (vUVWaero[W] == 0) ? 0 : std::atan2(vUVWaero[W], vUVWaero[U]);
220 set_alpha_rad( _MINMAX(alpha, -0.0873f, 0.349f) ); // -5 to 20 degrees
221
222 float beta = (velocity == 0) ? 0 : std::asin(vUVWaero[V]/velocity);
223 set_beta_rad( _MINMAX(beta, -0.2618f, 0.2618f) ); // -15 to 15 degrees
224 AOAdot = (AOA - WindAxis)*inv_dt;
225
226 printf("velocity: %f, vUVWaero: %3.2f, %3.2f, %3.2f, mach: %3.2f\n", velocity, vUVWaero[0], vUVWaero[1], vUVWaero[2], mach);
227 printf("agl: %5.4f, alpha: %5.4f, beta: %5.4f, adot: %5.4f, bdot: %5.4f\n", agl, AOA[ALPHA], AOA[BETA], AOAdot[ALPHA], AOAdot[BETA]);
228
229 /* Force and Moment Coefficients */
230 /* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */
231 float p = vPQR[P];
232 float q = vPQR[Q];
233 float r = vPQR[R];
234 float adot = AOAdot[ALPHA];
235
236 /*
237 * CDYL[LIFT] = (CLq*q + CLadot*adot)*cbar_2U;
238 * Clmn[PITCH] = (Cmq*q + Cmadot*adot)*cbar_2U;
239 *
240 * CDYL[SIDE] = (CYp*p + CYr*r)*b_2U;
241 * Clmn[ROLL] = (Clp*p + Clr*r)*b_2U;
242 * Clmn[YAW] = (Cnp*p + Cnr*r)*b_2U;
243 */
244 simd4_t<float,4> Ccbar2U = (xCq*q + xCadot*adot)*cbar_2U;
245 simd4_t<float,4> Cb2U = (xCp*p + xCr*r )*b_2U;
246
247 simd4_t<float,4> CDYL(0.0f, Cb2U[SIDE], Ccbar2U[LIFT]);
248 simd4_t<float,4> Clmn(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW]);
249 for (int i=0; i<4; ++i) {
250 CDYL += simd4_t<float,4>(xCDYLT.m4x4()[i]);
251 Clmn += simd4_t<float,4>(xClmnT.m4x4()[i]);
252 }
253 float CL = CDYL[LIFT];
254 CDYL += simd4_t<float,3>(CDi*CL*CL, 0.0f, 0.0f);
255 printf("CDYL: %7.2f, %7.2f, %7.2f\n", CDYL[DRAG], CDYL[SIDE], CDYL[LIFT]);
256 #if 0
257 printf(" CLa: %6.3f, CLadot: %6.3f, CLq: %6.3f\n", xCDYLT.ptr()[ALPHA][LIFT],CLadot*adot,CLq*q);
258 printf(" CDa: %6.3f, CDb: %6.3f, CDi: %6.3f\n", xCDYLT.ptr()[ALPHA][DRAG],xCDYLT.ptr()[BETA][DRAG],CDi*CL*CL);
259 printf(" CYb: %6.3f, CYp: %6.3f, CYr: %6.3f\n", xCDYLT.ptr()[BETA][SIDE],CYp*p,CYr*r);
260 printf(" Cma: %6.3f, Cmadot: %6.3f, Cmq: %6.3f\n", xClmnT.ptr()[ALPHA][PITCH],Cmadot*adot,Cmq*q);
261 printf(" Clb: %6.3f, Clp: %6.3f, Clr: %6.3f\n", xClmnT.ptr()[BETA][ROLL],Clp*p,Clr*r);
262 printf(" Cnb: %6.3f, Cnp: %6.3f, Cnr: %6.3f\n", xClmnT.ptr()[BETA][YAW],Cnp*p,Cnr*r);
263
264 printf(" Cmde: %6.3f\n", xClmnT.ptr()[ELEVATOR][PITCH]);
265 printf(" CYdr: %6.3f, Cldr: %6.3f, Cndr: %6.3f\n", xCDYLT.ptr()[RUDDER][SIDE], xClmnT.ptr()[RUDDER][ROLL], xClmnT.ptr()[RUDDER][YAW]);
266 printf(" Clda: %6.3f, CYda: %6.3f\n", xClmnT.ptr()[AILERON][ROLL], xClmnT.ptr()[AILERON][YAW]);
267 printf(" Cldf: %6.3f, CDdf: %6.3f, Cmdf: %6.3f\n", xCDYLT.ptr()[FLAPS][LIFT], xCDYLT.ptr()[FLAPS][DRAG], xClmnT.ptr()[FLAPS][PITCH]);
268 #endif
269
270 /* State Accelerations (convert coefficients to forces and moments) */
271 simd4_t<float,3> FDYL = CDYL*Coef2Force;
272 simd4_t<float,3> Mlmn = Clmn*Coef2Moment;
273
274 /* convert from wind axes to body axes */
275 vector = AOA;
276 angle = simd4::normalize(vector);
277 simd4x4_t<float,4> mWindBody = simd4x4::rotation_matrix(angle, vector);
278 simd4_t<float,3> FXYZ = mWindBody*FDYL;
279
280 /* Thrust */
281 for (int i=0; i<no_engines; ++i) {
282 FXYZ += FT[i]*th + FTM[i]*mach;
283 // Mlmn += MT*th;
284
285 printf("FDYL: %7.2f, %7.2f, %7.2f\n", FDYL[DRAG], FDYL[SIDE], FDYL[LIFT]);
286 printf("FT: %10.2f, %7.2f, MT: %7.2f\n", FT[i][X]*th, FTM[i][X]*mach, MT[i][X]*th);
287 }
288
289 /* gear forces */
290 WoW = false;
291 if (agl < 100.0f) {
292 int WoW_main = 0;
293 simd4_t<float,3> cg_agl_neu(0.0f, 0.0f, agl);
294 for (int i=0; i<no_gears; ++i) {
295 simd4_t<float,3> gear_ned = mBody2Ned*gear_pos[i];
296 simd4_t<float,3> lg_ground_neu = gear_ned + cg_agl_neu;
297 if (lg_ground_neu[Z] < 0.0f) { // weight on wheel
298 simd4_t<float,3> lg_vrot = simd4::cross(vPQR, gear_pos[i]);
299 simd4_t<float,3> lg_cg_vned = mBody2Ned*lg_vrot;
300 simd4_t<float,3> lg_vned = NEDdot + lg_cg_vned;
301 float Fn;
302
303 Fn = Cg_spring[i]*lg_ground_neu[Z] - Cg_damp[i]*lg_vned[Z];
304 if (Fn > 0.0f) Fn = 0.0f;
305
306 simd4_t<float,3> Fn_lg(0.0f, 0.0f, Fn);
307 simd4_t<float,3> mu_body(-0.02f-0.7f*br, 0.8f, 0.0f);
308
309 simd4_t<float,3> FLGear = mNed2Body*Fn_lg + vUVWaero*mu_body;
310 // simd4_t<float,3> MLGear = simd4::cross(gear_pos[i], FLGear);
311 #if 0
312 printf("FLGear[%i]: %10.2f %10.2f %10.2f\n",i,FLGear[0], FLGear[1], FLGear[2]);
313 printf("MLGear[%i]: %10.2f %10.2f %10.2f\n",i,MLGear[0], MLGear[1], MLGear[2]);
314 #endif
315
316 FXYZ += FLGear;
317 // Mlmn += MLGear;
318 if (i<3) WoW_main++;
319 }
320 WoW = (WoW_main == 3);
321 }
322 }
323
324 /* local body accelrations */
325 aXYZ = FXYZ*inv_m + gravity_body;
326 printf("aXYZ: %7.2f, %7.2f, %7.2f\n", aXYZ[X]/m, aXYZ[Y]/m, aXYZ[Z]/m);
327 printf("Mlmn: %7.2f, %7.2f, %7.2f\n", Mlmn[ROLL], Mlmn[PITCH], Mlmn[YAW]);
328
329 /* Dynamic Equations */
330
331 /* body-axis translational accelerations: forward, sideward, upward */
332 vUVWdot = aXYZ - simd4::cross(vPQR, vUVW);
333 vUVW += vUVWdot*dt;
334
335 /* body-axis rotational accelerations: rolling, pitching, yawing */
336 vPQRdot = mIinv*(Mlmn - vPQR*(mI*vPQR));
337 vPQR += vPQRdot*dt;
338
339 printf("PQRdot: %7.2f, %7.2f, %7.2f\n", vPQRdot[P], vPQRdot[Q], vPQRdot[R]);
340 printf("PQR: %7.2f, %7.2f, %7.2f\n", vPQR[P], vPQR[Q], vPQR[R]);
341 printf("UVWdot: %7.2f, %7.2f, %7.2f\n", vUVWdot[U], vUVWdot[V], vUVWdot[W]);
342 printf("UVW: %7.2f, %7.2f, %7.2f\n", vUVW[U], vUVW[V], vUVW[W]);
343
344 /* position of center of mass wrt earth: north, east, down */
345 NEDdot = mBody2Ned*vUVW;
346 simd4_t<float,3> NEDdist = NEDdot*dt;
347 printf("NEDdot: %7.2f, %7.2f, %7.2f\n", NEDdot[0], NEDdot[1], NEDdot[2]);
348 printf("NEDdist: %7.2f, %7.2f, %7.2f\n", NEDdist[0], NEDdist[1], NEDdist[2]);
349
350 #ifdef ENABLE_SP_FDM
351 double dist = simd4::magnitude( simd4_t<float,2>(NEDdist) );
352
353 double lat2 = 0.0, lon2 = 0.0, az2 = 0.0;
354 geo_direct_wgs_84( 0.0, location_geod[LATITUDE] * SGD_RADIANS_TO_DEGREES,
355 location_geod[LONGITUDE] * SGD_RADIANS_TO_DEGREES,
356 euler[PSI] * SGD_RADIANS_TO_DEGREES,
357 dist * SG_FEET_TO_METER, &lat2, &lon2, &az2 );
358 set_location_geod( lat2 * SGD_DEGREES_TO_RADIANS,
359 lon2 * SGD_DEGREES_TO_RADIANS,
360 location_geod[ALTITUDE] - NEDdist[DOWN] );
361 // set_heading_rad( az2 * SGD_DEGREES_TO_RADIANS );
362 #else
363 location_geod[X] += NEDdist[X];
364 location_geod[Y] += NEDdist[Y];
365 location_geod[Z] -= NEDdist[Z];
366 set_altitude_agl_ft(location_geod[DOWN]);
367 #endif
368 printf("GEOD: %7.2f, %7.2f, %7.2f\n", location_geod[0], location_geod[1], location_geod[2]);
369
370 /* angle of body wrt earth: phi (pitch), theta (roll), psi (heading) */
371 float sin_p = std::sin(euler[PHI]);
372 float cos_p = std::cos(euler[PHI]);
373 float sin_t = std::sin(euler[THETA]);
374 float cos_t = std::cos(euler[THETA]);
375 if (std::abs(cos_t) < 0.00001f) cos_t = std::copysign(0.00001f,cos_t);
376
377 euler_dot[PSI] = (q*sin_p + r*cos_p)/cos_t;
378 euler_dot[THETA] = q*cos_p - r*sin_p;
379 // euler_dot[PHI] = p + (q*sin_p + r*cos_p)*sin_t/cos_t;
380 euler_dot[PHI] = p + euler_dot[PSI]*sin_t;
381 euler += euler_dot*dt;
382 printf("euler: %7.2f, %7.2f, %7.2f\n", euler[PHI], euler[THETA], euler[PSI]);
383
384 #ifdef ENABLE_SP_FDM
385 copy_from_AISim();
386 #endif
387 }
388
389 #ifdef ENABLE_SP_FDM
390 bool
copy_to_AISim()391 FGAISim::copy_to_AISim()
392 {
393 set_rudder_norm(globals->get_controls()->get_rudder());
394 set_elevator_norm(globals->get_controls()->get_elevator());
395 set_aileron_norm(globals->get_controls()->get_aileron());
396 set_flaps_norm(globals->get_controls()->get_flaps());
397 set_throttle_norm(globals->get_controls()->get_throttle(0));
398 set_brake_norm(0.5f*(globals->get_controls()->get_brake_left()
399 +globals->get_controls()->get_brake_right()));
400
401 set_altitude_asl_ft(get_Altitude());
402 set_altitude_agl_ft(get_Altitude_AGL());
403 // set_location_geod(get_Latitude(), get_Longitude(), location_geod[ALTITUDE]);
404 // set_velocity_fps(get_V_calibrated_kts());
405
406 return true;
407 }
408
409 bool
copy_from_AISim()410 FGAISim::copy_from_AISim()
411 {
412 // Positions
413 _set_Geodetic_Position( location_geod[LATITUDE], location_geod[LONGITUDE]);
414
415 double sl_radius, lat_geoc;
416 sgGeodToGeoc(location_geod[LATITUDE], location_geod[ALTITUDE], &sl_radius,&lat_geoc);
417 _set_Geocentric_Position( lat_geoc, location_geod[LONGITUDE], sl_radius);
418
419 _update_ground_elev_at_pos();
420 _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET);
421 _set_Altitude( location_geod[ALTITUDE] );
422 _set_Altitude_AGL( location_geod[ALTITUDE] - get_Runway_altitude() );
423
424 // _set_Euler_Angles( roll, pitch, heading );
425 float heading = euler[PSI];
426 if (heading < 0) heading += SGD_2PI;
427 _set_Euler_Angles( euler[PHI], euler[THETA], heading );
428 _set_Alpha( AOA[ALPHA] );
429 _set_Beta( AOA[BETA] );
430
431 // Velocities
432 _set_V_equiv_kts( velocity*std::sqrt(sigma) * SG_FPS_TO_KT );
433 _set_V_calibrated_kts( std::sqrt( 2*qbar*sigma/rho) * SG_FPS_TO_KT );
434 set_V_ground_speed_kt( simd4::magnitude(NEDdot) * SG_FPS_TO_KT );
435 _set_Mach_number( mach );
436
437 _set_Velocities_Local( NEDdot[NORTH], NEDdot[EAST], NEDdot[DOWN] );
438 // _set_Velocities_Local_Airmass( vUVWaero[U], vUVWaero[V], vUVWaero[W] );
439 _set_Velocities_Body( vUVW[U], vUVW[V], vUVW[W] );
440 _set_Omega_Body( vPQR[P], vPQR[Q], vPQR[R] );
441 _set_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
442
443 // Accelerations
444 // set_Accels_Omega( vPQRdot[P], vPQRdot[Q], vPQRdot[R] );
445 _set_Accels_Body( vUVWdot[U], vUVWdot[V], vUVWdot[W] );
446
447 return true;
448 }
449 #endif
450
451 // ----------------------------------------------------------------------------
452
453 #ifndef _MINMAX
454 # define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
455 #endif
456
457 // 1976 Standard Atmosphere - Density (slugs/ft2): 0 - 101,000 ft
458 float FGAISim::density[101] = {
459 0.0023771699, 0.0023083901, 0.0022411400, 0.0021753900, 0.0021111399,
460 0.0020483399, 0.0019869800, 0.0019270401, 0.0018685000, 0.0018113200,
461 0.0017554900, 0.0017009900, 0.0016477900, 0.0015958800, 0.0015452200,
462 0.0014958100, 0.0014476100, 0.0014006100, 0.0013547899, 0.0013101200,
463 0.0012665900, 0.0012241700, 0.0011828500, 0.0011426000, 0.0011034100,
464 0.0010652600, 0.0010281201, 0.0009919840, 0.0009568270, 0.0009226310,
465 0.0008893780, 0.0008570500, 0.0008256280, 0.0007950960, 0.0007654340,
466 0.0007366270, 0.0007086570, 0.0006759540, 0.0006442340, 0.0006140020,
467 0.0005851890, 0.0005577280, 0.0005315560, 0.0005066120, 0.0004828380,
468 0.0004601800, 0.0004385860, 0.0004180040, 0.0003983890, 0.0003796940,
469 0.0003618760, 0.0003448940, 0.0003287090, 0.0003132840, 0.0002985830,
470 0.0002845710, 0.0002712170, 0.0002584900, 0.0002463600, 0.0002347990,
471 0.0002237810, 0.0002132790, 0.0002032710, 0.0001937320, 0.0001846410,
472 0.0001759760, 0.0001676290, 0.0001595480, 0.0001518670, 0.0001445660,
473 0.0001376250, 0.0001310260, 0.0001247530, 0.0001187880, 0.0001131160,
474 0.0001077220, 0.0001025920, 0.0000977131, 0.0000930725, 0.0000886582,
475 0.0000844590, 0.0000804641, 0.0000766632, 0.0000730467, 0.0000696054,
476 0.0000663307, 0.0000632142, 0.0000602481, 0.0000574249, 0.0000547376,
477 0.0000521794, 0.0000497441, 0.0000474254, 0.0000452178, 0.0000431158,
478 0.0000411140, 0.0000392078, 0.0000373923, 0.0000356632, 0.0000340162,
479 0.0000324473
480 };
481
482 // 1976 Standard Atmosphere - Speed of sound (ft/s): 0 - 101,000 ft
483 float FGAISim::vsound[101] = {
484 1116.450, 1112.610, 1108.750, 1104.880, 1100.990, 1097.090, 1093.180,
485 1089.250, 1085.310, 1081.360, 1077.390, 1073.400, 1069.400, 1065.390,
486 1061.360, 1057.310, 1053.250, 1049.180, 1045.080, 1040.970, 1036.850,
487 1032.710, 1028.550, 1024.380, 1020.190, 1015.980, 1011.750, 1007.510,
488 1003.240, 998.963, 994.664, 990.347, 986.010, 981.655, 977.280,
489 972.885, 968.471, 968.076, 968.076, 968.076, 968.076, 968.076,
490 968.076, 968.076, 968.076, 968.076, 968.076, 968.076, 968.076,
491 968.076, 968.076, 968.076, 968.076, 968.076, 968.076, 968.076,
492 968.076, 968.076, 968.076, 968.076, 968.076, 968.076, 968.076,
493 968.076, 968.076, 968.076, 968.337, 969.017, 969.698, 970.377,
494 971.056, 971.735, 972.413, 973.091, 973.768, 974.445, 975.121,
495 975.797, 976.472, 977.147, 977.822, 978.496, 979.169, 979.842,
496 980.515, 981.187, 981.858, 982.530, 983.200, 983.871, 984.541,
497 985.210, 985.879, 986.547, 987.215, 987.883, 988.550, 989.217,
498 989.883, 990.549, 991.214
499 };
500
501 void
update_velocity(float v)502 FGAISim::update_velocity(float v)
503 {
504 velocity = v;
505
506 /* altitude related */
507 float alt_idx = _MINMAX(location_geod[ALTITUDE]/1000.0f, 0, 100);
508 int idx = std::floor(alt_idx);
509 float fract = alt_idx - idx;
510 float ifract = 1.0f - fract;
511
512 /* linear interpolation for density */
513 rho = ifract*density[idx] + fract*density[idx+1];
514 qbar = 0.5f*rho*v*v;
515 sigma = rho/density[0];
516
517 float Sqbar = S*qbar;
518 float Sbqbar = Sqbar*b;
519 float Sqbarcbar = Sqbar*cbar;
520
521 /* Drag, Side, Lift */
522 Coef2Force = simd4_t<float,3>(-Sqbar, Sqbar, -Sqbar, Sqbar);
523
524 /* Roll, Pitch, Yaw */
525 Coef2Moment = simd4_t<float,3>(Sbqbar, Sqbarcbar, Sbqbar, Sbqbar);
526
527 /* linear interpolation for speed of sound */
528 float vs = ifract*vsound[idx] + fract*vsound[idx+1];
529 mach = v/vs;
530
531 /* useful semi-constants */
532 if (v < 0.0000001f) v = 0.0000001f;
533 b_2U = 0.5f*b/v;
534 cbar_2U = 0.5f*cbar/v;
535 }
536
537 simd4x4_t<float,4>
matrix_inverse(simd4x4_t<float,4> mtx)538 FGAISim::matrix_inverse(simd4x4_t<float,4> mtx)
539 {
540 simd4x4_t<float,4> dst;
541 simd4_t<float,4> v1, v2;
542
543 dst = simd4x4::transpose(mtx);
544
545 v1 = simd4_t<float,4>(mtx.m4x4()[3]);
546 v2 = simd4_t<float,4>(mtx.m4x4()[0]);
547 dst.ptr()[3][0] = -simd4::dot(v1, v2);
548
549 v2 = simd4_t<float,4>(mtx.m4x4()[1]);
550 dst.ptr()[3][1] = -simd4::dot(v1, v2);
551
552 v2 = simd4_t<float,4>(mtx.m4x4()[2]);
553 dst.ptr()[3][2] = -simd4::dot(v1, v2);
554
555 return dst;
556 }
557
558 simd4x4_t<float,4>
invert_inertia(simd4x4_t<float,4> mtx)559 FGAISim::invert_inertia(simd4x4_t<float,4> mtx)
560 {
561 float Ixx, Iyy, Izz, Ixz;
562 float k1, k3, k4, k6;
563 float denom;
564
565 Ixx = mtx.ptr()[0][0];
566 Iyy = mtx.ptr()[1][1];
567 Izz = mtx.ptr()[2][2];
568 Ixz = -mtx.ptr()[0][2];
569
570 k1 = Iyy*Izz;
571 k3 = Iyy*Ixz;
572 denom = 1.0f/(Ixx*k1 - Ixz*k3);
573
574 k1 *= denom;
575 k3 *= denom;
576 k4 = (Izz*Ixx - Ixz*Ixz)*denom;
577 k6 = Ixx*Iyy*denom;
578
579 return simd4x4_t<float,4>( k1, 0.0f, k3, 0.0f,
580 0.0f, k4, 0.0f, 0.0f,
581 k3, 0.0f, k6, 0.0f,
582 0.0f, 0.0f, 0.0f, 0.0f );
583 }
584
585
586 bool
load(std::string path)587 FGAISim::load(std::string path)
588 {
589 /* defaults for the Cessna 172p */
590 S = 174.0f;
591 cbar = 4.90f;
592 b = 35.8f;
593
594 m = 2267.0f/AISIM_G; // max: 2650.0f;
595
596 I[XX] = 948.0f;
597 I[YY] = 1346.0f;
598 I[ZZ] = 1967.0f;
599
600 // Center of Gravity and gears are in the structural frame
601 // X-axis is directed afterwards,
602 // Y-axis is directed towards the right,
603 // Z-axis is directed upwards
604 // positions are in inches
605 cg[X] = -2.2f;
606 cg[Y] = 0.0f;
607 cg[Z] = -22.5f;
608
609 // gear ground contact points relative tot aero ref. pt. at (0,0,0)
610 no_gears = 3;
611 /* nose */
612 gear_pos[0][X] = -50.0f;
613 gear_pos[0][Y] = 0.0f;
614 gear_pos[0][Z] = -78.9f;
615 Cg_spring[0] = 1800.0f;
616 Cg_damp[0] = 600.0f;
617 /* left */
618 gear_pos[1][X] = 15.0f;
619 gear_pos[1][Y] = -43.0f;
620 gear_pos[1][Z] = -74.9f;
621 Cg_spring[1] = 5400.0f;
622 Cg_damp[1] = 1600.0f;
623 /* right */
624 gear_pos[2][X] = 15.0f;
625 gear_pos[2][Y] = 43.0f;
626 gear_pos[2][Z] = -74.9f;
627 Cg_spring[2] = 5400.0f;
628 Cg_damp[2] = 1600.0f;
629
630 float de_max = 24.0f*SGD_DEGREES_TO_RADIANS;
631 float dr_max = 16.0f*SGD_DEGREES_TO_RADIANS;
632 float da_max = 17.5f*SGD_DEGREES_TO_RADIANS;
633 float df_max = 30.0f*SGD_DEGREES_TO_RADIANS;
634
635 /* thuster / propulsion */
636 // (FWD,RIGHT,DOWN)
637 simd4_t<float,3> dir(1.0f,0.0f,0.0f);
638 no_engines = 1;
639
640 CTmax = 0.057f/144;
641 CTu = -0.096f;
642
643 // if propeller driven
644 float D = 75.0f*INCHES_TO_FEET;
645 float RPS = 2700.0f/60.0f;
646 float Ixx = 1.67f; // propeller
647
648 simd4::normalize(dir);
649 FT[0] = dir * (CTmax * RPS*RPS * D*D*D*D); // Thrust force
650 FTM[0] = dir * (CTu/(RPS*D))*vsound[0]; // Thrust force due to Mach
651 MT[0] = dir * (-Ixx*(2.0f*RPS*float(SGD_PI)));// Thrus moment
652 // if propeller driven
653
654 /* aerodynamic coefficients */
655 CLmin = 0.307f;
656 CLa = 5.13f;
657 CLadot = 1.70f;
658 CLq = 3.90f;
659 CLdf_n = 0.705f*df_max;
660
661 CDmin = 0.027f;
662 CDa = 0.158f;
663 CDb = 0.192f;
664 CDi = 0.0446f;
665 CDdf_n = 0.052f*df_max;
666
667 CYb = -0.31f;
668 CYp = 0.006f;
669 CYr = 0.262f;
670 CYdr_n = 0.091f*dr_max;
671
672 #if 0
673 Clb = -0.057f;
674 Clp = -0.613f;
675 Clr = 0.079f;
676 Clda_n = 0.170f*da_max;
677 Cldr_n = 0.01f*dr_max;
678 #else
679 Clb = 0.0;
680 Clp = 0.0;
681 Clr = 0.0f;
682 Clda_n = 0.0f*da_max;
683 Cldr_n = 0.0f*dr_max;
684 #endif
685
686 Cma = -1.0f;
687 Cmadot = -4.42f;
688 Cmq = -10.5f;
689 Cmde_n = -1.05f*de_max;
690 Cmdf_n = -0.059f*df_max;
691
692 #if 0
693 Cnb = 0.0630f;
694 Cnp = -0.0028f;
695 Cnr = -0.0681f;
696 Cnda_n = -0.0100f*da_max;
697 Cndr_n = -0.0398f*dr_max;
698 #else
699 Cnb = 0.0f;
700 Cnp = 0.0f;
701 Cnr = 0.0f;
702 Cnda_n = 0.0f*da_max;
703 Cndr_n = 0.0f*dr_max;
704 #endif
705
706 return true;
707 }
708
709
710 // Register the subsystem.
711 #if 0
712 SGSubsystemMgr::Registrant<FGAISim> registrantFGAISim;
713 #endif
714