fly/flightgear
1
0
Fork 0
Code Activity

Add the first version of AISim but

Browse source
This commit is contained in:
Erik Hofman 2016-12-07 11:05:07 +01:00
parent 7ed4a5162e
commit 7ec7a8b60c
2 changed files with 731 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

514
src/FDM/SP/AISim.cpp Normal file
View file

@ -0,0 +1,514 @@
// AISim.cxx -- interface to the AI Sim
//
// Written by Erik Hofman, started November 2016
//
// Copyright (C) 2016 Erik Hofman <erik@ehofman.com>
//
//
// 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 2 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, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
#include <cmath>
#include <limits>
#include <stdio.h>
#include <simgear/constants.h>
#include <simgear/math/simd.hxx>
#include <simgear/math/simd4x4.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <Aircraft/controls.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
#include <FDM/flight.hxx>
#include "AISim.hpp"
FGAISim::FGAISim(double dt)
{
SGPropertyNode_ptr aero = fgGetNode("sim/aero", true);
load(aero->getStringValue());
load("");
for (int i=0; i<4; i++) {
xCDYLT[i] = 0.0f;
xClmnT[i] = 0.0f;
}
PQR_body_prev = 0.0f;
ABY_body_prev = 0.0f;
br = 0.0f;
UVW_body = 0.0f;
velocity = 0.0f;
NED_cm = 0.0f;
PQR_body = 0.0f;
ABY_body = 0.0f;
ABY_dot = 0.0f;
euler = 0.0f;
wind_ned = 0.0f;
agl = 0.0f;
copy_to_AISim();
set_location_geod(get_Latitude(), get_Longitude(), get_Altitude());
set_alpha_rad(0.0f);
set_beta_rad(0.0f);
set_velocity_fps(0.0f);
/* useful constants */
FThrust = 0.0f;
MThrust = 0.0f;
gravity = 0.0f; gravity[DOWN] = G;
dt2_2m = 0.5f*dt*dt/m;
}
FGAISim::~FGAISim()
{
}
// Initialize the AISim flight model, dt is the time increment for
// each subsequent iteration through the EOM
void
FGAISim::init() {
// do init common to all the FDM's
common_init();
// now do init specific to the AISim
SG_LOG( SG_FLIGHT, SG_INFO, "Starting initializing AISim" );
set_location_geod( get_Latitude(), get_Longitude(), get_Altitude() );
set_euler_angles_rad( get_Phi(), get_Theta(), get_Psi() );
UVW_body[U] = fgGetFloat("sim/presets/uBody-fps");
UVW_body[V] = fgGetFloat("sim/presets/vBody-fps");
UVW_body[W] = fgGetFloat("sim/presets/wBody-fps");
set_velocity_fps( simd4::magnitude(UVW_body) );
}
void
FGAISim::update(double ddt)
{
if (is_suspended())
return;
// initialize all of AISim vars
float dt = float(ddt);
copy_to_AISim();
ABY_dot = (ABY_body - ABY_body_prev)*dt;
PQR_dot = (PQR_body - PQR_body_prev)*dt;
/* update the history */
PQR_body_prev = PQR_body;
ABY_body_prev = ABY_body;
// Earth-to-Body-Axis Transformation Matrix
// body = pitch, roll, yaw
simd4_t<float,3> angles = euler;
float angle = simd4::normalize(angles);
simd4x4_t<float,4> EBM = simd4x4::rotation_matrix<float>(angle, angles);
// Body-Axis Gravity Components
simd4_t<float,4> gb = EBM*simd4_t<float,4>(gravity);
simd4_t<float,4> windb = EBM*simd4_t<float,4>(wind_ned);
// Air-Relative velocity vector
simd4_t<float,3> Va = UVW_body + simd4_t<float,3>(windb);
update_UVW_body(simd4::magnitude(Va));
if (std::abs(velocity) > 1.0f) {
set_alpha_rad( std::atan(Va[3]/std::abs(Va[1])) );
set_beta_rad( std::asin(Va[2]/velocity) );
}
// Force and Moment Coefficients
simd4_t<float,4> CDYLT = 0.0f;
simd4_t<float,4> ClmnT = 0.0f;
for (int i=0; i<4; i++) {
CDYLT += xCDYLT[i];
ClmnT += xClmnT[i];
}
float p = PQR_body[P];
float q = PQR_body[Q];
float r = PQR_body[R];
do {
float adot = ABY_dot[ALPHA];
float CL = CDYLT[LIFT];
CDYLT[DRAG] += CDi*CL*CL;
CDYLT[LIFT] += (CLadot*adot + CLq*q)*cbar_2U;
CDYLT[SIDE] += (CYp*p + CYr*r)*b_2U;
ClmnT[ROLL] += (Clp*p + Clr*r)*b_2U;
ClmnT[YAW] += (Cnp*p + Cnr*r)*b_2U;
ClmnT[PITCH] += (Cmadot*adot + Cmq*q)*cbar_2U;
}
while (0);
// State Accelerations
simd4_t<float,4> FDYLT = CDYLT*C2F;
simd4_t<float,4> MlmnT = ClmnT*C2M;
/* convert from wind axes to body axes */
simd4_t<float,3> aby = ABY_body;
angle = simd4::normalize(aby);
simd4x4_t<float,4> WBM = simd4x4::rotation_matrix<float>(angle, aby);
simd4_t<float,4>FXYZ = WBM*FDYLT;
// Thrust -- todo: propulsion in non x-directions
FThrust[X] = FDYLT[THRUST];
MThrust[ROLL] = MlmnT[THRUST];
FXYZ += FThrust;
// MlmnT += MThrust;
// Gear forces - http://tutorial.math.lamar.edu/Classes/DE/Vibrations.aspx
if (0) { // WoW
simd4_t<float,3> FLMLG = 0.0f, FRLMG = 0.0f, FNLG = 0.0f;
float gear_comp = 1.0f + _MINMAX(agl/gear[Z], -1.0f, 0.0f);
FNLG[Z] = FLMLG[Z] = FRLMG[Z] = Cgear*gear_comp;
simd4_t<float,3> FLGear = 0.0f, MLGear = 0.0f;
FXYZ += FLGear;
MlmnT += MLGear;
}
FXYZ /= m;
// Dynamic Equations
/* body-axis velocity: forward, sideward, upward */
float u = UVW_body[U];
float v = UVW_body[V];
float w = UVW_body[W];
simd4_t<float,3> dUVW = FXYZ + gb;
dUVW[U] += r*v - q*w;
dUVW[V] += -r*u + p*w;
dUVW[W] += q*u - p*v;
UVW_body += dUVW;
printf("UVW: %5.4f, %5.4f, %5.4f\n", dUVW[U], dUVW[V], dUVW[W]);
/* position of center of mass wrt earth: north, east, down */
simd4_t<float,4> dNED_cm;
dNED_cm = simd4x4::transpose(EBM)*simd4_t<float,4>(UVW_body);
double az2, dist = simd4::magnitude(dNED_cm);
SGGeod pos2, pos = getPosition();
geo_direct_wgs_84 ( pos, euler[PSI] * SGD_RADIANS_TO_DEGREES,
dist, pos2, &az2 );
set_location_geod( pos2.getLatitudeRad(),
pos2.getLongitudeRad(), pos.getElevationFt() );
/* body-axis iniertial rates: pitching, rolling, yawing */
simd4_t<float,3> dPQR;
dPQR[P] = I[ZZ]*MlmnT[ROLL]-(I[ZZ]+I[YY])*r*q/I[XX];
dPQR[Q] = MlmnT[PITCH]-(I[XX]+I[ZZ])*p*r;
dPQR[R] = I[XX]*MlmnT[YAW] +(I[XX]-I[YY])*p*q/I[ZZ];
// PQR_body += dPQR*dt;
/* angle of body wrt earth: phi (roll), theta (pitch), psi (yaw) */
float cos_t = std::cos(euler[THETA]);
float sin_t = std::sin(euler[THETA]);
if (std::abs(cos_t) < 0.00001f) cos_t = std::copysign(0.00001f,cos_t);
float sin_p = std::sin(euler[PHI]);
float cos_p = std::cos(euler[PHI]);
simd4_t<float,3> deuler;
deuler[PHI] = P+(sin_p*Q+cos_p*r)*sin_t/cos_t;
deuler[THETA] = cos_p*q-sin_p*r;
deuler[PSI] = (sin_p*q+cos_p*r) / cos_t;
euler += deuler;
copy_from_AISim();
}
bool
FGAISim::copy_to_AISim()
{
set_rudder_norm(globals->get_controls()->get_rudder());
set_elevator_norm(globals->get_controls()->get_elevator());
set_aileron_norm(globals->get_controls()->get_aileron());
set_flaps_norm(globals->get_controls()->get_flaps());
set_throttle_norm(globals->get_controls()->get_throttle(0));
set_brake_norm(0.5f*(globals->get_controls()->get_brake_left()
+globals->get_controls()->get_brake_right()));
set_altitude_agl_ft(get_Altitude_AGL());
// set_location_geod(get_Latitude(), get_Longitude(), get_Altitude());
// set_velocity_fps(get_V_calibrated_kts());
return true;
}
bool
FGAISim::copy_from_AISim()
{
// Accelerations
// _set_Accels_Omega( PQR_body[P], PQR_body[Q], PQR_body[R] );
// Velocities
_set_Velocities_Local( UVW_body[U], UVW_body[V], UVW_body[W] );
_set_Omega_Body( PQR_body[P], PQR_body[Q], PQR_body[R] );
// Positions
double sl_radius, lat_geoc;
sgGeodToGeoc( NED_cm[LATITUDE], NED_cm[ALTITUDE], &sl_radius, &lat_geoc );
_set_Geocentric_Position( lat_geoc, NED_cm[LONGITUDE], sl_radius);
// _update_ground_elev_at_pos();
// _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET);
_set_Euler_Angles( euler[PHI], euler[THETA], euler[PSI] );
_set_Alpha( ABY_body[ALPHA] );
_set_Beta( ABY_body[BETA] );
return true;
}
void
FGAISim::update_UVW_body(float f)
{
velocity = f;
xCDYLT[VELOCITY][THRUST] = CTu*f;
if (std::abs(f) < 0.00001f) f = std::copysign(0.00001f,f);
b_2U = 0.5f*b/f;
cbar_2U = 0.5f*cbar/f;
update_qbar();
}
// ----------------------------------------------------------------------------
void
FGAISim::update_qbar()
{
unsigned int hi = _MINMAX(std::rint(NED_cm[ALTITUDE]/1000.0f), 0, 100);
float rho = env[hi][RHO];
float qbar = 0.5f*rho*velocity*velocity;
float Sqbar = S*qbar;
float Sbqbar = Sqbar*b;
float Sqbarcbar = Sqbar*cbar;
C2F = Sqbar;
C2F[THRUST] = rho*RPS2D4*CTmax;
C2M = Sbqbar;
C2M[PITCH] = Sqbarcbar;
}
// 1976 Standard Atmosphere
// Density Speed of sound
// slugs/ft2 ft/s
float FGAISim::env[101][2] =
{
{ 0.00237717f, 1116.45f },
{ 0.00230839f, 1112.61f },
{ 0.00224114f, 1108.75f },
{ 0.00217539f, 1104.88f },
{ 0.00211114f, 1100.99f },
{ 0.00204834f, 1097.09f },
{ 0.00198698f, 1093.18f },
{ 0.00192704f, 1089.25f },
{ 0.00186850f, 1085.31f },
{ 0.00181132f, 1081.36f },
{ 0.00175549f, 1077.39f },
{ 0.00170099f, 1073.40f },
{ 0.00164779f, 1069.40f },
{ 0.00159588f, 1065.39f },
{ 0.00154522f, 1061.36f },
{ 0.00149581f, 1057.31f },
{ 0.00144761f, 1053.25f },
{ 0.00140061f, 1049.18f },
{ 0.00135479f, 1045.08f },
{ 0.00131012f, 1040.97f },
{ 0.00126659f, 1036.85f },
{ 0.00122417f, 1032.71f },
{ 0.00118285f, 1028.55f },
{ 0.00114260f, 1024.38f },
{ 0.00110341f, 1020.19f },
{ 0.00106526f, 1015.98f },
{ 0.00102812f, 1011.75f },
{ 0.000991984f, 1007.51f },
{ 0.000956827f, 1003.24f },
{ 0.000922631f, 998.963f },
{ 0.000889378f, 994.664f },
{ 0.000857050f, 990.347f },
{ 0.000825628f, 986.010f },
{ 0.000795096f, 981.655f },
{ 0.000765434f, 977.280f },
{ 0.000736627f, 972.885f },
{ 0.000708657f, 968.471f },
{ 0.000675954f, 968.076f },
{ 0.000644234f, 968.076f },
{ 0.000614002f, 968.076f },
{ 0.000585189f, 968.076f },
{ 0.000557728f, 968.076f },
{ 0.000531556f, 968.076f },
{ 0.000506612f, 968.076f },
{ 0.000482838f, 968.076f },
{ 0.000460180f, 968.076f },
{ 0.000438586f, 968.076f },
{ 0.000418004f, 968.076f },
{ 0.000398389f, 968.076f },
{ 0.000379694f, 968.076f },
{ 0.000361876f, 968.076f },
{ 0.000344894f, 968.076f },
{ 0.000328709f, 968.076f },
{ 0.000313284f, 968.076f },
{ 0.000298583f, 968.076f },
{ 0.000284571f, 968.076f },
{ 0.000271217f, 968.076f },
{ 0.000258490f, 968.076f },
{ 0.000246360f, 968.076f },
{ 0.000234799f, 968.076f },
{ 0.000223781f, 968.076f },
{ 0.000213279f, 968.076f },
{ 0.000203271f, 968.076f },
{ 0.000193732f, 968.076f },
{ 0.000184641f, 968.076f },
{ 0.000175976f, 968.076f },
{ 0.000167629f, 968.337f },
{ 0.000159548f, 969.017f },
{ 0.000151867f, 969.698f },
{ 0.000144566f, 970.377f },
{ 0.000137625f, 971.056f },
{ 0.000131026f, 971.735f },
{ 0.000124753f, 972.413f },
{ 0.000118788f, 973.091f },
{ 0.000113116f, 973.768f },
{ 0.000107722f, 974.445f },
{ 0.000102592f, 975.121f },
{ 0.0000977131f, 975.797f },
{ 0.0000930725f, 976.472f },
{ 0.0000886582f, 977.147f },
{ 0.0000844590f, 977.822f },
{ 0.0000804641f, 978.496f },
{ 0.0000766632f, 979.169f },
{ 0.0000730467f, 979.842f },
{ 0.0000696054f, 980.515f },
{ 0.0000663307f, 981.187f },
{ 0.0000632142f, 981.858f },
{ 0.0000602481f, 982.530f },
{ 0.0000574249f, 983.200f },
{ 0.0000547376f, 983.871f },
{ 0.0000521794f, 984.541f },
{ 0.0000497441f, 985.210f },
{ 0.0000474254f, 985.879f },
{ 0.0000452178f, 986.547f },
{ 0.0000431158f, 987.215f },
{ 0.0000411140f, 987.883f },
{ 0.0000392078f, 988.550f },
{ 0.0000373923f, 989.217f },
{ 0.0000356632f, 989.883f },
{ 0.0000340162f, 990.549f },
{ 0.0000324473f, 991.214f }
};
bool
FGAISim::load(std::string path)
{
/* defaults for the Cessna 182, taken from Roskam */
S = 172.0f;
cbar = 4.90f;
b = 36.0f;
m = 2650.0f*G;
I[XX] = 948.0f;
I[YY] = 1346.0f;
I[ZZ] = 1967.0f;
// gear ground contact points relative tot cg at (0,0,0)
float no_gears = 3.0f;
Cgear = 5400.0f*no_gears;
#if 1
gear[X] = 0.0f*INCHES_TO_FEET;
gear[Y] = 0.0f*INCHES_TO_FEET;
gear[Z] = -54.4f*INCHES_TO_FEET;
#else
/* nose */
gear[0][X] = -47.8f*INCHES_TO_FEET;
gear[0][Y] = 0.0f*INCHES_TO_FEET;
gear[0][Z] = -54.4f*INCHES_TO_FEET;
/* left */
gear[1][X] = 17.2f*INCHES_TO_FEET;
gear[1][Y] = -43.0f*INCHES_TO_FEET;
gear[1][Z] = -54.4f*INCHES_TO_FEET;
/* right */
gear[2][X] = 17.2f*INCHES_TO_FEET;
gear[2][Y] = 43.0f*INCHES_TO_FEET;
gear[2][Z] = -54.4f*INCHES_TO_FEET;
#endif
float de_max = 24.0f*SG_DEGREES_TO_RADIANS;
float dr_max = 16.0f*SG_DEGREES_TO_RADIANS;
float da_max = 17.5f*SG_DEGREES_TO_RADIANS;
float df_max = 30.0f*SG_DEGREES_TO_RADIANS;
no_engines = 1.0f;
CTmax = 0.073f*no_engines;
CTu = -0.0960f*no_engines;
CLmin = 0.307f;
CLa = 4.410f;
CLadot = 1.70f;
CLq = 3.90f;
CLdf_n = 0.6685f*df_max;
CDmin = 0.0270f;
CDa = 0.121f;
CDb = 0.0f;
CDi = 0.0f;
CDdf_n = 0.0816f*df_max;
CYb = -0.393f;
CYp = -0.0750f;
CYr = 0.214f;
CYdr_n = 0.187f*dr_max;
Clb = -0.0923f;
Clp = -0.484f;
Clr = 0.0798f;
Clda_n = 0.2290f*da_max;
Cldr_n = 0.0147f*dr_max;
Cma = -0.613f;
Cmadot = -7.27f;
Cmq = -12.40f;
Cmde_n = -1.122f*de_max;
Cmdf_n = -0.2177f*df_max;
Cnb = 0.0587f;
Cnp = -0.0278f;
Cnr = -0.0937;
Cnda_n = -0.0216f*da_max;
Cndr_n = -0.0645f*dr_max;
/* thuster / propulsion */
float D = 75.0f*INCHES_TO_FEET;
float RPS = 2700.0f/60.0f;
float prop_Ixx = 1.67f;
C2M[THRUST] = prop_Ixx*(2.0f*RPS*PI)/no_engines;
RPS2D4 = RPS*RPS * D*D*D*D;
return true;
}

