fly/flightgear
1
0
Fork 0
Code Activity

A new batch of updates, slowly moving forward

Browse source
This commit is contained in:
Erik Hofman 2017-01-06 15:02:35 +01:00
parent d762db2db8
commit 9a71f6348f
2 changed files with 528 additions and 459 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

770
src/FDM/SP/AISim.cpp
View file

@ -20,55 +20,82 @@
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
// //
#include "AISim.hpp"
#include <cmath> #include <cmath>
#include <limits> #include <limits>
#include <stdio.h> #include <stdio.h>
#ifndef ENABLE_SP_FDM #ifdef ENABLE_SP_FDM
# define ENABLE_SP_FDM 1 # include <simgear/constants.h>
#endif # include <simgear/math/simd.hxx>
# include <simgear/math/simd4x4.hxx>
# include <simgear/math/sg_geodesy.hxx>
#if ENABLE_SP_FDM # include <Aircraft/controls.hxx>
#include <simgear/constants.h> # include <Main/fg_props.hxx>
#include <simgear/math/simd.hxx> # include <Main/globals.hxx>
#include <simgear/math/simd4x4.hxx> # include <FDM/flight.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <Aircraft/controls.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
#include <FDM/flight.hxx>
#else #else
#include "simd.hxx" # include "simd.hxx"
#include "simd4x4.hxx" # include "simd4x4.hxx"
#endif #endif
#include "AISim.hpp"
FGAISim::FGAISim(double dt) :
FGAISim::FGAISim(double dt) br(0),
location_geod(0.0),
NED_distance(0.0f),
NED_body(0.0f),
UVW_body(0.0f),
UVW_dot(0.0f),
PQR_body(0.0f),
PQR_dot(0.0f),
AOA_body(0.0f),
AOA_dot(0.0f),
euler_body(0.0f),
euler_dot(0.0f),
wind_ned(0.0f),
UVW_aero(0.0f),
IQR_dot_fact(0.0f),
agl(0.0f),
velocity(0.0f),
mach(0.0f),
b_2U(0.0f),
cbar_2U(0.0f),
WoW(true),
no_engines(0),
no_gears(0),
cg(0.0f),
I(0.0f),
S(0.0f),
cbar(0.0f),
b(0.0f),
m(0.0f),
gravity_ned(0.0f, 0.0f, AISIM_G),
rho(0.0f),
qbar(0.0f),
sigma(0.0f)
{ {
for (int i=0; i<2; ++i) {
xCqadot[i] = 0.0f;
xCpr[i] = 0.0f;
}
for (int i=0; i<4; ++i) { for (int i=0; i<4; ++i) {
xCDYLT[i] = 0.0f; xCDYL[i] = 0.0f;
xClmnT[i] = 0.0f; xClmn[i] = 0.0f;
} }
PQR_body_prev = 0.0f; for (int i=0; i<AISIM_MAX; ++i) {
ABY_body_prev = 0.0f; FT[i] = FTM[i] = MT[i] = 0.0f;
br = 0.0f; gear_pos[i] = 0.0f;
Cg_spring[i] = Cg_damp[i] = 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;
#if ENABLE_SP_FDM #ifdef ENABLE_SP_FDM
SGPropertyNode_ptr aero = fgGetNode("sim/aero", true); SGPropertyNode_ptr aero = fgGetNode("sim/aero", true);
load(aero->getStringValue()); load(aero->getStringValue());
#else #else
@ -97,15 +124,22 @@ FGAISim::FGAISim(double dt)
xCqadot[LIFT][1] = CLadot; xCqadot[LIFT][1] = CLadot;
xCqadot[PITCH][0] = Cmq; xCqadot[PITCH][0] = Cmq;
xCqadot[PITCH][1] = Cmadot; xCqadot[PITCH][1] = Cmadot;
xCDYLT[MIN][LIFT] = CLmin; xCDYL[MIN][LIFT] = CLmin;
xCDYLT[MIN][DRAG] = CDmin; xCDYL[MIN][DRAG] = CDmin;
C2M[THRUST] = prop_Ixx*(2.0f*RPS*PI)/no_engines; // calculate the initial c.g. position above ground level to position
RPS2D4 = RPS*RPS * D*D*D*D; // the aircraft on it's wheels.
for (int i=0; i<no_gears; ++i) {
if (gear_pos[i][Z] < agl) agl = gear_pos[i][Z];
}
// Genter of Gravity
if (no_gears > (AISIM_MAX-1)) no_gears = AISIM_MAX-1;
gear_pos[no_gears] = 0.0f;
Cg_spring[no_gears] = 20000.0f;
Cg_damp[no_gears] = 2000.0f;
no_gears++;
FThrust = MThrust = 0.0f; IQR_dot_fact = simd4_t<float,3>(I[ZZ]-I[YY], I[XX]-I[ZZ], I[YY]-I[XX]);
gravity = 0.0f; gravity[DOWN] = G;
dt2_2m = 0.5f*dt*dt/m;
} }
FGAISim::~FGAISim() FGAISim::~FGAISim()
@ -116,166 +150,216 @@ FGAISim::~FGAISim()
// each subsequent iteration through the EOM // each subsequent iteration through the EOM
void void
FGAISim::init() { FGAISim::init() {
#if ENABLE_SP_FDM #ifdef ENABLE_SP_FDM
// do init common to all the FDM's // do init common to all the FDM's
common_init(); common_init();
// now do init specific to the AISim // now do init specific to the AISim
SG_LOG( SG_FLIGHT, SG_INFO, "Starting initializing AISim" ); SG_LOG( SG_FLIGHT, SG_INFO, "Starting initializing AISim" );
double sl_radius, lat_geoc; // right now agl holds the lowest contact point of the aircraft
sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc ); set_Altitude( get_Altitude()-agl );
set_location_geoc( lat_geoc, get_Longitude(), get_Altitude() ); set_location_geod( get_Latitude(), get_Longitude(), get_Altitude() );
set_altitude_agl_ft( get_Altitude_AGL() );
set_altitude_agl_ft( 0.0f );
set_euler_angles_rad( get_Phi(), get_Theta(), get_Psi() ); set_euler_angles_rad( get_Phi(), get_Theta(), get_Psi() );
UVW_body[U] = fgGetFloat("sim/presets/uBody-fps"); set_velocity_fps( fgGetFloat("sim/presets/uBody-fps"),
UVW_body[V] = fgGetFloat("sim/presets/vBody-fps"); fgGetFloat("sim/presets/vBody-fps"),
UVW_body[W] = fgGetFloat("sim/presets/wBody-fps"); fgGetFloat("sim/presets/wBody-fps"));
set_velocity_fps( UVW_body[U] );
#endif #endif
} }
void void
FGAISim::update(double ddt) FGAISim::update(double ddt)
{ {
#if ENABLE_SP_FDM #ifdef ENABLE_SP_FDM
if (is_suspended()) if (is_suspended() || ddt == 0)
return; return;
#endif #endif
// initialize all of AISim vars // initialize all of AISim vars
float dt = float(ddt); float dt = float(ddt);
#ifdef ENABLE_SP_FDM
copy_to_AISim(); copy_to_AISim();
ABY_dot = (ABY_body - ABY_body_prev)*dt; #endif
PQR_dot = (PQR_body - PQR_body_prev)*dt;
/* update the history */ /* Earth-to-Body-Axis Transformation Matrix */
PQR_body_prev = PQR_body; /* body = pitch, roll, yaw */
ABY_body_prev = ABY_body; simd4_t<float,3> vector = euler_body;
float angle = simd4::normalize(vector);
simd4x4_t<float,4> EB_mtx = simd4x4::rotation_matrix<float>(angle, vector);
simd4x4_t<float,4> BE_mtx = simd4x4::transpose(EB_mtx);
// Earth-to-Body-Axis Transformation Matrix /* Air-Relative velocity vector */
// body = pitch, roll, yaw UVW_aero = UVW_body + EB_mtx*wind_ned;
simd4_t<float,3> angles = euler; update_velocity( simd4::magnitude(UVW_aero) );
float angle = simd4::normalize(angles); printf("velocity: %f, UVW_aero: %3.2f, %3.2f, %3.2f, mach: %3.2f\n", velocity, UVW_aero[0], UVW_aero[1], UVW_aero[2], mach);
simd4x4_t<float,4> EBM = simd4x4::rotation_matrix<float>(angle, angles);
// Body-Axis Gravity Components simd4_t<float,2> AOA_body_prev = AOA_body;
simd4_t<float,3> windb = EBM*wind_ned; set_alpha_rad(UVW_aero[W]==0 ? 0.0f : std::atan2(UVW_aero[W], UVW_aero[U]));
set_beta_rad(velocity==0 ? 0.0f : std::asin(UVW_aero[V]/velocity) );
AOA_dot = (AOA_body - AOA_body_prev)*(1/dt);
printf("agl: %5.4f, alpha: %5.4f, beta: %5.4f, adot: %5.4f, bdot: %5.4f\n", agl, AOA_body[ALPHA], AOA_body[BETA], AOA_dot[ALPHA], AOA_dot[BETA]);
// Air-Relative velocity vector /* Force and Moment Coefficients */
simd4_t<float,3> Va = UVW_body + windb; /* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */
set_velocity_fps(Va[U]);
printf("velocity: %f, Va: %3.2f, %3.2f, %3.2f, %3.2f\n", velocity, Va[0], Va[1], Va[2], Va[3]);
if (std::abs(Va[1]) > 0.01f) {
set_alpha_rad( std::atan(Va[3]/std::abs(Va[1])) );
}
if (std::abs(velocity) > 0.1f) {
set_beta_rad( std::asin(Va[2]/velocity) );
}
printf("alpha: %5.4f, beta: %5.4f\n", ABY_body[ALPHA], ABY_body[BETA]);
// Force and Moment Coefficients
float p = PQR_body[P]; float p = PQR_body[P];
float q = PQR_body[Q]; float q = PQR_body[Q];
float r = PQR_body[R]; float r = PQR_body[R];
float adot = ABY_dot[ALPHA]; float adot = AOA_dot[ALPHA];
/** /*
* CDYLT[LIFT] = (CLq*q + CLadot*adot)*cbar_2U; * CDYL[LIFT] = (CLq*q + CLadot*adot)*cbar_2U;
* ClmnT[PITCH] = (Cmq*q + Cmadot*adot)*cbar_2U; * Clmn[PITCH] = (Cmq*q + Cmadot*adot)*cbar_2U;
( *
* CDYLT[SIDE] = (CYp*p + CYr*r)*b_2U; * CDYL[SIDE] = (CYp*p + CYr*r)*b_2U;
* ClmnT[ROLL] = (Clp*p + Clr*r)*b_2U; * Clmn[ROLL] = (Clp*p + Clr*r)*b_2U;
* ClmnT[YAW] = (Cnp*p + Cnr*r)*b_2U; * Clmn[YAW] = (Cnp*p + Cnr*r)*b_2U;
*/ */
simd4_t<float,4> Ccbar2U = (xCqadot[0]*q + xCqadot[1]*adot)*cbar_2U; simd4_t<float,3> Ccbar2U = (xCqadot[0]*q + xCqadot[1]*adot)*cbar_2U;
simd4_t<float,4> Cb2U = (xCpr[0]*p + xCpr[1]*r)*b_2U; simd4_t<float,3> Cb2U = (xCpr[0]*p + xCpr[1]*r )*b_2U;
/* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */ simd4_t<float,3> CDYL(0.0f, Cb2U[SIDE], Ccbar2U[LIFT]);
simd4_t<float,4> CDYLT(0.0f, Cb2U[SIDE], Ccbar2U[LIFT], 0.0f); simd4_t<float,3> Clmn(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW]);
simd4_t<float,4> ClmnT(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW], 0.0f);
for (int i=0; i<4; ++i) { for (int i=0; i<4; ++i) {
CDYLT += xCDYLT[i]; CDYL += xCDYL[i];
ClmnT += xClmnT[i]; Clmn += xClmn[i];
} }
float CL = CDYLT[LIFT]; float CL = CDYL[LIFT];
CDYLT += simd4_t<float,4>(CDi*CL*CL, 0.0f, 0.0f, 0.0f); CDYL += simd4_t<float,3>(CDi*CL*CL, 0.0f, 0.0f);
printf("CDYL: %7.2f, %7.2f, %7.2f\n", CDYL[DRAG], CDYL[SIDE], CDYL[LIFT]);
/* State Accelerations (convert coefficients to forces and moments) */ /* State Accelerations (convert coefficients to forces and moments) */
simd4_t<float,4> FDYLT = CDYLT*C2F; simd4_t<float,3> FDYL = CDYL*C2F;
simd4_t<float,4> MlmnT = ClmnT*C2M; simd4_t<float,3> Mlmn = Clmn*C2M;
printf("FDYL: %7.2f, %7.2f, %7.2f\n", FDYL[DRAG], FDYL[SIDE], FDYL[LIFT]);
printf("CDYLT: %7.2f, %7.2f, %7.2f, %7.2f\n", CDYLT[DRAG], CDYLT[SIDE], CDYLT[LIFT], CDYLT[THRUST]);
printf("FDYLT: %7.2f, %7.2f, %7.2f, %7.2f\n", FDYLT[DRAG], FDYLT[SIDE], FDYLT[LIFT], FDYLT[THRUST]);
/* convert from wind axes to body axes */ /* convert from wind axes to body axes */
simd4_t<float,3> aby = ABY_body; // alpha, beta, gamma vector = AOA_body;
angle = simd4::normalize(aby); angle = simd4::normalize(vector);
simd4x4_t<float,4> WB_mtx = simd4x4::rotation_matrix<float>(angle, vector);
simd4_t<float,3>FXYZ = WB_mtx*FDYL;
simd4x4_t<float,4> WBM = simd4x4::rotation_matrix<float>(angle, aby); /* Thrust */
simd4_t<float,3>FXYZ = EBM*gravity + WBM*FDYLT; for (int i=0; i<no_engines; ++i) {
FXYZ += FT[i]*th + FTM[i]*mach;
// Thrust -- todo: propulsion in non x-directions // Mlmn += MT*th;
FThrust[X] = FDYLT[THRUST]; printf("FT: %10.2f, %7.2f, MT: %7.2f\n", FT[i][X]*th, FTM[i][X]*mach, MT[i][X]*th);
FXYZ += FThrust;
// MThrust[ROLL] = MlmnT[THRUST];
// MlmnT += MThrust;
// Gear forces
agl = NED_cm[ALTITUDE];
float gear_comp = 0.2f + _MINMAX(agl/gear[Z], -0.2f, 0.0f);
if (gear_comp > 0.1f) {
simd4_t<float,3> FLGear = 0.0f; FLGear[Z] = Cgear*gear_comp;
// simd4_t<float,3> MLGear = 0.0f;
FXYZ += FLGear;
// MlmnT += MLGear;
} }
FXYZ /= m;
printf("FXYZ: %7.2f, %7.2f, %7.2f, %7.2f\n", FXYZ[X], FXYZ[Y], FXYZ[Z], FXYZ[THRUST]);
printf("MlmnT: %7.2f, %7.2f, %7.2f, %7.2f\n", MlmnT[ROLL], MlmnT[PITCH], MlmnT[YAW], MlmnT[THRUST]);
// Dynamic Equations /* gear forces */
/* body-axis velocity: forward, sideward, upward */ #if 1
simd4_t<float,3> dUVW = FXYZ + simd4::cross(UVW_body, PQR_body); WoW = false;
UVW_body += dUVW; if (agl < 100.0f) {
int WoW_main = 0;
simd4_t<float,3> cg_agl_neu(0.0f, 0.0f, agl);
for (int i=0; i<no_gears; ++i) {
simd4_t<float,3> gear_ned = BE_mtx*gear_pos[i];
simd4_t<float,3> lg_ground_neu = gear_ned + cg_agl_neu;
if (lg_ground_neu[Z] < 0.0f) { // weight on wheel
simd4_t<float,3> lg_vrot = simd4::cross(PQR_body, gear_pos[i]);
simd4_t<float,3> lg_cg_vned = BE_mtx*lg_vrot;
simd4_t<float,3> lg_vned = NED_body + lg_cg_vned;
float Fn;
Fn = Cg_spring[i]*lg_ground_neu[Z] - Cg_damp[i]*lg_vned[Z];
if (Fn > 0.0f) Fn = 0.0f;
simd4_t<float,3> Fn_lg(0.0f, 0.0f, Fn);
simd4_t<float,3> mu_body(-0.02f-0.7f*br, 0.8f, 0.0f);
simd4_t<float,3> FLGear = EB_mtx*Fn_lg + UVW_aero*mu_body;
simd4_t<float,3> MLGear = simd4::cross(gear_pos[i], FLGear);
printf("FLGear[%i]: %10.2f %10.2f %10.2f\n",i,FLGear[0], FLGear[1], FLGear[2]);
printf("MLGear[%i]: %10.2f %10.2f %10.2f\n",i,MLGear[0], MLGear[1], MLGear[2]);
FXYZ += FLGear;
// Mlmn += MLGear;
if (i<3) WoW_main++;
}
WoW = (WoW_main == 3);
}
}
#endif
printf("FXYZ/m:%7.2f, %7.2f, %7.2f\n", FXYZ[X]/m, FXYZ[Y]/m, FXYZ[Z]/m);
printf("Mlmn: %7.2f, %7.2f, %7.2f\n", Mlmn[ROLL], Mlmn[PITCH], Mlmn[YAW]);
/* Dynamic Equations */
/* body-axis rotational accelerations: rolling, pitching, yawing
* PQR_dot[P] = (Mlmn[ROLL] - q*r*(I[ZZ] - I[YY]))/I[XX];
* PQR_dot[Q] = (Mlmn[PITCH] - p*r*(I[XX] - I[ZZ]))/I[YY];
* PQR_dot[R] = ( Mlmn[YAW] - p*q*(I[YY] - I[XX]))/I[ZZ];
* \-------------/
* IQR_dot_fact
*/
simd4_t<float,3> qp(PQR_body[Q], PQR_body[P], PQR_body[P]);
simd4_t<float,3> rq(PQR_body[R], PQR_body[R], PQR_body[Q]);
PQR_dot = (Mlmn - qp*rq*IQR_dot_fact)/I;
PQR_body = PQR_dot*dt;
printf("PQR: %7.2f, %7.2f, %7.2f\n", PQR_body[P], PQR_body[Q], PQR_body[R]);
/* body-axis translational accelerations: forward, sideward, upward
gravity(x,y,z) = EB_mtx*(0,0,AISIM_G)
Udot = FX/m + gravity(x) + Vbody*Rbody - Wbody*Qbody;
Vdot = FY/m + gravity(y) + Wbody*Pbody - Ubody*Rbody;
Wdot = FZ/m + gravity(z) + Ubody*Qbody - Vbody*Pbody;
*/
simd4_t<float,3> gravity_body = EB_mtx*gravity_ned;
UVW_dot = FXYZ*(1.0f/m) + gravity_body + simd4::cross(UVW_aero, PQR_body);
UVW_body += UVW_dot*dt;
if (WoW && UVW_body[Z] > 0) UVW_body[Z] = 0.0f;
printf("UVWdot:%7.2f, %7.2f, %7.2f\n", UVW_dot[U], UVW_dot[V], UVW_dot[W]);
printf("UVW: %7.2f, %7.2f, %7.2f\n", UVW_body[U], UVW_body[V], UVW_body[W]); printf("UVW: %7.2f, %7.2f, %7.2f\n", UVW_body[U], UVW_body[V], UVW_body[W]);
/* position of center of mass wrt earth: north, east, down */ /* position of center of mass wrt earth: north, east, down */
simd4_t<float,3> dNED_cm = simd4x4::transpose(EBM)*UVW_body; NED_body = BE_mtx*UVW_body;
NED_cm += dNED_cm*dt; NED_distance = NED_body*dt;
printf("NED: %7.2f, %7.2f, %7.2f\n", printf("vNED: %7.2f, %7.2f, %7.2f\n", NED_body[0], NED_body[1], NED_body[2]);
NED_cm[LATITUDE], NED_cm[LONGITUDE], NED_cm[ALTITUDE]); printf("dNED: %7.2f, %7.2f, %7.2f\n", NED_distance[0], NED_distance[1], NED_distance[2]);
/* body-axis iniertial rates: pitching, rolling, yawing */ double dist = simd4::magnitude(simd4_t<float,2>(NED_distance));
simd4_t<float,3> dPQR; #ifdef ENABLE_SP_FDM
dPQR[P] = I[ZZ]*MlmnT[ROLL]-(I[ZZ]+I[YY])*r*q/I[XX]; double lat2 = 0.0, lon2 = 0.0, az2 = 0.0;
dPQR[Q] = MlmnT[PITCH]-(I[XX]+I[ZZ])*p*r; geo_direct_wgs_84( 0.0, location_geod[LATITUDE] * SGD_RADIANS_TO_DEGREES,
dPQR[R] = I[XX]*MlmnT[YAW] +(I[XX]-I[YY])*p*q/I[ZZ]; location_geod[LONGITUDE] * SGD_RADIANS_TO_DEGREES,
// PQR_body += dPQR*dt; euler_body[PSI] * SGD_RADIANS_TO_DEGREES,
printf("PQR: %7.2f, %7.2f, %7.2f\n", PQR_body[P], PQR_body[Q], PQR_body[R]); dist * SG_FEET_TO_METER, &lat2, &lon2, &az2 );
set_location_geod( lat2 * SGD_DEGREES_TO_RADIANS,
/* angle of body wrt earth: phi (roll), theta (pitch), psi (yaw) */ lon2 * SGD_DEGREES_TO_RADIANS,
float cos_t = std::cos(euler[THETA]); location_geod[ALTITUDE] - NED_distance[DOWN] );
float sin_t = std::sin(euler[THETA]); // set_heading_rad( az2 * SGD_DEGREES_TO_RADIANS );
#else
location_geod += NED_distance;
set_altitude_agl_ft(location_geod[DOWN]);
#endif
printf("GEOD: %7.2f, %7.2f, %7.2f\n", location_geod[0], location_geod[1], location_geod[2]);
/* angle of body wrt earth: phi (pitch), theta (roll), psi (heading) */
float sin_p = std::sin(euler_body[PHI]);
float cos_p = std::cos(euler_body[PHI]);
float sin_t = std::sin(euler_body[THETA]);
float cos_t = std::cos(euler_body[THETA]);
if (std::abs(cos_t) < 0.00001f) cos_t = std::copysign(0.00001f,cos_t); if (std::abs(cos_t) < 0.00001f) cos_t = std::copysign(0.00001f,cos_t);
float sin_p = std::sin(euler[PHI]); euler_dot[PSI] = (q*sin_p - r*cos_p)/cos_t;
float cos_p = std::cos(euler[PHI]); euler_dot[THETA] = q*cos_p - r*sin_p;
simd4_t<float,3> deuler; // euler_dot[PHI] = p + (q*sin_p + r*cos_p)*sin_t/cos_t;
deuler[PHI] = P+(sin_p*Q+cos_p*r)*sin_t/cos_t; euler_dot[PHI] = p + euler_dot[PSI]*sin_t;
deuler[THETA] = cos_p*q-sin_p*r; euler_body += euler_dot;
deuler[PSI] = (sin_p*q+cos_p*r) / cos_t; printf("euler: %7.2f, %7.2f, %7.2f\n", euler_body[PHI], euler_body[THETA], euler_body[PSI]);
euler += deuler;
printf("euler: %7.2f, %7.2f, %7.2f\n", euler[PHI], euler[THETA], euler[PSI]); #ifdef ENABLE_SP_FDM
copy_from_AISim(); copy_from_AISim();
#endif
} }
#if ENABLE_SP_FDM #ifdef ENABLE_SP_FDM
bool bool
FGAISim::copy_to_AISim() FGAISim::copy_to_AISim()
{ {
@ -287,9 +371,9 @@ FGAISim::copy_to_AISim()
set_brake_norm(0.5f*(globals->get_controls()->get_brake_left() set_brake_norm(0.5f*(globals->get_controls()->get_brake_left()
+globals->get_controls()->get_brake_right())); +globals->get_controls()->get_brake_right()));
set_altitude_asl_ft(get_Altitude());
set_altitude_agl_ft(get_Altitude_AGL()); set_altitude_agl_ft(get_Altitude_AGL());
// set_location_geod(get_Latitude(), get_Longitude(), location_geod[ALTITUDE]);
// set_location_geod(get_Latitude(), get_Longitude(), get_Altitude());
// set_velocity_fps(get_V_calibrated_kts()); // set_velocity_fps(get_V_calibrated_kts());
return true; return true;
@ -298,257 +382,225 @@ FGAISim::copy_to_AISim()
bool bool
FGAISim::copy_from_AISim() FGAISim::copy_from_AISim()
{ {
// Accelerations // Positions
// _set_Accels_Omega( PQR_body[P], PQR_body[Q], PQR_body[R] ); _set_Geodetic_Position( location_geod[LATITUDE], location_geod[LONGITUDE]);
double sl_radius, lat_geoc;
sgGeodToGeoc(location_geod[LATITUDE], location_geod[ALTITUDE], &sl_radius,&lat_geoc);
_set_Geocentric_Position( lat_geoc, location_geod[LONGITUDE], sl_radius);
_update_ground_elev_at_pos();
_set_Sea_level_radius( sl_radius * SG_METER_TO_FEET);
_set_Altitude( location_geod[ALTITUDE] );
_set_Altitude_AGL( location_geod[ALTITUDE] - get_Runway_altitude() );
_set_Euler_Angles( euler_body[PHI], euler_body[THETA], euler_body[PSI] );
_set_Alpha( AOA_body[ALPHA] );
_set_Beta( AOA_body[BETA] );
// Velocities // Velocities
_set_Velocities_Local( UVW_body[U], UVW_body[V], UVW_body[W] ); _set_V_equiv_kts( velocity*std::sqrt(sigma) * SG_FPS_TO_KT );
_set_V_calibrated_kts( std::sqrt( 2*qbar*sigma/rho) * SG_FPS_TO_KT );
set_V_ground_speed_kt( simd4::magnitude(NED_body) * SG_FPS_TO_KT );
_set_Mach_number( mach );
_set_Velocities_Local( NED_body[NORTH], NED_body[EAST], NED_body[DOWN] );
// _set_Velocities_Local_Airmass( UVW_aero[U], UVW_aero[V], UVW_aero[W] );
_set_Velocities_Body( UVW_body[U], UVW_body[V], UVW_body[W] );
_set_Omega_Body( PQR_body[P], PQR_body[Q], PQR_body[R] ); _set_Omega_Body( PQR_body[P], PQR_body[Q], PQR_body[R] );
_set_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
// Positions // Accelerations
double sl_radius, lat_geoc; // set_Accels_Omega( PQR_dot[P], PQR_dot[Q], PQR_dot[R] );
sgGeodToGeoc( NED_cm[LATITUDE], NED_cm[ALTITUDE], &sl_radius, &lat_geoc ); _set_Accels_Body( UVW_dot[U], UVW_dot[V], UVW_dot[W] );
_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; return true;
} }
#endif #endif
void
FGAISim::update_UVW_body(float f)
{
velocity = 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 #define INCHES_TO_FEET 0.08333333333f
FGAISim::update_qbar() #ifndef _MINMAX
{ # define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
unsigned int hi = _MINMAX(std::rint(-NED_cm[ALTITUDE]/1000.0f), 0, 100); #endif
float mach = velocity/env[hi][VSOUND];
float rho = env[hi][RHO]; // 1976 Standard Atmosphere - Density (slugs/ft2): 0 - 101,000 ft
float FGAISim::density[101] = {
0.0023771699, 0.0023083901, 0.0022411400, 0.0021753900, 0.0021111399,
0.0020483399, 0.0019869800, 0.0019270401, 0.0018685000, 0.0018113200,
0.0017554900, 0.0017009900, 0.0016477900, 0.0015958800, 0.0015452200,
0.0014958100, 0.0014476100, 0.0014006100, 0.0013547899, 0.0013101200,
0.0012665900, 0.0012241700, 0.0011828500, 0.0011426000, 0.0011034100,
0.0010652600, 0.0010281201, 0.0009919840, 0.0009568270, 0.0009226310,
0.0008893780, 0.0008570500, 0.0008256280, 0.0007950960, 0.0007654340,
0.0007366270, 0.0007086570, 0.0006759540, 0.0006442340, 0.0006140020,
0.0005851890, 0.0005577280, 0.0005315560, 0.0005066120, 0.0004828380,
0.0004601800, 0.0004385860, 0.0004180040, 0.0003983890, 0.0003796940,
0.0003618760, 0.0003448940, 0.0003287090, 0.0003132840, 0.0002985830,
0.0002845710, 0.0002712170, 0.0002584900, 0.0002463600, 0.0002347990,
0.0002237810, 0.0002132790, 0.0002032710, 0.0001937320, 0.0001846410,
0.0001759760, 0.0001676290, 0.0001595480, 0.0001518670, 0.0001445660,
0.0001376250, 0.0001310260, 0.0001247530, 0.0001187880, 0.0001131160,
0.0001077220, 0.0001025920, 0.0000977131, 0.0000930725, 0.0000886582,
0.0000844590, 0.0000804641, 0.0000766632, 0.0000730467, 0.0000696054,
0.0000663307, 0.0000632142, 0.0000602481, 0.0000574249, 0.0000547376,
0.0000521794, 0.0000497441, 0.0000474254, 0.0000452178, 0.0000431158,
0.0000411140, 0.0000392078, 0.0000373923, 0.0000356632, 0.0000340162,
0.0000324473
};
// 1976 Standard Atmosphere - Speed of sound (ft/s): 0 - 101,000 ft
float FGAISim::vsound[101] = {
1116.450, 1112.610, 1108.750, 1104.880, 1100.990, 1097.090, 1093.180,
1089.250, 1085.310, 1081.360, 1077.390, 1073.400, 1069.400, 1065.390,
1061.360, 1057.310, 1053.250, 1049.180, 1045.080, 1040.970, 1036.850,
1032.710, 1028.550, 1024.380, 1020.190, 1015.980, 1011.750, 1007.510,
1003.240, 998.963, 994.664, 990.347, 986.010, 981.655, 977.280,
972.885, 968.471, 968.076, 968.076, 968.076, 968.076, 968.076,
968.076, 968.076, 968.076, 968.076, 968.076, 968.076, 968.076,
968.076, 968.076, 968.076, 968.076, 968.076, 968.076, 968.076,
968.076, 968.076, 968.076, 968.076, 968.076, 968.076, 968.076,
968.076, 968.076, 968.076, 968.337, 969.017, 969.698, 970.377,
971.056, 971.735, 972.413, 973.091, 973.768, 974.445, 975.121,
975.797, 976.472, 977.147, 977.822, 978.496, 979.169, 979.842,
980.515, 981.187, 981.858, 982.530, 983.200, 983.871, 984.541,
985.210, 985.879, 986.547, 987.215, 987.883, 988.550, 989.217,
989.883, 990.549, 991.214
};
void
FGAISim::update_velocity(float v)
{
velocity = v;
/* altitude related */
float alt_idx = _MINMAX(location_geod[ALTITUDE]/1000.0f, 0, 100);
int idx = std::floor(alt_idx);
float fract = alt_idx - idx;
float ifract = 1.0f - fract;
/* linear interpolation for density */
rho = ifract*density[idx] + fract*density[idx+1];
qbar = 0.5f*rho*v*v;
sigma = rho/density[0];
float qbar = 0.5f*rho*velocity*velocity;
float Sqbar = S*qbar; float Sqbar = S*qbar;
float Sbqbar = Sqbar*b; float Sbqbar = Sqbar*b;
float Sqbarcbar = Sqbar*cbar; float Sqbarcbar = Sqbar*cbar;
C2F = Sqbar; /* Drag, Side, Lift */
C2F[THRUST] = rho*RPS2D4*CTmax; C2F = simd4_t<float,3>(-Sqbar, Sqbar, -Sqbar);
C2M = Sbqbar; /* Roll, Pitch, Yaw */
C2M[PITCH] = Sqbarcbar; C2M = simd4_t<float,3>(Sbqbar, Sqbarcbar, Sbqbar);
xCDYLT[VELOCITY][THRUST] = CTu*mach; /* linear interpolation for speed of sound */
float vs = ifract*vsound[idx] + fract*vsound[idx+1];
mach = v/vs;
/* useful semi-constants */
if (v < 0.0000001f) v = 0.0000001f;
b_2U = 0.5f*b/v;
cbar_2U = 0.5f*cbar/v;
} }
// 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 bool
FGAISim::load(std::string path) FGAISim::load(std::string path)
{ {
/* defaults for the Cessna 182, taken from Roskam */ /* defaults for the Cessna 172p */
S = 172.0f; S = 174.0f;
cbar = 4.90f; cbar = 4.90f;
b = 36.0f; b = 35.8f;
m = 2267.0f/AISIM_G; // max: 2650.0f;
cg[X] = 0.0f;
cg[Y] = 0.0f;
cg[Z] = -8.1f*INCHES_TO_FEET;
m = 2650.0f*G;
I[XX] = 948.0f; I[XX] = 948.0f;
I[YY] = 1346.0f; I[YY] = 1346.0f;
I[ZZ] = 1967.0f; I[ZZ] = 1967.0f;
// gear ground contact points relative tot cg at (0,0,0) // gear ground contact points relative tot aero ref. pt. at (0,0,0)
float no_gears = 3.0f; no_gears = 3;
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 */ /* nose */
gear[0][X] = -47.8f*INCHES_TO_FEET; gear_pos[0][X] = -47.8f*INCHES_TO_FEET;
gear[0][Y] = 0.0f*INCHES_TO_FEET; gear_pos[0][Y] = 0.0f*INCHES_TO_FEET;
gear[0][Z] = -54.4f*INCHES_TO_FEET; gear_pos[0][Z] = -54.4f*INCHES_TO_FEET;
Cg_spring[0] = 1800.0f;
Cg_damp[0] = 600.0f;
/* left */ /* left */
gear[1][X] = 17.2f*INCHES_TO_FEET; gear_pos[1][X] = 17.2f*INCHES_TO_FEET;
gear[1][Y] = -43.0f*INCHES_TO_FEET; gear_pos[1][Y] = -43.0f*INCHES_TO_FEET;
gear[1][Z] = -54.4f*INCHES_TO_FEET; gear_pos[1][Z] = -54.4f*INCHES_TO_FEET;
Cg_spring[1] = 5400.0f;
Cg_damp[1] = 1600.0f;
/* right */ /* right */
gear[2][X] = 17.2f*INCHES_TO_FEET; gear_pos[2][X] = 17.2f*INCHES_TO_FEET;
gear[2][Y] = 43.0f*INCHES_TO_FEET; gear_pos[2][Y] = 43.0f*INCHES_TO_FEET;
gear[2][Z] = -54.4f*INCHES_TO_FEET; gear_pos[2][Z] = -54.4f*INCHES_TO_FEET;
#endif Cg_spring[2] = 5400.0f;
Cg_damp[2] = 1600.0f;
float de_max = 24.0f*SG_DEGREES_TO_RADIANS; float de_max = 24.0f*SGD_DEGREES_TO_RADIANS;
float dr_max = 16.0f*SG_DEGREES_TO_RADIANS; float dr_max = 16.0f*SGD_DEGREES_TO_RADIANS;
float da_max = 17.5f*SG_DEGREES_TO_RADIANS; float da_max = 17.5f*SGD_DEGREES_TO_RADIANS;
float df_max = 30.0f*SG_DEGREES_TO_RADIANS; float df_max = 30.0f*SGD_DEGREES_TO_RADIANS;
/* thuster / propulsion */ /* thuster / propulsion */
no_engines = 1.0f; no_engines = 1;
CTmax = 0.073f*no_engines; CTmax = 0.057f/144;
CTu = -0.0960f*no_engines; CTu = -0.096f;
D = 75.0f*INCHES_TO_FEET; // (FWD,RIGHT,DOWN)
RPS = 2700.0f/60.0f; simd4_t<float,3> dir(1.0f,0.0f,0.0f);
prop_Ixx = 1.67f; // if propeller driven
float D = 75.0f*INCHES_TO_FEET;
float RPS = 2700.0f/60.0f;
float Ixx = 1.67f; // propeller
simd4::normalize(dir);
FT[0] = dir * (CTmax * RPS*RPS * D*D*D*D); // Thrust force
FTM[0] = dir * (CTu/(RPS*D))*vsound[0]; // Thrust force due to Mach
MT[0] = dir * (-Ixx*(2.0f*RPS*float(SGD_PI)));// Thrus moment
// if propeller driven
/* aerodynamic coefficients */ /* aerodynamic coefficients */
CLmin = 0.307f; CLmin = 0.307f;
CLa = 4.410f; CLa = 5.13f;
CLadot = 1.70f; CLadot = 1.70f;
CLq = 3.90f; CLq = 3.90f;
CLdf_n = 0.6685f*df_max; CLdf_n = 0.705f*df_max;
CDmin = 0.0270f; CDmin = 0.027f;
CDa = 0.121f; CDa = 0.158f;
CDb = 0.0f; CDb = 0.192f;
CDi = 0.0f; CDi = 0.0446f;
CDdf_n = 0.0816f*df_max; CDdf_n = 0.052f*df_max;
CYb = -0.393f; CYb = -0.31f;
CYp = -0.0750f; CYp = 0.006f;
CYr = 0.214f; CYr = 0.262f;
CYdr_n = 0.187f*dr_max; CYdr_n = 0.091f*dr_max;
Clb = -0.0923f; Clb = -0.057f;
Clp = -0.484f; Clp = -0.613f;
Clr = 0.0798f; Clr = 0.079f;
Clda_n = 0.2290f*da_max; Clda_n = 0.170f*da_max;
Cldr_n = 0.0147f*dr_max; Cldr_n = 0.01f*dr_max;
Cma = -0.613f; Cma = -1.0f;
Cmadot = -7.27f; Cmadot = -4.42f;
Cmq = -12.40f; Cmq = -10.5f;
Cmde_n = -1.122f*de_max; Cmde_n = -1.05f*de_max;
Cmdf_n = -0.2177f*df_max; Cmdf_n = -0.059f*df_max;
Cnb = 0.0587f; Cnb = 0.0630f;
Cnp = -0.0278f; Cnp = -0.0028f;
Cnr = -0.0937; Cnr = -0.0681f;
Cnda_n = -0.0216f*da_max; Cnda_n = -0.0100f*da_max;
Cndr_n = -0.0645f*dr_max; Cndr_n = -0.0398f*dr_max;
return true; return true;
} }

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

@ -24,36 +24,48 @@
#ifndef _FGAISim_HXX #ifndef _FGAISim_HXX
#define _FGAISim_HXX #define _FGAISim_HXX
#include <string> #ifdef HAVE_CONFIG_H
# include <config.h>
#include <simgear/math/SGVec3.hxx>
#include <simgear/math/simd.hxx>
//nclude <FDM/flight.hxx>
#define SG_DEGREES_TO_RADIANS 0.0174532925f
#define G 32.174f
#define PI 3.1415926535f
#define INCHES_TO_FEET 0.0833333333f
#ifndef _MINMAX
# define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
#endif #endif
class FGAISim : public FGInterface #include <string>
#include <cmath>
#ifdef ENABLE_SP_FDM
# include <simgear/math/simd.hxx>
# include <simgear/constants.h>
# include <FDM/flight.hxx>
#else
# include "simd.hxx"
# define SG_METER_TO_FEET 3.2808399
# define SG_FEET_TO_METER (1/SG_METER_TO_FEET)
# define SGD_DEGREES_TO_RADIANS 0.0174532925
# define SGD_RADIANS_TO_DEGREES (1/SGD_DEGREES_TO_RADIANS)
# define SGD_PI 3.1415926535
#endif
// #define SG_DEGREES_TO_RADIANS 0.0174532925f
// max. no. gears, maxi. no. engines
#define AISIM_MAX 4
#define AISIM_G 32.174f
class FGAISim
#ifdef ENABLE_SP_FDM
: public FGInterface
#endif
{ {
private: private:
enum { X=0, Y=1, Z=2 }; enum { X=0, Y=1, Z=2 };
enum { XX=0, YY=1, ZZ=2, XZ=3 }; enum { XX=0, YY=1, ZZ=2, XZ=3 };
enum { LATITUDE=0, LONGITUDE=1, ALTITUDE=2 }; enum { LATITUDE=0, LONGITUDE=1, ALTITUDE=2 };
enum { NORTH=0, EAST=2, DOWN=3 }; enum { NORTH=0, EAST=1, DOWN=2 };
enum { LEFT=0, RIGHT=1 }; enum { LEFT=0, RIGHT=1, UP=2 };
enum { MAX=0, VELOCITY=1 }; enum { MAX=0, VELOCITY=1, PROPULSION=2 };
enum { PROPULSION=0, FLAPS=2, RUDDER=2, MIN=3, AILERON=3, ELEVATOR=3 }; enum { FLAPS=2, RUDDER=2, MIN=3, AILERON=3, ELEVATOR=3 };
enum { DRAG=0, SIDE=1, LIFT=2, THRUST=3 }; enum { DRAG=0, SIDE=1, LIFT=2 };
enum { ROLL=0, PITCH=1, YAW=2 }; enum { ROLL=0, PITCH=1, YAW=2 };
enum { ALPHA=0, BETA=1 }; enum { ALPHA=0, BETA=1 };
enum { RHO=0, VSOUND=1 };
enum { ALT=3, ASL=3 };
enum { PHI, THETA, PSI }; enum { PHI, THETA, PSI };
enum { P=0, Q=1, R=2 }; enum { P=0, Q=1, R=2 };
enum { U=0, V=1, W=2 }; enum { U=0, V=1, W=2 };
@ -70,137 +82,142 @@ public:
bool load(std::string path); bool load(std::string path);
#ifdef ENABLE_SP_FDM
// copy FDM state to AISim structures // copy FDM state to AISim structures
bool copy_to_AISim(); bool copy_to_AISim();
// copy AISim structures to FDM state // copy AISim structures to FDM state
bool copy_from_AISim(); bool copy_from_AISim();
#endif
/* controls */ /* controls */
inline void set_rudder_norm(float f) { inline void set_rudder_norm(float f) {
xCDYLT[RUDDER][SIDE] = CYdr_n*f; xCDYL[RUDDER][SIDE] = CYdr_n*f;
xClmnT[RUDDER][ROLL] = Cldr_n*f; xClmn[RUDDER][ROLL] = Cldr_n*f;
xClmnT[RUDDER][YAW] = Cndr_n*f; xClmn[RUDDER][YAW] = Cndr_n*f;
} }
inline void set_elevator_norm(float f) { inline void set_elevator_norm(float f) {
xClmnT[ELEVATOR][PITCH] = Cmde_n*f; xClmn[ELEVATOR][PITCH] = Cmde_n*f;
} }
inline void set_aileron_norm(float f) { inline void set_aileron_norm(float f) {
xClmnT[AILERON][ROLL] = Clda_n*f; xClmn[AILERON][ROLL] = Clda_n*f;
xClmnT[AILERON][YAW] = Cnda_n*f; xClmn[AILERON][YAW] = Cnda_n*f;
} }
inline void set_flaps_norm(float f) { inline void set_flaps_norm(float f) {
xCDYLT[FLAPS][LIFT] = CLdf_n*f; xCDYL[FLAPS][LIFT] = CLdf_n*f;
xCDYLT[FLAPS][DRAG] = CDdf_n*f; xCDYL[FLAPS][DRAG] = CDdf_n*f;
xClmnT[FLAPS][PITCH] = Cmdf_n*f; xClmn[FLAPS][PITCH] = Cmdf_n*f;
}
inline void set_throttle_norm(float f) {
xCDYLT[MAX][THRUST] = f;
xClmnT[PROPULSION][THRUST] = f;
} }
inline void set_throttle_norm(float f) { th = f; }
inline void set_brake_norm(float f) { br = f; } inline void set_brake_norm(float f) { br = f; }
/* (initial) state, local frame */ /* (initial) state, local frame */
inline void set_location_geoc(const SGVec3f& p) { NED_cm = p.simd3(); } inline void set_location_geod(const simd4_t<double,3>& p) {
inline void set_location_geoc(float lat_geoc, float lon, float alt) { location_geod = p;
NED_cm[LATITUDE] = lat_geoc;
NED_cm[LONGITUDE] = lon;
NED_cm[ALTITUDE] = alt;
} }
inline void set_altitude_asl_ft(float f) { NED_cm[DOWN] = -f; }; inline void set_location_geod(double lat, double lon, double alt) {
inline void set_altitude_agl_ft(float f) { agl = _MINMAX(f, 0.0f, 100000.0f); } location_geod = simd4_t<double,3>(lat, lon, alt);
}
inline void set_altitude_asl_ft(float f) { location_geod[ALTITUDE] = f; };
inline void set_altitude_agl_ft(float f) { agl = f; }
inline void set_pitch(float f) { euler[PHI] = f; } inline void set_euler_angles_rad(const simd4_t<float,3>& e) {
inline void set_roll(float f) { euler[THETA] = f; } euler_body = e;
inline void set_yaw(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) { inline void set_euler_angles_rad(float phi, float theta, float psi) {
euler[PHI] = phi; euler_body = simd4_t<float,3>(phi, theta, psi);
euler[THETA] = theta; }
euler[PSI] = psi; inline void set_pitch_rad(float f) { euler_body[PHI] = f; }
inline void set_roll_rad(float f) { euler_body[THETA] = f; }
inline void set_heading_rad(float f) { euler_body[PSI] = f; }
void set_velocity_fps(const simd4_t<float,3>& v) { UVW_body = v; }
void set_velocity_fps(float u, float v, float w) {
UVW_body = simd4_t<float,3>(u, v, w);
}
void set_velocity_fps(float u) {
UVW_body = simd4_t<float,3>(u, 0.0f, 0.0f);
} }
void set_velocity_fps(float f) { inline void set_wind_ned_fps(const simd4_t<float,3>& w) { wind_ned = w; }
UVW_body = 0.0f; UVW_body[X] = f; update_UVW_body(f); inline void set_wind_ned_fps(float n, float e, float d) {
} wind_ned = simd4_t<float,3>(n, e, d);
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) { inline void set_alpha_rad(float f) {
xCDYLT[ALPHA][DRAG] = CDa*std::abs(f); xCDYL[ALPHA][DRAG] = CDa*std::abs(f);
xCDYLT[ALPHA][LIFT] = CLa*f; xCDYL[ALPHA][LIFT] = CLa*f;
xClmnT[ALPHA][PITCH] = Cma*f; xClmn[ALPHA][PITCH] = Cma*f;
ABY_body[ALPHA] = f; AOA_body[ALPHA] = f;
} }
inline void set_beta_rad(float f) { inline void set_beta_rad(float f) {
xCDYLT[BETA][DRAG] = CDb*std::abs(f); xCDYL[BETA][DRAG] = CDb*std::abs(f);
xCDYLT[BETA][SIDE] = CYb*f; xCDYL[BETA][SIDE] = CYb*f;
xClmnT[BETA][ROLL] = Clb*f; xClmn[BETA][ROLL] = Clb*f;
xClmnT[BETA][YAW] = Cnb*f; xClmn[BETA][YAW] = Cnb*f;
ABY_body[BETA] = f; AOA_body[BETA] = f;
} }
inline float get_alpha_rad() { inline float get_alpha_rad() {
return ABY_body[ALPHA]; return AOA_body[ALPHA];
} }
inline float get_beta_rad() { inline float get_beta_rad() {
return ABY_body[BETA]; return AOA_body[BETA];
} }
private: private:
void update_qbar(); void update_velocity(float v);
void update_UVW_body(float f);
/* aircraft normalized controls */ /* aircraft normalized controls */
float br; /* brake */ float th; /* throttle command */
float br; /* brake command */
/* thuster / propulsion */
float D, RPS, prop_Ixx;
/* aircraft state */ /* aircraft state */
simd4_t<float,3> NED_cm; /* lat, lon, altitude */ simd4_t<double,3> location_geod; /* lat, lon, altitude */
simd4_t<float,3> UVW_body; /* fwd, up, side speed */ simd4_t<float,5> NED_distance; /* North, East, Down rel. pos. */
simd4_t<float,3> NED_body; /* North, East, Down speeds */
simd4_t<float,3> UVW_body; /* fwd, up, side speed */
simd4_t<float,3> UVW_dot; /* fwd, up, side accelerations */
simd4_t<float,3> PQR_body; /* P, Q, R rate */ simd4_t<float,3> PQR_body; /* P, Q, R rate */
simd4_t<float,3> PQR_dot; /* Pdot, Qdot, Rdot accel. */ simd4_t<float,3> PQR_dot; /* P, Q, R accelerations */
simd4_t<float,3> ABY_body; /* alpha, beta, gamma */ simd4_t<float,2> AOA_body; /* alpha, beta */
simd4_t<float,3> ABY_dot; /* adot, bdot, ydot */ simd4_t<float,2> AOA_dot; /* adot, bdot */
simd4_t<float,3> euler; /* phi, theta, psi */ simd4_t<float,3> euler_body; /* phi, theta, psi */
simd4_t<float,3> wind_ned; simd4_t<float,3> euler_dot; /* change in phi, theta, psi */
float agl, velocity; simd4_t<float,3> wind_ned; /* wind north, east, down */
/* 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 */ /* This should reduce the time spent in update() since controls */
/* change less often than the update function runs which might */ /* change less often than the update function runs which might */
/* run 20 to 60 times (or more) per second */ /* run 20 to 60 times (or more) per second */
/* cache, values that don't change very often */ /* cache */
simd4_t<float,3> FThrust, MThrust; simd4_t<float,3> UVW_aero; /* airmass relative to the body */
simd4_t<float,3> FT[AISIM_MAX]; /* thrust force */
simd4_t<float,3> FTM[AISIM_MAX]; /* thrust due to mach force */
simd4_t<float,3> MT[AISIM_MAX]; /* thrust moment */
simd4_t<float,3> IQR_dot_fact;
float agl, velocity, mach;
float b_2U, cbar_2U; float b_2U, cbar_2U;
bool WoW;
/* dynamic coefficients (already multiplied with their value) */ /* dynamic coefficients (already multiplied with their value) */
simd4_t<float,4> xCqadot[2], xCpr[2]; simd4_t<float,3> xCqadot[2], xCpr[2];
simd4_t<float,4> xCDYLT[4]; simd4_t<float,3> xCDYL[4];
simd4_t<float,4> xClmnT[4]; simd4_t<float,3> xClmn[4];
simd4_t<float,4> C2F, C2M; simd4_t<float,3> C2F, C2M;
/* ---------------------------------------------------------------- */ /* ---------------------------------------------------------------- */
/* aircraft static data */ /* aircraft static data */
int no_engines; int no_engines, no_gears;
simd4_t<float,3> gear; /* NED_cms in structural frame */ simd4_t<float,3> gear_pos[AISIM_MAX]; /* pos in structural frame */
simd4_t<float,3> cg; /* center of gravity */
simd4_t<float,3> I; /* inertia */ simd4_t<float,3> I; /* inertia */
float S, cbar, b; /* wing area, mean average chord, span */ float S, cbar, b; /* wing area, mean average chord, span */
float m; /* mass */ float m; /* mass */
float RPS2D4; /* propeller diameter, ening RPS */
/* static oefficients, *_n is for normalized surface deflection */ /* static coefficients, *_n is for normalized surface deflection */
float Cgear; /* gear */ float Cg_spring[AISIM_MAX]; /* gear spring coeffients */
float Cg_damp[AISIM_MAX]; /* gear damping coefficients */
float CTmax, CTu; /* thrust max, due to speed */ float CTmax, CTu; /* thrust max, due to speed */
float CLmin, CLa, CLadot, CLq, CLdf_n; float CLmin, CLa, CLadot, CLq, CLdf_n;
float CDmin, CDa, CDb, CDi, CDdf_n; float CDmin, CDa, CDb, CDi, CDdf_n;
@ -209,10 +226,10 @@ private:
float Cma, Cmadot, Cmq, Cmde_n, Cmdf_n; float Cma, Cmadot, Cmq, Cmde_n, Cmdf_n;
float Cnb, Cnp, Cnr, Cnda_n, Cndr_n; float Cnb, Cnp, Cnr, Cnda_n, Cndr_n;
/* static environment data */ /* environment data */
static float env[101][2]; static float density[101], vsound[101];
simd4_t<float,3> gravity; simd4_t<float,3> gravity_ned;
float dt2_2m; float rho, qbar, sigma;
}; };
#endif // _FGAISim_HXX #endif // _FGAISim_HXX