A new batch of updates, slowly moving forward
This commit is contained in:
Erik Hofman
parent
d762db2db8
commit
9a71f6348f
2 changed files with 528 additions and 459 deletions
|
|
@ -20,55 +20,82 @@
|
|||
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||||
//
|
||||
|
||||
#include "AISim.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <stdio.h>
|
||||
|
||||
#ifndef ENABLE_SP_FDM
|
||||
# define ENABLE_SP_FDM 1
|
||||
#endif
|
||||
#ifdef ENABLE_SP_FDM
|
||||
# include <simgear/constants.h>
|
||||
# include <simgear/math/simd.hxx>
|
||||
# include <simgear/math/simd4x4.hxx>
|
||||
# include <simgear/math/sg_geodesy.hxx>
|
||||
|
||||
#if ENABLE_SP_FDM
|
||||
#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 <Aircraft/controls.hxx>
|
||||
# include <Main/fg_props.hxx>
|
||||
# include <Main/globals.hxx>
|
||||
# include <FDM/flight.hxx>
|
||||
#else
|
||||
#include "simd.hxx"
|
||||
#include "simd4x4.hxx"
|
||||
# include "simd.hxx"
|
||||
# include "simd4x4.hxx"
|
||||
#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) {
|
||||
xCDYLT[i] = 0.0f;
|
||||
xClmnT[i] = 0.0f;
|
||||
xCDYL[i] = 0.0f;
|
||||
xClmn[i] = 0.0f;
|
||||
}
|
||||
|
||||
PQR_body_prev = 0.0f;
|
||||
ABY_body_prev = 0.0f;
|
||||
for (int i=0; i<AISIM_MAX; ++i) {
|
||||
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);
|
||||
load(aero->getStringValue());
|
||||
#else
|
||||
|
|
@ -97,15 +124,22 @@ FGAISim::FGAISim(double dt)
|
|||
xCqadot[LIFT][1] = CLadot;
|
||||
xCqadot[PITCH][0] = Cmq;
|
||||
xCqadot[PITCH][1] = Cmadot;
|
||||
xCDYLT[MIN][LIFT] = CLmin;
|
||||
xCDYLT[MIN][DRAG] = CDmin;
|
||||
xCDYL[MIN][LIFT] = CLmin;
|
||||
xCDYL[MIN][DRAG] = CDmin;
|
||||
|
||||
C2M[THRUST] = prop_Ixx*(2.0f*RPS*PI)/no_engines;
|
||||
RPS2D4 = RPS*RPS * D*D*D*D;
|
||||
// calculate the initial c.g. position above ground level to position
|
||||
// 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;
|
||||
gravity = 0.0f; gravity[DOWN] = G;
|
||||
dt2_2m = 0.5f*dt*dt/m;
|
||||
IQR_dot_fact = simd4_t<float,3>(I[ZZ]-I[YY], I[XX]-I[ZZ], I[YY]-I[XX]);
|
||||
}
|
||||
|
||||
FGAISim::~FGAISim()
|
||||
|
|
@ -116,166 +150,216 @@ FGAISim::~FGAISim()
|
|||
// each subsequent iteration through the EOM
|
||||
void
|
||||
FGAISim::init() {
|
||||
#if ENABLE_SP_FDM
|
||||
#ifdef ENABLE_SP_FDM
|
||||
// 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" );
|
||||
|
||||
double sl_radius, lat_geoc;
|
||||
sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc );
|
||||
set_location_geoc( lat_geoc, get_Longitude(), get_Altitude() );
|
||||
// right now agl holds the lowest contact point of the aircraft
|
||||
set_Altitude( get_Altitude()-agl );
|
||||
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() );
|
||||
|
||||
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( UVW_body[U] );
|
||||
set_velocity_fps( fgGetFloat("sim/presets/uBody-fps"),
|
||||
fgGetFloat("sim/presets/vBody-fps"),
|
||||
fgGetFloat("sim/presets/wBody-fps"));
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
FGAISim::update(double ddt)
|
||||
{
|
||||
#if ENABLE_SP_FDM
|
||||
if (is_suspended())
|
||||
#ifdef ENABLE_SP_FDM
|
||||
if (is_suspended() || ddt == 0)
|
||||
return;
|
||||
#endif
|
||||
|
||||
// initialize all of AISim vars
|
||||
float dt = float(ddt);
|
||||
|
||||
#ifdef ENABLE_SP_FDM
|
||||
copy_to_AISim();
|
||||
ABY_dot = (ABY_body - ABY_body_prev)*dt;
|
||||
PQR_dot = (PQR_body - PQR_body_prev)*dt;
|
||||
#endif
|
||||
|
||||
/* 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> 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
|
||||
// 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);
|
||||
/* Air-Relative velocity vector */
|
||||
UVW_aero = UVW_body + EB_mtx*wind_ned;
|
||||
update_velocity( simd4::magnitude(UVW_aero) );
|
||||
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);
|
||||
|
||||
// Body-Axis Gravity Components
|
||||
simd4_t<float,3> windb = EBM*wind_ned;
|
||||
simd4_t<float,2> AOA_body_prev = AOA_body;
|
||||
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
|
||||
simd4_t<float,3> Va = UVW_body + windb;
|
||||
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
|
||||
/* Force and Moment Coefficients */
|
||||
/* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */
|
||||
float p = PQR_body[P];
|
||||
float q = PQR_body[Q];
|
||||
float r = PQR_body[R];
|
||||
float adot = ABY_dot[ALPHA];
|
||||
float adot = AOA_dot[ALPHA];
|
||||
|
||||
/**
|
||||
* CDYLT[LIFT] = (CLq*q + CLadot*adot)*cbar_2U;
|
||||
* ClmnT[PITCH] = (Cmq*q + Cmadot*adot)*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;
|
||||
/*
|
||||
* CDYL[LIFT] = (CLq*q + CLadot*adot)*cbar_2U;
|
||||
* Clmn[PITCH] = (Cmq*q + Cmadot*adot)*cbar_2U;
|
||||
*
|
||||
* CDYL[SIDE] = (CYp*p + CYr*r)*b_2U;
|
||||
* Clmn[ROLL] = (Clp*p + Clr*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,4> Cb2U = (xCpr[0]*p + xCpr[1]*r)*b_2U;
|
||||
simd4_t<float,3> Ccbar2U = (xCqadot[0]*q + xCqadot[1]*adot)*cbar_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,4> CDYLT(0.0f, Cb2U[SIDE], Ccbar2U[LIFT], 0.0f);
|
||||
simd4_t<float,4> ClmnT(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW], 0.0f);
|
||||
simd4_t<float,3> CDYL(0.0f, Cb2U[SIDE], Ccbar2U[LIFT]);
|
||||
simd4_t<float,3> Clmn(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW]);
|
||||
for (int i=0; i<4; ++i) {
|
||||
CDYLT += xCDYLT[i];
|
||||
ClmnT += xClmnT[i];
|
||||
CDYL += xCDYL[i];
|
||||
Clmn += xClmn[i];
|
||||
}
|
||||
float CL = CDYLT[LIFT];
|
||||
CDYLT += simd4_t<float,4>(CDi*CL*CL, 0.0f, 0.0f, 0.0f);
|
||||
float CL = CDYL[LIFT];
|
||||
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) */
|
||||
simd4_t<float,4> FDYLT = CDYLT*C2F;
|
||||
simd4_t<float,4> MlmnT = ClmnT*C2M;
|
||||
|
||||
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]);
|
||||
simd4_t<float,3> FDYL = CDYL*C2F;
|
||||
simd4_t<float,3> Mlmn = Clmn*C2M;
|
||||
printf("FDYL: %7.2f, %7.2f, %7.2f\n", FDYL[DRAG], FDYL[SIDE], FDYL[LIFT]);
|
||||
|
||||
/* convert from wind axes to body axes */
|
||||
simd4_t<float,3> aby = ABY_body; // alpha, beta, gamma
|
||||
angle = simd4::normalize(aby);
|
||||
vector = AOA_body;
|
||||
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);
|
||||
simd4_t<float,3>FXYZ = EBM*gravity + WBM*FDYLT;
|
||||
|
||||
// Thrust -- todo: propulsion in non x-directions
|
||||
FThrust[X] = FDYLT[THRUST];
|
||||
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;
|
||||
/* Thrust */
|
||||
for (int i=0; i<no_engines; ++i) {
|
||||
FXYZ += FT[i]*th + FTM[i]*mach;
|
||||
// Mlmn += MT*th;
|
||||
printf("FT: %10.2f, %7.2f, MT: %7.2f\n", FT[i][X]*th, FTM[i][X]*mach, MT[i][X]*th);
|
||||
}
|
||||
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
|
||||
/* body-axis velocity: forward, sideward, upward */
|
||||
simd4_t<float,3> dUVW = FXYZ + simd4::cross(UVW_body, PQR_body);
|
||||
UVW_body += dUVW;
|
||||
/* gear forces */
|
||||
#if 1
|
||||
WoW = false;
|
||||
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]);
|
||||
|
||||
/* position of center of mass wrt earth: north, east, down */
|
||||
simd4_t<float,3> dNED_cm = simd4x4::transpose(EBM)*UVW_body;
|
||||
NED_cm += dNED_cm*dt;
|
||||
printf("NED: %7.2f, %7.2f, %7.2f\n",
|
||||
NED_cm[LATITUDE], NED_cm[LONGITUDE], NED_cm[ALTITUDE]);
|
||||
NED_body = BE_mtx*UVW_body;
|
||||
NED_distance = NED_body*dt;
|
||||
printf("vNED: %7.2f, %7.2f, %7.2f\n", NED_body[0], NED_body[1], NED_body[2]);
|
||||
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 */
|
||||
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;
|
||||
printf("PQR: %7.2f, %7.2f, %7.2f\n", PQR_body[P], PQR_body[Q], PQR_body[R]);
|
||||
|
||||
/* 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]);
|
||||
double dist = simd4::magnitude(simd4_t<float,2>(NED_distance));
|
||||
#ifdef ENABLE_SP_FDM
|
||||
double lat2 = 0.0, lon2 = 0.0, az2 = 0.0;
|
||||
geo_direct_wgs_84( 0.0, location_geod[LATITUDE] * SGD_RADIANS_TO_DEGREES,
|
||||
location_geod[LONGITUDE] * SGD_RADIANS_TO_DEGREES,
|
||||
euler_body[PSI] * SGD_RADIANS_TO_DEGREES,
|
||||
dist * SG_FEET_TO_METER, &lat2, &lon2, &az2 );
|
||||
set_location_geod( lat2 * SGD_DEGREES_TO_RADIANS,
|
||||
lon2 * SGD_DEGREES_TO_RADIANS,
|
||||
location_geod[ALTITUDE] - NED_distance[DOWN] );
|
||||
// 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);
|
||||
|
||||
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;
|
||||
printf("euler: %7.2f, %7.2f, %7.2f\n", euler[PHI], euler[THETA], euler[PSI]);
|
||||
|
||||
euler_dot[PSI] = (q*sin_p - r*cos_p)/cos_t;
|
||||
euler_dot[THETA] = q*cos_p - r*sin_p;
|
||||
// euler_dot[PHI] = p + (q*sin_p + r*cos_p)*sin_t/cos_t;
|
||||
euler_dot[PHI] = p + euler_dot[PSI]*sin_t;
|
||||
euler_body += euler_dot;
|
||||
printf("euler: %7.2f, %7.2f, %7.2f\n", euler_body[PHI], euler_body[THETA], euler_body[PSI]);
|
||||
|
||||
#ifdef ENABLE_SP_FDM
|
||||
copy_from_AISim();
|
||||
#endif
|
||||
}
|
||||
|
||||
#if ENABLE_SP_FDM
|
||||
#ifdef ENABLE_SP_FDM
|
||||
bool
|
||||
FGAISim::copy_to_AISim()
|
||||
{
|
||||
|
|
@ -287,9 +371,9 @@ FGAISim::copy_to_AISim()
|
|||
set_brake_norm(0.5f*(globals->get_controls()->get_brake_left()
|
||||
+globals->get_controls()->get_brake_right()));
|
||||
|
||||
set_altitude_asl_ft(get_Altitude());
|
||||
set_altitude_agl_ft(get_Altitude_AGL());
|
||||
|
||||
// set_location_geod(get_Latitude(), get_Longitude(), get_Altitude());
|
||||
// set_location_geod(get_Latitude(), get_Longitude(), location_geod[ALTITUDE]);
|
||||
// set_velocity_fps(get_V_calibrated_kts());
|
||||
|
||||
return true;
|
||||
|
|
@ -298,257 +382,225 @@ FGAISim::copy_to_AISim()
|
|||
bool
|
||||
FGAISim::copy_from_AISim()
|
||||
{
|
||||
// Accelerations
|
||||
// _set_Accels_Omega( PQR_body[P], PQR_body[Q], PQR_body[R] );
|
||||
|
||||
// Positions
|
||||
_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
|
||||
_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_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
|
||||
|
||||
// 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] );
|
||||
// Accelerations
|
||||
// set_Accels_Omega( PQR_dot[P], PQR_dot[Q], PQR_dot[R] );
|
||||
_set_Accels_Body( UVW_dot[U], UVW_dot[V], UVW_dot[W] );
|
||||
|
||||
return true;
|
||||
}
|
||||
#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
|
||||
FGAISim::update_qbar()
|
||||
{
|
||||
unsigned int hi = _MINMAX(std::rint(-NED_cm[ALTITUDE]/1000.0f), 0, 100);
|
||||
float mach = velocity/env[hi][VSOUND];
|
||||
float rho = env[hi][RHO];
|
||||
#define INCHES_TO_FEET 0.08333333333f
|
||||
#ifndef _MINMAX
|
||||
# define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
|
||||
#endif
|
||||
|
||||
// 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 Sbqbar = Sqbar*b;
|
||||
float Sqbarcbar = Sqbar*cbar;
|
||||
|
||||
C2F = Sqbar;
|
||||
C2F[THRUST] = rho*RPS2D4*CTmax;
|
||||
/* Drag, Side, Lift */
|
||||
C2F = simd4_t<float,3>(-Sqbar, Sqbar, -Sqbar);
|
||||
|
||||
C2M = Sbqbar;
|
||||
C2M[PITCH] = Sqbarcbar;
|
||||
/* Roll, Pitch, Yaw */
|
||||
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
|
||||
FGAISim::load(std::string path)
|
||||
{
|
||||
/* defaults for the Cessna 182, taken from Roskam */
|
||||
S = 172.0f;
|
||||
/* defaults for the Cessna 172p */
|
||||
S = 174.0f;
|
||||
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[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
|
||||
// gear ground contact points relative tot aero ref. pt. at (0,0,0)
|
||||
no_gears = 3;
|
||||
/* 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;
|
||||
gear_pos[0][X] = -47.8f*INCHES_TO_FEET;
|
||||
gear_pos[0][Y] = 0.0f*INCHES_TO_FEET;
|
||||
gear_pos[0][Z] = -54.4f*INCHES_TO_FEET;
|
||||
Cg_spring[0] = 1800.0f;
|
||||
Cg_damp[0] = 600.0f;
|
||||
/* 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;
|
||||
gear_pos[1][X] = 17.2f*INCHES_TO_FEET;
|
||||
gear_pos[1][Y] = -43.0f*INCHES_TO_FEET;
|
||||
gear_pos[1][Z] = -54.4f*INCHES_TO_FEET;
|
||||
Cg_spring[1] = 5400.0f;
|
||||
Cg_damp[1] = 1600.0f;
|
||||
/* 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
|
||||
gear_pos[2][X] = 17.2f*INCHES_TO_FEET;
|
||||
gear_pos[2][Y] = 43.0f*INCHES_TO_FEET;
|
||||
gear_pos[2][Z] = -54.4f*INCHES_TO_FEET;
|
||||
Cg_spring[2] = 5400.0f;
|
||||
Cg_damp[2] = 1600.0f;
|
||||
|
||||
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;
|
||||
float de_max = 24.0f*SGD_DEGREES_TO_RADIANS;
|
||||
float dr_max = 16.0f*SGD_DEGREES_TO_RADIANS;
|
||||
float da_max = 17.5f*SGD_DEGREES_TO_RADIANS;
|
||||
float df_max = 30.0f*SGD_DEGREES_TO_RADIANS;
|
||||
|
||||
/* thuster / propulsion */
|
||||
no_engines = 1.0f;
|
||||
CTmax = 0.073f*no_engines;
|
||||
CTu = -0.0960f*no_engines;
|
||||
no_engines = 1;
|
||||
CTmax = 0.057f/144;
|
||||
CTu = -0.096f;
|
||||
|
||||
D = 75.0f*INCHES_TO_FEET;
|
||||
RPS = 2700.0f/60.0f;
|
||||
prop_Ixx = 1.67f;
|
||||
// (FWD,RIGHT,DOWN)
|
||||
simd4_t<float,3> dir(1.0f,0.0f,0.0f);
|
||||
// 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 */
|
||||
CLmin = 0.307f;
|
||||
CLa = 4.410f;
|
||||
CLadot = 1.70f;
|
||||
CLq = 3.90f;
|
||||
CLdf_n = 0.6685f*df_max;
|
||||
CLmin = 0.307f;
|
||||
CLa = 5.13f;
|
||||
CLadot = 1.70f;
|
||||
CLq = 3.90f;
|
||||
CLdf_n = 0.705f*df_max;
|
||||
|
||||
CDmin = 0.0270f;
|
||||
CDa = 0.121f;
|
||||
CDb = 0.0f;
|
||||
CDi = 0.0f;
|
||||
CDdf_n = 0.0816f*df_max;
|
||||
CDmin = 0.027f;
|
||||
CDa = 0.158f;
|
||||
CDb = 0.192f;
|
||||
CDi = 0.0446f;
|
||||
CDdf_n = 0.052f*df_max;
|
||||
|
||||
CYb = -0.393f;
|
||||
CYp = -0.0750f;
|
||||
CYr = 0.214f;
|
||||
CYdr_n = 0.187f*dr_max;
|
||||
CYb = -0.31f;
|
||||
CYp = 0.006f;
|
||||
CYr = 0.262f;
|
||||
CYdr_n = 0.091f*dr_max;
|
||||
|
||||
Clb = -0.0923f;
|
||||
Clp = -0.484f;
|
||||
Clr = 0.0798f;
|
||||
Clda_n = 0.2290f*da_max;
|
||||
Cldr_n = 0.0147f*dr_max;
|
||||
Clb = -0.057f;
|
||||
Clp = -0.613f;
|
||||
Clr = 0.079f;
|
||||
Clda_n = 0.170f*da_max;
|
||||
Cldr_n = 0.01f*dr_max;
|
||||
|
||||
Cma = -0.613f;
|
||||
Cmadot = -7.27f;
|
||||
Cmq = -12.40f;
|
||||
Cmde_n = -1.122f*de_max;
|
||||
Cmdf_n = -0.2177f*df_max;
|
||||
Cma = -1.0f;
|
||||
Cmadot = -4.42f;
|
||||
Cmq = -10.5f;
|
||||
Cmde_n = -1.05f*de_max;
|
||||
Cmdf_n = -0.059f*df_max;
|
||||
|
||||
Cnb = 0.0587f;
|
||||
Cnp = -0.0278f;
|
||||
Cnr = -0.0937;
|
||||
Cnda_n = -0.0216f*da_max;
|
||||
Cndr_n = -0.0645f*dr_max;
|
||||
Cnb = 0.0630f;
|
||||
Cnp = -0.0028f;
|
||||
Cnr = -0.0681f;
|
||||
Cnda_n = -0.0100f*da_max;
|
||||
Cndr_n = -0.0398f*dr_max;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,36 +24,48 @@
|
|||
#ifndef _FGAISim_HXX
|
||||
#define _FGAISim_HXX
|
||||
|
||||
#include <string>
|
||||
|
||||
#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)))
|
||||
#ifdef HAVE_CONFIG_H
|
||||
# include <config.h>
|
||||
#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:
|
||||
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 };
|
||||
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 { NORTH=0, EAST=1, DOWN=2 };
|
||||
enum { LEFT=0, RIGHT=1, UP=2 };
|
||||
enum { MAX=0, VELOCITY=1, PROPULSION=2 };
|
||||
enum { FLAPS=2, RUDDER=2, MIN=3, AILERON=3, ELEVATOR=3 };
|
||||
enum { DRAG=0, SIDE=1, LIFT=2 };
|
||||
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 };
|
||||
|
|
@ -70,137 +82,142 @@ public:
|
|||
|
||||
bool load(std::string path);
|
||||
|
||||
#ifdef ENABLE_SP_FDM
|
||||
// copy FDM state to AISim structures
|
||||
bool copy_to_AISim();
|
||||
|
||||
// copy AISim structures to FDM state
|
||||
bool copy_from_AISim();
|
||||
#endif
|
||||
|
||||
/* 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;
|
||||
xCDYL[RUDDER][SIDE] = CYdr_n*f;
|
||||
xClmn[RUDDER][ROLL] = Cldr_n*f;
|
||||
xClmn[RUDDER][YAW] = Cndr_n*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) {
|
||||
xClmnT[AILERON][ROLL] = Clda_n*f;
|
||||
xClmnT[AILERON][YAW] = Cnda_n*f;
|
||||
xClmn[AILERON][ROLL] = Clda_n*f;
|
||||
xClmn[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;
|
||||
xCDYL[FLAPS][LIFT] = CLdf_n*f;
|
||||
xCDYL[FLAPS][DRAG] = CDdf_n*f;
|
||||
xClmn[FLAPS][PITCH] = Cmdf_n*f;
|
||||
}
|
||||
inline void set_throttle_norm(float f) { th = f; }
|
||||
inline void set_brake_norm(float f) { br = f; }
|
||||
|
||||
/* (initial) state, local frame */
|
||||
inline void set_location_geoc(const SGVec3f& p) { NED_cm = p.simd3(); }
|
||||
inline void set_location_geoc(float lat_geoc, float lon, float alt) {
|
||||
NED_cm[LATITUDE] = lat_geoc;
|
||||
NED_cm[LONGITUDE] = lon;
|
||||
NED_cm[ALTITUDE] = alt;
|
||||
inline void set_location_geod(const simd4_t<double,3>& p) {
|
||||
location_geod = p;
|
||||
}
|
||||
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_location_geod(double lat, double lon, double alt) {
|
||||
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_roll(float f) { euler[THETA] = f; }
|
||||
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(const simd4_t<float,3>& e) {
|
||||
euler_body = e;
|
||||
}
|
||||
inline void set_euler_angles_rad(float phi, float theta, float psi) {
|
||||
euler[PHI] = phi;
|
||||
euler[THETA] = theta;
|
||||
euler[PSI] = psi;
|
||||
euler_body = simd4_t<float,3>(phi, theta, 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) {
|
||||
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_wind_ned_fps(const simd4_t<float,3>& w) { wind_ned = w; }
|
||||
inline void set_wind_ned_fps(float n, float e, float d) {
|
||||
wind_ned = simd4_t<float,3>(n, e, 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;
|
||||
xCDYL[ALPHA][DRAG] = CDa*std::abs(f);
|
||||
xCDYL[ALPHA][LIFT] = CLa*f;
|
||||
xClmn[ALPHA][PITCH] = Cma*f;
|
||||
AOA_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;
|
||||
xCDYL[BETA][DRAG] = CDb*std::abs(f);
|
||||
xCDYL[BETA][SIDE] = CYb*f;
|
||||
xClmn[BETA][ROLL] = Clb*f;
|
||||
xClmn[BETA][YAW] = Cnb*f;
|
||||
AOA_body[BETA] = f;
|
||||
}
|
||||
inline float get_alpha_rad() {
|
||||
return ABY_body[ALPHA];
|
||||
return AOA_body[ALPHA];
|
||||
}
|
||||
inline float get_beta_rad() {
|
||||
return ABY_body[BETA];
|
||||
return AOA_body[BETA];
|
||||
}
|
||||
|
||||
private:
|
||||
void update_qbar();
|
||||
void update_UVW_body(float f);
|
||||
void update_velocity(float v);
|
||||
|
||||
/* aircraft normalized controls */
|
||||
float br; /* brake */
|
||||
|
||||
/* thuster / propulsion */
|
||||
float D, RPS, prop_Ixx;
|
||||
float th; /* throttle command */
|
||||
float br; /* brake command */
|
||||
|
||||
/* aircraft state */
|
||||
simd4_t<float,3> NED_cm; /* lat, lon, altitude */
|
||||
simd4_t<float,3> UVW_body; /* fwd, up, side speed */
|
||||
simd4_t<double,3> location_geod; /* lat, lon, altitude */
|
||||
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_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;
|
||||
|
||||
simd4_t<float,3> PQR_dot; /* P, Q, R accelerations */
|
||||
simd4_t<float,2> AOA_body; /* alpha, beta */
|
||||
simd4_t<float,2> AOA_dot; /* adot, bdot */
|
||||
simd4_t<float,3> euler_body; /* phi, theta, psi */
|
||||
simd4_t<float,3> euler_dot; /* change in phi, theta, psi */
|
||||
simd4_t<float,3> wind_ned; /* wind north, east, down */
|
||||
|
||||
/* ---------------------------------------------------------------- */
|
||||
/* 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;
|
||||
/* cache */
|
||||
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;
|
||||
bool WoW;
|
||||
|
||||
/* dynamic coefficients (already multiplied with their value) */
|
||||
simd4_t<float,4> xCqadot[2], xCpr[2];
|
||||
simd4_t<float,4> xCDYLT[4];
|
||||
simd4_t<float,4> xClmnT[4];
|
||||
simd4_t<float,4> C2F, C2M;
|
||||
simd4_t<float,3> xCqadot[2], xCpr[2];
|
||||
simd4_t<float,3> xCDYL[4];
|
||||
simd4_t<float,3> xClmn[4];
|
||||
simd4_t<float,3> C2F, C2M;
|
||||
|
||||
/* ---------------------------------------------------------------- */
|
||||
/* aircraft static data */
|
||||
int no_engines;
|
||||
simd4_t<float,3> gear; /* NED_cms in structural frame */
|
||||
int no_engines, no_gears;
|
||||
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 */
|
||||
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 */
|
||||
/* static coefficients, *_n is for normalized surface deflection */
|
||||
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 CLmin, CLa, CLadot, CLq, CLdf_n;
|
||||
float CDmin, CDa, CDb, CDi, CDdf_n;
|
||||
|
|
@ -209,10 +226,10 @@ private:
|
|||
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;
|
||||
/* environment data */
|
||||
static float density[101], vsound[101];
|
||||
simd4_t<float,3> gravity_ned;
|
||||
float rho, qbar, sigma;
|
||||
};
|
||||
|
||||
#endif // _FGAISim_HXX
|
||||
|
|
|
|||
Loading…
Reference in a new issue