217
src/FDM/SP/AISim.hpp Normal file
View file

@ -0,0 +1,217 @@
// AISim.cxx -- interface to the AI Sim
//
// Written by Erik Hofman, started November 2016
//
// Copyright (C) 2016 Erik Hofman <erik@ehofman.com>
//
//
// 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 2 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, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
#ifndef _FGAISim_HXX
#define _FGAISim_HXX
#include <string>
#include <simgear/math/SGVec3.hxx>
#include <FDM/flight.hxx>
#define G 32.174f
#define PI 3.1415926535f
#define INCHES_TO_FEET 0.0833333333f
#define FPS_TO_KTS 0.592484313161f
#ifndef _MINMAX
# define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
#endif
class FGAISim : public FGInterface
{
private:
enum { X=0, Y=1, Z=2 };
enum { XX=0, YY=1, ZZ=2, XZ=3 };
enum { LATITUDE=0, LONGITUDE=1, ALTITUDE=2 };
enum { NORTH=0, EAST=2, DOWN=3, N=0, E=2, D=3 };
enum { LEFT=0, RIGHT=1 };
enum { MAX=0, VELOCITY=1 };
enum { PROPULSION=0, FLAPS=2, RUDDER=2, MIN=3, AILERON=3, ELEVATOR=3 };
enum { DRAG=0, SIDE=1, LIFT=2, THRUST=3 };
enum { ROLL=0, PITCH=1, YAW=2 };
enum { ALPHA=0, BETA=1 };
enum { RHO=0, VSOUND=1 };
enum { ALT=3, ASL=3 };
enum { PHI, THETA, PSI };
enum { P=0, Q=1, R=2 };
enum { U=0, V=1, W=2 };
public:
FGAISim(double dt);
~FGAISim();
// reset flight params to a specific location
void init();
// update location based on properties
void update(double dt);
bool load(std::string path);
// copy FDM state to AISim structures
bool copy_to_AISim();
// copy AISim structures to FDM state
bool copy_from_AISim();
/* controls */
inline void set_rudder_norm(float f) {
xCDYLT[RUDDER][SIDE] = CYdr_n*f;
xClmnT[RUDDER][ROLL] = Cldr_n*f;
xClmnT[RUDDER][YAW] = Cndr_n*f;
}
inline void set_elevator_norm(float f) {
xClmnT[ELEVATOR][PITCH] = Cmde_n*f;
}
inline void set_aileron_norm(float f) {
xClmnT[AILERON][ROLL] = Clda_n*f;
xClmnT[AILERON][YAW] = Cnda_n*f;
}
inline void set_flaps_norm(float f) {
xCDYLT[FLAPS][LIFT] = CLdf_n*f;
xCDYLT[FLAPS][DRAG] = CDdf_n*f;
xClmnT[FLAPS][PITCH] = Cmdf_n*f;
}
inline void set_throttle_norm(float f) {
xCDYLT[MAX][THRUST] = f;
xClmnT[PROPULSION][THRUST] = f;
}
inline void set_brake_norm(float f) { br = f; }
/* (initial) state, local frame */
inline void set_location_geod(const SGVec3f& p) {
NED_cm = p.simd3(); update_qbar();
}
inline void set_location_geod(float lat, float lon, float alt) {
NED_cm[LATITUDE] = lat;
NED_cm[LONGITUDE] = lon;
NED_cm[ALTITUDE] = alt;
update_qbar();
}
inline void set_altitude_asl_ft(float f) { NED_cm[DOWN] = -f; };
inline void set_altitude_agl_ft(float f) { agl = _MINMAX(f, 0.0f, 100000.0f); }
inline void set_pitch_rad(float f) { euler[PHI] = f; }
inline void set_roll_rad(float f) { euler[THETA] = f; }
inline void set_yaw_rad(float f) { euler[PSI] = f; }
inline void set_euler_angles_rad(const SGVec3f& o) { euler = o.simd3(); }
inline void set_euler_angles_rad(float phi, float theta, float psi) {
euler[PHI] = phi;
euler[THETA] = theta;
euler[PSI] = psi;
}
void set_velocity_fps(float f) {
UVW_body = 0.0f; UVW_body[X] = f; update_UVW_body(f);
}
inline void set_wind_ned(const SGVec3f& w) { wind_ned = w.simd3(); }
inline void set_wind_ned(float n, float e, float d) {
wind_ned[NORTH] = n;
wind_ned[EAST] = e;
wind_ned[DOWN] = d;
}
inline void set_alpha_rad(float f) {
xCDYLT[ALPHA][DRAG] = CDa*std::abs(f);
xCDYLT[ALPHA][LIFT] = CLa*f;
xClmnT[ALPHA][PITCH] = Cma*f;
ABY_body[ALPHA] = f;
}
inline void set_beta_rad(float f) {
xCDYLT[BETA][DRAG] = CDb*std::abs(f);
xCDYLT[BETA][SIDE] = CYb*f;
xClmnT[BETA][ROLL] = Clb*f;
xClmnT[BETA][YAW] = Cnb*f;
ABY_body[BETA] = f;
}
inline float get_alpha_rad() {
return ABY_body[ALPHA];
}
inline float get_beta_rad() {
return ABY_body[BETA];
}
private:
void update_qbar();
void update_UVW_body(float f);
/* aircraft normalized controls */
float br; /* brake */
/* aircraft state */
simd4_t<float,3> NED_cm; /* lat, lon, altitude */
simd4_t<float,3> UVW_body; /* fwd, up, side speed */
simd4_t<float,3> PQR_body; /* P, Q, R rate */
simd4_t<float,3> PQR_dot; /* Pdot, Qdot, Rdot accel. */
simd4_t<float,3> ABY_body; /* alpha, beta, gamma */
simd4_t<float,3> ABY_dot; /* adot, bdot, ydot */
simd4_t<float,3> euler; /* phi, theta, psi */
simd4_t<float,3> wind_ned;
float agl, velocity;
/* history, these change between every call to update() */
simd4_t<float,3> PQR_body_prev, ABY_body_prev;
/* ---------------------------------------------------------------- */
/* This should reduce the time spent in update() since controls */
/* change less often than the update function runs which might */
/* run 20 to 60 times (or more) per second */
/* cache, values that don't change very often */
simd4_t<float,3> FThrust, MThrust;
float b_2U, cbar_2U;
/* dynamic coefficients (already multiplied with their value) */
simd4_t<float,4> xCDYLT[4];
simd4_t<float,4> xClmnT[4];
simd4_t<float,4> C2F, C2M;
/* ---------------------------------------------------------------- */
/* aircraft static data */
int no_engines;
simd4_t<float,3> gear; /* NED_cms in structural frame */
simd4_t<float,3> I; /* inertia */
float S, cbar, b; /* wing area, mean average chord, span */
float m; /* mass */
float RPS2D4; /* propeller diameter, ening RPS */
/* static oefficients, *_n is for normalized surface deflection */
float Cgear; /* gear */
float CTmax, CTu; /* thrust max, due to speed */
float CLmin, CLa, CLadot, CLq, CLdf_n;
float CDmin, CDa, CDb, CDi, CDdf_n;
float CYb, CYp, CYr, CYdr_n;
float Clb, Clp, Clr, Clda_n, Cldr_n;
float Cma, Cmadot, Cmq, Cmde_n, Cmdf_n;
float Cnb, Cnp, Cnr, Cnda_n, Cndr_n;
/* static environment data */
static float env[101][2];
simd4_t<float,3> gravity;
float dt2_2m;
};
#endif // _FGAISim_HXX