Add the latest code for reference in the JSBSim mailing list while bug hunting
This commit is contained in:
parent
1ef1d5ad43
commit
9f9f1c631f
2 changed files with 258 additions and 179 deletions
|
@ -2,7 +2,7 @@
|
||||||
//
|
//
|
||||||
// Written by Erik Hofman, started November 2016
|
// Written by Erik Hofman, started November 2016
|
||||||
//
|
//
|
||||||
// Copyright (C) 2016 Erik Hofman <erik@ehofman.com>
|
// Copyright (C) 2016,2017 Erik Hofman <erik@ehofman.com>
|
||||||
//
|
//
|
||||||
//
|
//
|
||||||
// This program is free software; you can redistribute it and/or
|
// This program is free software; you can redistribute it and/or
|
||||||
|
@ -41,28 +41,30 @@
|
||||||
# include "simd4x4.hxx"
|
# include "simd4x4.hxx"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef _MINMAX
|
||||||
|
# define _MINMAX(a,b,c) (((a)>(c)) ? (c) : (((a)<(b)) ? (b) : (a)))
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
FGAISim::FGAISim(double dt) :
|
FGAISim::FGAISim(double dt) :
|
||||||
br(0),
|
br(0),
|
||||||
location_geod(0.0),
|
location_geod(0.0),
|
||||||
NED_distance(0.0f),
|
NEDdot(0.0f),
|
||||||
NED_body(0.0f),
|
vUVW(0.0f),
|
||||||
UVW_body(0.0f),
|
vUVWdot(0.0f),
|
||||||
UVW_dot(0.0f),
|
vPQR(0.0f),
|
||||||
PQR_body(0.0f),
|
vPQRdot(0.0f),
|
||||||
PQR_dot(0.0f),
|
AOA(0.0f),
|
||||||
AOA_body(0.0f),
|
AOAdot(0.0f),
|
||||||
AOA_dot(0.0f),
|
euler(0.0f),
|
||||||
euler_body(0.0f),
|
|
||||||
euler_dot(0.0f),
|
euler_dot(0.0f),
|
||||||
wind_ned(0.0f),
|
wind_ned(0.0f),
|
||||||
UVW_aero(0.0f),
|
vUVWaero(0.0f),
|
||||||
IQR_dot_fact(0.0f),
|
|
||||||
agl(0.0f),
|
|
||||||
velocity(0.0f),
|
|
||||||
mach(0.0f),
|
|
||||||
b_2U(0.0f),
|
b_2U(0.0f),
|
||||||
cbar_2U(0.0f),
|
cbar_2U(0.0f),
|
||||||
|
velocity(0.0f),
|
||||||
|
mach(0.0f),
|
||||||
|
agl(0.0f),
|
||||||
WoW(true),
|
WoW(true),
|
||||||
no_engines(0),
|
no_engines(0),
|
||||||
no_gears(0),
|
no_gears(0),
|
||||||
|
@ -77,15 +79,11 @@ FGAISim::FGAISim(double dt) :
|
||||||
qbar(0.0f),
|
qbar(0.0f),
|
||||||
sigma(0.0f)
|
sigma(0.0f)
|
||||||
{
|
{
|
||||||
|
simd4x4::zeros(xCDYLT);
|
||||||
|
simd4x4::zeros(xClmnT);
|
||||||
|
|
||||||
for (int i=0; i<2; ++i) {
|
xCq = xCadot = 0.0f;
|
||||||
xCqadot[i] = 0.0f;
|
xCp = xCr = 0.0f;
|
||||||
xCpr[i] = 0.0f;
|
|
||||||
}
|
|
||||||
for (int i=0; i<4; ++i) {
|
|
||||||
xCDYL[i] = 0.0f;
|
|
||||||
xClmn[i] = 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i=0; i<AISIM_MAX; ++i) {
|
for (int i=0; i<AISIM_MAX; ++i) {
|
||||||
FT[i] = FTM[i] = MT[i] = 0.0f;
|
FT[i] = FTM[i] = MT[i] = 0.0f;
|
||||||
|
@ -114,18 +112,23 @@ FGAISim::FGAISim(double dt) :
|
||||||
set_beta_rad(0.0f);
|
set_beta_rad(0.0f);
|
||||||
|
|
||||||
/* useful constants */
|
/* useful constants */
|
||||||
xCpr[SIDE][0] = CYp;
|
xCp[SIDE] = CYp;
|
||||||
xCpr[SIDE][1] = CYr;
|
xCr[SIDE] = CYr;
|
||||||
xCpr[ROLL][0] = Clp;
|
xCp[ROLL] = Clp;
|
||||||
xCpr[ROLL][1] = Clr;
|
xCr[ROLL] = Clr;
|
||||||
xCpr[YAW][0] = Cnp;
|
xCp[YAW] = Cnp;
|
||||||
xCpr[YAW][1] = Cnr;
|
xCr[YAW] = Cnr;
|
||||||
xCqadot[LIFT][0] = CLq;
|
|
||||||
xCqadot[LIFT][1] = CLadot;
|
xCq[LIFT] = CLq;
|
||||||
xCqadot[PITCH][0] = Cmq;
|
xCadot[LIFT] = CLadot;
|
||||||
xCqadot[PITCH][1] = Cmadot;
|
|
||||||
xCDYL[MIN][LIFT] = CLmin;
|
xCq[PITCH] = Cmq;
|
||||||
xCDYL[MIN][DRAG] = CDmin;
|
xCadot[PITCH] = Cmadot;
|
||||||
|
|
||||||
|
xCDYLT.ptr()[MIN][LIFT] = CLmin;
|
||||||
|
xCDYLT.ptr()[MIN][DRAG] = CDmin;
|
||||||
|
|
||||||
|
inv_m = 1.0f/m;
|
||||||
|
|
||||||
// calculate the initial c.g. position above ground level to position
|
// calculate the initial c.g. position above ground level to position
|
||||||
// the aircraft on it's wheels.
|
// the aircraft on it's wheels.
|
||||||
|
@ -139,7 +142,11 @@ FGAISim::FGAISim(double dt) :
|
||||||
Cg_damp[no_gears] = 2000.0f;
|
Cg_damp[no_gears] = 2000.0f;
|
||||||
no_gears++;
|
no_gears++;
|
||||||
|
|
||||||
IQR_dot_fact = simd4_t<float,3>(I[ZZ]-I[YY], I[XX]-I[ZZ], I[YY]-I[XX]);
|
mI = simd4x4_t<float,4>( I[XX], 0.0f, -I[XZ], 0.0f,
|
||||||
|
0.0f, I[YY], 0.0f, 0.0f,
|
||||||
|
-I[XZ], 0.0f, I[ZZ], 0.0f,
|
||||||
|
0.0f, 0.0f, 0.0f, 0.0f);
|
||||||
|
mIinv = invert_inertia(mI);
|
||||||
}
|
}
|
||||||
|
|
||||||
FGAISim::~FGAISim()
|
FGAISim::~FGAISim()
|
||||||
|
@ -181,35 +188,41 @@ FGAISim::update(double ddt)
|
||||||
|
|
||||||
// initialize all of AISim vars
|
// initialize all of AISim vars
|
||||||
float dt = float(ddt);
|
float dt = float(ddt);
|
||||||
|
float inv_dt = 1.0f/dt;
|
||||||
|
|
||||||
#ifdef ENABLE_SP_FDM
|
#ifdef ENABLE_SP_FDM
|
||||||
copy_to_AISim();
|
copy_to_AISim();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* Earth-to-Body-Axis Transformation Matrix */
|
/* Earth-to-Body-Axis Transformation Matrix */
|
||||||
/* body = pitch, roll, yaw */
|
/* matrices to compensate for pitch, roll and heading */
|
||||||
simd4_t<float,3> vector = euler_body;
|
simd4_t<float,3> vector = euler;
|
||||||
float angle = simd4::normalize(vector);
|
float angle = simd4::normalize(vector);
|
||||||
simd4x4_t<float,4> EB_mtx = simd4x4::rotation_matrix<float>(angle, vector);
|
simd4x4_t<float,4> mNed2Body = simd4x4::rotation_matrix(angle, vector);
|
||||||
simd4x4_t<float,4> BE_mtx = simd4x4::transpose(EB_mtx);
|
simd4x4_t<float,4> mBody2Ned = simd4x4::transpose(mNed2Body);
|
||||||
|
simd4_t<float,3> gravity_body = mNed2Body*gravity_ned;
|
||||||
|
|
||||||
/* Air-Relative velocity vector */
|
/* Air-Relative velocity vector */
|
||||||
UVW_aero = UVW_body + EB_mtx*wind_ned;
|
vUVWaero = vUVW + mNed2Body*wind_ned;
|
||||||
update_velocity( simd4::magnitude(UVW_aero) );
|
update_velocity( simd4::magnitude( simd4_t<float,2>(vUVWaero) ) );
|
||||||
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);
|
|
||||||
|
|
||||||
simd4_t<float,2> AOA_body_prev = AOA_body;
|
simd4_t<float,3> WindAxis = AOA;
|
||||||
set_alpha_rad(UVW_aero[W]==0 ? 0.0f : std::atan2(UVW_aero[W], UVW_aero[U]));
|
float alpha = (vUVWaero[W] == 0) ? 0 : std::atan2(vUVWaero[W], vUVWaero[U]);
|
||||||
set_beta_rad(velocity==0 ? 0.0f : std::asin(UVW_aero[V]/velocity) );
|
set_alpha_rad( _MINMAX(alpha, -0.0873f, 0.349f) ); // -5 to 20 degrees
|
||||||
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]);
|
float beta = (velocity == 0) ? 0 : std::asin(vUVWaero[V]/velocity);
|
||||||
|
set_beta_rad( _MINMAX(beta, -0.2618f, 0.2618f) ); // -15 to 15 degrees
|
||||||
|
AOAdot = (AOA - WindAxis)*inv_dt;
|
||||||
|
|
||||||
|
printf("velocity: %f, vUVWaero: %3.2f, %3.2f, %3.2f, mach: %3.2f\n", velocity, vUVWaero[0], vUVWaero[1], vUVWaero[2], mach);
|
||||||
|
printf("agl: %5.4f, alpha: %5.4f, beta: %5.4f, adot: %5.4f, bdot: %5.4f\n", agl, AOA[ALPHA], AOA[BETA], AOAdot[ALPHA], AOAdot[BETA]);
|
||||||
|
|
||||||
/* Force and Moment Coefficients */
|
/* Force and Moment Coefficients */
|
||||||
/* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */
|
/* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */
|
||||||
float p = PQR_body[P];
|
float p = vPQR[P];
|
||||||
float q = PQR_body[Q];
|
float q = vPQR[Q];
|
||||||
float r = PQR_body[R];
|
float r = vPQR[R];
|
||||||
float adot = AOA_dot[ALPHA];
|
float adot = AOAdot[ALPHA];
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* CDYL[LIFT] = (CLq*q + CLadot*adot)*cbar_2U;
|
* CDYL[LIFT] = (CLq*q + CLadot*adot)*cbar_2U;
|
||||||
|
@ -219,50 +232,63 @@ printf("agl: %5.4f, alpha: %5.4f, beta: %5.4f, adot: %5.4f, bdot: %5.4f\n", agl,
|
||||||
* Clmn[ROLL] = (Clp*p + Clr*r)*b_2U;
|
* Clmn[ROLL] = (Clp*p + Clr*r)*b_2U;
|
||||||
* Clmn[YAW] = (Cnp*p + Cnr*r)*b_2U;
|
* Clmn[YAW] = (Cnp*p + Cnr*r)*b_2U;
|
||||||
*/
|
*/
|
||||||
simd4_t<float,3> Ccbar2U = (xCqadot[0]*q + xCqadot[1]*adot)*cbar_2U;
|
simd4_t<float,3> Ccbar2U = (xCq*q + xCadot*adot)*cbar_2U;
|
||||||
simd4_t<float,3> Cb2U = (xCpr[0]*p + xCpr[1]*r )*b_2U;
|
simd4_t<float,3> Cb2U = (xCp*p + xCr*r )*b_2U;
|
||||||
|
|
||||||
simd4_t<float,3> CDYL(0.0f, Cb2U[SIDE], Ccbar2U[LIFT]);
|
simd4_t<float,3> CDYL(0.0f, Cb2U[SIDE], Ccbar2U[LIFT]);
|
||||||
simd4_t<float,3> Clmn(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW]);
|
simd4_t<float,3> Clmn(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW]);
|
||||||
for (int i=0; i<4; ++i) {
|
for (int i=0; i<4; ++i) {
|
||||||
CDYL += xCDYL[i];
|
CDYL.v4() += xCDYLT.m4x4()[i];
|
||||||
Clmn += xClmn[i];
|
Clmn.v4() += xClmnT.m4x4()[i];
|
||||||
}
|
}
|
||||||
float CL = CDYL[LIFT];
|
float CL = CDYL[LIFT];
|
||||||
CDYL += simd4_t<float,3>(CDi*CL*CL, 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]);
|
printf("CDYL: %7.2f, %7.2f, %7.2f\n", CDYL[DRAG], CDYL[SIDE], CDYL[LIFT]);
|
||||||
|
#if 0
|
||||||
|
printf(" CLa: %6.3f, CLadot: %6.3f, CLq: %6.3f\n", xCDYLT.ptr()[ALPHA][LIFT],CLadot*adot,CLq*q);
|
||||||
|
printf(" CDa: %6.3f, CDb: %6.3f, CDi: %6.3f\n", xCDYLT.ptr()[ALPHA][DRAG],xCDYLT.ptr()[BETA][DRAG],CDi*CL*CL);
|
||||||
|
printf(" CYb: %6.3f, CYp: %6.3f, CYr: %6.3f\n", xCDYLT.ptr()[BETA][SIDE],CYp*p,CYr*r);
|
||||||
|
printf(" Cma: %6.3f, Cmadot: %6.3f, Cmq: %6.3f\n", xClmnT.ptr()[ALPHA][PITCH],Cmadot*adot,Cmq*q);
|
||||||
|
printf(" Clb: %6.3f, Clp: %6.3f, Clr: %6.3f\n", xClmnT.ptr()[BETA][ROLL],Clp*p,Clr*r);
|
||||||
|
printf(" Cnb: %6.3f, Cnp: %6.3f, Cnr: %6.3f\n", xClmnT.ptr()[BETA][YAW],Cnp*p,Cnr*r);
|
||||||
|
|
||||||
|
printf(" Cmde: %6.3f\n", xClmnT.ptr()[ELEVATOR][PITCH]);
|
||||||
|
printf(" CYdr: %6.3f, Cldr: %6.3f, Cndr: %6.3f\n", xCDYLT.ptr()[RUDDER][SIDE], xClmnT.ptr()[RUDDER][ROLL], xClmnT.ptr()[RUDDER][YAW]);
|
||||||
|
printf(" Clda: %6.3f, CYda: %6.3f\n", xClmnT.ptr()[AILERON][ROLL], xClmnT.ptr()[AILERON][YAW]);
|
||||||
|
printf(" Cldf: %6.3f, CDdf: %6.3f, Cmdf: %6.3f\n", xCDYLT.ptr()[FLAPS][LIFT], xCDYLT.ptr()[FLAPS][DRAG], xClmnT.ptr()[FLAPS][PITCH]);
|
||||||
|
#endif
|
||||||
|
|
||||||
/* State Accelerations (convert coefficients to forces and moments) */
|
/* State Accelerations (convert coefficients to forces and moments) */
|
||||||
simd4_t<float,3> FDYL = CDYL*C2F;
|
simd4_t<float,3> FDYL = CDYL*Coef2Force;
|
||||||
simd4_t<float,3> Mlmn = Clmn*C2M;
|
simd4_t<float,3> Mlmn = Clmn*Coef2Moment;
|
||||||
printf("FDYL: %7.2f, %7.2f, %7.2f\n", FDYL[DRAG], FDYL[SIDE], FDYL[LIFT]);
|
|
||||||
|
|
||||||
/* convert from wind axes to body axes */
|
/* convert from wind axes to body axes */
|
||||||
vector = AOA_body;
|
vector = AOA;
|
||||||
angle = simd4::normalize(vector);
|
angle = simd4::normalize(vector);
|
||||||
simd4x4_t<float,4> WB_mtx = simd4x4::rotation_matrix<float>(angle, vector);
|
simd4x4_t<float,4> mWindBody = simd4x4::rotation_matrix(angle, vector);
|
||||||
simd4_t<float,3>FXYZ = WB_mtx*FDYL;
|
simd4_t<float,3> FXYZ = mWindBody*FDYL;
|
||||||
|
|
||||||
/* Thrust */
|
/* Thrust */
|
||||||
for (int i=0; i<no_engines; ++i) {
|
for (int i=0; i<no_engines; ++i) {
|
||||||
FXYZ += FT[i]*th + FTM[i]*mach;
|
FXYZ += FT[i]*th + FTM[i]*mach;
|
||||||
// Mlmn += MT*th;
|
// 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);
|
|
||||||
|
printf("FDYL: %7.2f, %7.2f, %7.2f\n", FDYL[DRAG], FDYL[SIDE], FDYL[LIFT]);
|
||||||
|
printf("FT: %10.2f, %7.2f, MT: %7.2f\n", FT[i][X]*th, FTM[i][X]*mach, MT[i][X]*th);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* gear forces */
|
/* gear forces */
|
||||||
#if 1
|
|
||||||
WoW = false;
|
WoW = false;
|
||||||
if (agl < 100.0f) {
|
if (agl < 100.0f) {
|
||||||
int WoW_main = 0;
|
int WoW_main = 0;
|
||||||
simd4_t<float,3> cg_agl_neu(0.0f, 0.0f, agl);
|
simd4_t<float,3> cg_agl_neu(0.0f, 0.0f, agl);
|
||||||
for (int i=0; i<no_gears; ++i) {
|
for (int i=0; i<no_gears; ++i) {
|
||||||
simd4_t<float,3> gear_ned = BE_mtx*gear_pos[i];
|
simd4_t<float,3> gear_ned = mBody2Ned*gear_pos[i];
|
||||||
simd4_t<float,3> lg_ground_neu = gear_ned + cg_agl_neu;
|
simd4_t<float,3> lg_ground_neu = gear_ned + cg_agl_neu;
|
||||||
if (lg_ground_neu[Z] < 0.0f) { // weight on wheel
|
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_vrot = simd4::cross(vPQR, gear_pos[i]);
|
||||||
simd4_t<float,3> lg_cg_vned = BE_mtx*lg_vrot;
|
simd4_t<float,3> lg_cg_vned = mBody2Ned*lg_vrot;
|
||||||
simd4_t<float,3> lg_vned = NED_body + lg_cg_vned;
|
simd4_t<float,3> lg_vned = NEDdot + lg_cg_vned;
|
||||||
float Fn;
|
float Fn;
|
||||||
|
|
||||||
Fn = Cg_spring[i]*lg_ground_neu[Z] - Cg_damp[i]*lg_vned[Z];
|
Fn = Cg_spring[i]*lg_ground_neu[Z] - Cg_damp[i]*lg_vned[Z];
|
||||||
|
@ -271,10 +297,12 @@ printf("FT: %10.2f, %7.2f, MT: %7.2f\n", FT[i][X]*th, FTM[i][X]*mach, MT[i][X]*t
|
||||||
simd4_t<float,3> Fn_lg(0.0f, 0.0f, Fn);
|
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> 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> FLGear = mNed2Body*Fn_lg + vUVWaero*mu_body;
|
||||||
simd4_t<float,3> MLGear = simd4::cross(gear_pos[i], FLGear);
|
simd4_t<float,3> MLGear = simd4::cross(gear_pos[i], FLGear);
|
||||||
|
#if 0
|
||||||
printf("FLGear[%i]: %10.2f %10.2f %10.2f\n",i,FLGear[0], FLGear[1], FLGear[2]);
|
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]);
|
printf("MLGear[%i]: %10.2f %10.2f %10.2f\n",i,MLGear[0], MLGear[1], MLGear[2]);
|
||||||
|
#endif
|
||||||
|
|
||||||
FXYZ += FLGear;
|
FXYZ += FLGear;
|
||||||
// Mlmn += MLGear;
|
// Mlmn += MLGear;
|
||||||
|
@ -283,76 +311,66 @@ printf("MLGear[%i]: %10.2f %10.2f %10.2f\n",i,MLGear[0], MLGear[1], MLGear[2]);
|
||||||
WoW = (WoW_main == 3);
|
WoW = (WoW_main == 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
printf("FXYZ/m:%7.2f, %7.2f, %7.2f\n", FXYZ[X]/m, FXYZ[Y]/m, FXYZ[Z]/m);
|
/* local body accelrations */
|
||||||
printf("Mlmn: %7.2f, %7.2f, %7.2f\n", Mlmn[ROLL], Mlmn[PITCH], Mlmn[YAW]);
|
aXYZ = FXYZ*inv_m + gravity_body;
|
||||||
|
printf("aXYZ: %7.2f, %7.2f, %7.2f\n", aXYZ[X]/m, aXYZ[Y]/m, aXYZ[Z]/m);
|
||||||
|
printf("Mlmn: %7.2f, %7.2f, %7.2f\n", Mlmn[ROLL], Mlmn[PITCH], Mlmn[YAW]);
|
||||||
|
|
||||||
/* Dynamic Equations */
|
/* Dynamic Equations */
|
||||||
|
|
||||||
/* body-axis rotational accelerations: rolling, pitching, yawing
|
/* body-axis translational accelerations: forward, sideward, upward */
|
||||||
* PQR_dot[P] = (Mlmn[ROLL] - q*r*(I[ZZ] - I[YY]))/I[XX];
|
vUVWdot = aXYZ - simd4::cross(vPQR, vUVW);
|
||||||
* PQR_dot[Q] = (Mlmn[PITCH] - p*r*(I[XX] - I[ZZ]))/I[YY];
|
vUVW += vUVWdot*dt;
|
||||||
* PQR_dot[R] = ( Mlmn[YAW] - p*q*(I[YY] - I[XX]))/I[ZZ];
|
|
||||||
* \-------------/
|
/* body-axis rotational accelerations: rolling, pitching, yawing */
|
||||||
* IQR_dot_fact
|
vPQRdot = mIinv*(Mlmn - vPQR*(mI*vPQR));
|
||||||
*/
|
vPQR += vPQRdot*dt;
|
||||||
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
|
printf("PQRdot: %7.2f, %7.2f, %7.2f\n", vPQRdot[P], vPQRdot[Q], vPQRdot[R]);
|
||||||
gravity(x,y,z) = EB_mtx*(0,0,AISIM_G)
|
printf("PQR: %7.2f, %7.2f, %7.2f\n", vPQR[P], vPQR[Q], vPQR[R]);
|
||||||
Udot = FX/m + gravity(x) + Vbody*Rbody - Wbody*Qbody;
|
printf("UVWdot: %7.2f, %7.2f, %7.2f\n", vUVWdot[U], vUVWdot[V], vUVWdot[W]);
|
||||||
Vdot = FY/m + gravity(y) + Wbody*Pbody - Ubody*Rbody;
|
printf("UVW: %7.2f, %7.2f, %7.2f\n", vUVW[U], vUVW[V], vUVW[W]);
|
||||||
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 */
|
/* position of center of mass wrt earth: north, east, down */
|
||||||
NED_body = BE_mtx*UVW_body;
|
NEDdot = mBody2Ned*vUVW;
|
||||||
NED_distance = NED_body*dt;
|
simd4_t<float,3> NEDdist = NEDdot*dt;
|
||||||
printf("vNED: %7.2f, %7.2f, %7.2f\n", NED_body[0], NED_body[1], NED_body[2]);
|
printf("NEDdot: %7.2f, %7.2f, %7.2f\n", NEDdot[0], NEDdot[1], NEDdot[2]);
|
||||||
printf("dNED: %7.2f, %7.2f, %7.2f\n", NED_distance[0], NED_distance[1], NED_distance[2]);
|
printf("NEDdist: %7.2f, %7.2f, %7.2f\n", NEDdist[0], NEDdist[1], NEDdist[2]);
|
||||||
|
|
||||||
double dist = simd4::magnitude(simd4_t<float,2>(NED_distance));
|
|
||||||
#ifdef ENABLE_SP_FDM
|
#ifdef ENABLE_SP_FDM
|
||||||
|
double dist = simd4::magnitude( simd4_t<float,2>(NEDdist) );
|
||||||
|
|
||||||
double lat2 = 0.0, lon2 = 0.0, az2 = 0.0;
|
double lat2 = 0.0, lon2 = 0.0, az2 = 0.0;
|
||||||
geo_direct_wgs_84( 0.0, location_geod[LATITUDE] * SGD_RADIANS_TO_DEGREES,
|
geo_direct_wgs_84( 0.0, location_geod[LATITUDE] * SGD_RADIANS_TO_DEGREES,
|
||||||
location_geod[LONGITUDE] * SGD_RADIANS_TO_DEGREES,
|
location_geod[LONGITUDE] * SGD_RADIANS_TO_DEGREES,
|
||||||
euler_body[PSI] * SGD_RADIANS_TO_DEGREES,
|
euler[PSI] * SGD_RADIANS_TO_DEGREES,
|
||||||
dist * SG_FEET_TO_METER, &lat2, &lon2, &az2 );
|
dist * SG_FEET_TO_METER, &lat2, &lon2, &az2 );
|
||||||
set_location_geod( lat2 * SGD_DEGREES_TO_RADIANS,
|
set_location_geod( lat2 * SGD_DEGREES_TO_RADIANS,
|
||||||
lon2 * SGD_DEGREES_TO_RADIANS,
|
lon2 * SGD_DEGREES_TO_RADIANS,
|
||||||
location_geod[ALTITUDE] - NED_distance[DOWN] );
|
location_geod[ALTITUDE] - NEDdist[DOWN] );
|
||||||
// set_heading_rad( az2 * SGD_DEGREES_TO_RADIANS );
|
// set_heading_rad( az2 * SGD_DEGREES_TO_RADIANS );
|
||||||
#else
|
#else
|
||||||
location_geod += NED_distance;
|
location_geod[X] += NEDdist[X];
|
||||||
|
location_geod[Y] += NEDdist[Y];
|
||||||
|
location_geod[Z] -= NEDdist[Z];
|
||||||
set_altitude_agl_ft(location_geod[DOWN]);
|
set_altitude_agl_ft(location_geod[DOWN]);
|
||||||
#endif
|
#endif
|
||||||
printf("GEOD: %7.2f, %7.2f, %7.2f\n", location_geod[0], location_geod[1], location_geod[2]);
|
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) */
|
/* angle of body wrt earth: phi (pitch), theta (roll), psi (heading) */
|
||||||
float sin_p = std::sin(euler_body[PHI]);
|
float sin_p = std::sin(euler[PHI]);
|
||||||
float cos_p = std::cos(euler_body[PHI]);
|
float cos_p = std::cos(euler[PHI]);
|
||||||
float sin_t = std::sin(euler_body[THETA]);
|
float sin_t = std::sin(euler[THETA]);
|
||||||
float cos_t = std::cos(euler_body[THETA]);
|
float cos_t = std::cos(euler[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);
|
||||||
|
|
||||||
euler_dot[PSI] = (q*sin_p - r*cos_p)/cos_t;
|
euler_dot[PSI] = (q*sin_p + r*cos_p)/cos_t;
|
||||||
euler_dot[THETA] = q*cos_p - r*sin_p;
|
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 + (q*sin_p + r*cos_p)*sin_t/cos_t;
|
||||||
euler_dot[PHI] = p + euler_dot[PSI]*sin_t;
|
euler_dot[PHI] = p + euler_dot[PSI]*sin_t;
|
||||||
euler_body += euler_dot;
|
euler += euler_dot;
|
||||||
printf("euler: %7.2f, %7.2f, %7.2f\n", euler_body[PHI], euler_body[THETA], euler_body[PSI]);
|
printf("euler: %7.2f, %7.2f, %7.2f\n", euler[PHI], euler[THETA], euler[PSI]);
|
||||||
|
|
||||||
#ifdef ENABLE_SP_FDM
|
#ifdef ENABLE_SP_FDM
|
||||||
copy_from_AISim();
|
copy_from_AISim();
|
||||||
|
@ -394,25 +412,28 @@ FGAISim::copy_from_AISim()
|
||||||
_set_Altitude( location_geod[ALTITUDE] );
|
_set_Altitude( location_geod[ALTITUDE] );
|
||||||
_set_Altitude_AGL( location_geod[ALTITUDE] - get_Runway_altitude() );
|
_set_Altitude_AGL( location_geod[ALTITUDE] - get_Runway_altitude() );
|
||||||
|
|
||||||
_set_Euler_Angles( euler_body[PHI], euler_body[THETA], euler_body[PSI] );
|
// _set_Euler_Angles( roll, pitch, heading );
|
||||||
_set_Alpha( AOA_body[ALPHA] );
|
float heading = euler[PSI];
|
||||||
_set_Beta( AOA_body[BETA] );
|
if (heading < 0) heading += SGD_2PI;
|
||||||
|
_set_Euler_Angles( euler[PHI], euler[THETA], heading );
|
||||||
|
_set_Alpha( AOA[ALPHA] );
|
||||||
|
_set_Beta( AOA[BETA] );
|
||||||
|
|
||||||
// Velocities
|
// Velocities
|
||||||
_set_V_equiv_kts( velocity*std::sqrt(sigma) * SG_FPS_TO_KT );
|
_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_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_V_ground_speed_kt( simd4::magnitude(NEDdot) * SG_FPS_TO_KT );
|
||||||
_set_Mach_number( mach );
|
_set_Mach_number( mach );
|
||||||
|
|
||||||
_set_Velocities_Local( NED_body[NORTH], NED_body[EAST], NED_body[DOWN] );
|
_set_Velocities_Local( NEDdot[NORTH], NEDdot[EAST], NEDdot[DOWN] );
|
||||||
// _set_Velocities_Local_Airmass( UVW_aero[U], UVW_aero[V], UVW_aero[W] );
|
// _set_Velocities_Local_Airmass( vUVWaero[U], vUVWaero[V], vUVWaero[W] );
|
||||||
_set_Velocities_Body( UVW_body[U], UVW_body[V], UVW_body[W] );
|
_set_Velocities_Body( vUVW[U], vUVW[V], vUVW[W] );
|
||||||
_set_Omega_Body( PQR_body[P], PQR_body[Q], PQR_body[R] );
|
_set_Omega_Body( vPQR[P], vPQR[Q], vPQR[R] );
|
||||||
_set_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
|
_set_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
|
||||||
|
|
||||||
// Accelerations
|
// Accelerations
|
||||||
// set_Accels_Omega( PQR_dot[P], PQR_dot[Q], PQR_dot[R] );
|
// set_Accels_Omega( vPQRdot[P], vPQRdot[Q], vPQRdot[R] );
|
||||||
_set_Accels_Body( UVW_dot[U], UVW_dot[V], UVW_dot[W] );
|
_set_Accels_Body( vUVWdot[U], vUVWdot[V], vUVWdot[W] );
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -489,11 +510,11 @@ FGAISim::update_velocity(float v)
|
||||||
float Sbqbar = Sqbar*b;
|
float Sbqbar = Sqbar*b;
|
||||||
float Sqbarcbar = Sqbar*cbar;
|
float Sqbarcbar = Sqbar*cbar;
|
||||||
|
|
||||||
/* Drag, Side, Lift */
|
/* Drag, Side, Lift */
|
||||||
C2F = simd4_t<float,3>(-Sqbar, Sqbar, -Sqbar);
|
Coef2Force = simd4_t<float,3>(-Sqbar, Sqbar, -Sqbar, Sqbar);
|
||||||
|
|
||||||
/* Roll, Pitch, Yaw */
|
/* Roll, Pitch, Yaw */
|
||||||
C2M = simd4_t<float,3>(Sbqbar, Sqbarcbar, Sbqbar);
|
Coef2Moment = simd4_t<float,3>(Sbqbar, Sqbarcbar, Sbqbar, Sbqbar);
|
||||||
|
|
||||||
/* linear interpolation for speed of sound */
|
/* linear interpolation for speed of sound */
|
||||||
float vs = ifract*vsound[idx] + fract*vsound[idx+1];
|
float vs = ifract*vsound[idx] + fract*vsound[idx+1];
|
||||||
|
@ -505,6 +526,34 @@ FGAISim::update_velocity(float v)
|
||||||
cbar_2U = 0.5f*cbar/v;
|
cbar_2U = 0.5f*cbar/v;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
simd4x4_t<float,4>
|
||||||
|
FGAISim::invert_inertia(simd4x4_t<float,4> mtx)
|
||||||
|
{
|
||||||
|
float Ixx, Iyy, Izz, Ixz;
|
||||||
|
float k1, k3, k4, k6;
|
||||||
|
float denom;
|
||||||
|
|
||||||
|
Ixx = mtx.ptr()[0][0];
|
||||||
|
Iyy = mtx.ptr()[1][1];
|
||||||
|
Izz = mtx.ptr()[2][2];
|
||||||
|
Ixz = -mtx.ptr()[0][2];
|
||||||
|
|
||||||
|
k1 = Iyy*Izz;
|
||||||
|
k3 = Iyy*Ixz;
|
||||||
|
denom = 1.0f/(Ixx*k1 - Ixz*k3);
|
||||||
|
|
||||||
|
k1 *= denom;
|
||||||
|
k3 *= denom;
|
||||||
|
k4 = (Izz*Ixx - Ixz*Ixz)*denom;
|
||||||
|
k6 = Ixx*Iyy*denom;
|
||||||
|
|
||||||
|
return simd4x4_t<float,4>( k1, 0.0f, k3, 0.0f,
|
||||||
|
0.0f, k4, 0.0f, 0.0f,
|
||||||
|
k3, 0.0f, k6, 0.0f,
|
||||||
|
0.0f, 0.0f, 0.0f, 0.0f );
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
bool
|
bool
|
||||||
FGAISim::load(std::string path)
|
FGAISim::load(std::string path)
|
||||||
{
|
{
|
||||||
|
@ -549,12 +598,13 @@ FGAISim::load(std::string path)
|
||||||
float df_max = 30.0f*SGD_DEGREES_TO_RADIANS;
|
float df_max = 30.0f*SGD_DEGREES_TO_RADIANS;
|
||||||
|
|
||||||
/* thuster / propulsion */
|
/* thuster / propulsion */
|
||||||
|
// (FWD,RIGHT,DOWN)
|
||||||
|
simd4_t<float,3> dir(1.0f,0.0f,0.0f);
|
||||||
no_engines = 1;
|
no_engines = 1;
|
||||||
|
|
||||||
CTmax = 0.057f/144;
|
CTmax = 0.057f/144;
|
||||||
CTu = -0.096f;
|
CTu = -0.096f;
|
||||||
|
|
||||||
// (FWD,RIGHT,DOWN)
|
|
||||||
simd4_t<float,3> dir(1.0f,0.0f,0.0f);
|
|
||||||
// if propeller driven
|
// if propeller driven
|
||||||
float D = 75.0f*INCHES_TO_FEET;
|
float D = 75.0f*INCHES_TO_FEET;
|
||||||
float RPS = 2700.0f/60.0f;
|
float RPS = 2700.0f/60.0f;
|
||||||
|
@ -584,23 +634,47 @@ FGAISim::load(std::string path)
|
||||||
CYr = 0.262f;
|
CYr = 0.262f;
|
||||||
CYdr_n = 0.091f*dr_max;
|
CYdr_n = 0.091f*dr_max;
|
||||||
|
|
||||||
|
#if 0
|
||||||
Clb = -0.057f;
|
Clb = -0.057f;
|
||||||
Clp = -0.613f;
|
Clp = -0.613f;
|
||||||
Clr = 0.079f;
|
Clr = 0.079f;
|
||||||
Clda_n = 0.170f*da_max;
|
Clda_n = 0.170f*da_max;
|
||||||
Cldr_n = 0.01f*dr_max;
|
Cldr_n = 0.01f*dr_max;
|
||||||
|
#else
|
||||||
|
Clb = 0.0;
|
||||||
|
Clp = 0.0;
|
||||||
|
Clr = 0.0f;
|
||||||
|
Clda_n = 0.0f*da_max;
|
||||||
|
Cldr_n = 0.0f*dr_max;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if 0
|
||||||
Cma = -1.0f;
|
Cma = -1.0f;
|
||||||
Cmadot = -4.42f;
|
Cmadot = -4.42f;
|
||||||
Cmq = -10.5f;
|
Cmq = -10.5f;
|
||||||
Cmde_n = -1.05f*de_max;
|
Cmde_n = -1.05f*de_max;
|
||||||
Cmdf_n = -0.059f*df_max;
|
Cmdf_n = -0.059f*df_max;
|
||||||
|
#else
|
||||||
|
Cma = 0.0f;
|
||||||
|
Cmadot = 0.0f;
|
||||||
|
Cmq = 0.0f;
|
||||||
|
Cmde_n = -1.05f*de_max;
|
||||||
|
Cmdf_n = 0.0f*df_max;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if 0
|
||||||
Cnb = 0.0630f;
|
Cnb = 0.0630f;
|
||||||
Cnp = -0.0028f;
|
Cnp = -0.0028f;
|
||||||
Cnr = -0.0681f;
|
Cnr = -0.0681f;
|
||||||
Cnda_n = -0.0100f*da_max;
|
Cnda_n = -0.0100f*da_max;
|
||||||
Cndr_n = -0.0398f*dr_max;
|
Cndr_n = -0.0398f*dr_max;
|
||||||
|
#else
|
||||||
|
Cnb = 0.0f;
|
||||||
|
Cnp = 0.0f;
|
||||||
|
Cnr = 0.0f;
|
||||||
|
Cnda_n = 0.0f*da_max;
|
||||||
|
Cndr_n = 0.0f*dr_max;
|
||||||
|
#endif
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -2,7 +2,7 @@
|
||||||
//
|
//
|
||||||
// Written by Erik Hofman, started November 2016
|
// Written by Erik Hofman, started November 2016
|
||||||
//
|
//
|
||||||
// Copyright (C) 2016 Erik Hofman <erik@ehofman.com>
|
// Copyright (C) 2016,2017 Erik Hofman <erik@ehofman.com>
|
||||||
//
|
//
|
||||||
//
|
//
|
||||||
// This program is free software; you can redistribute it and/or
|
// This program is free software; you can redistribute it and/or
|
||||||
|
@ -37,6 +37,7 @@
|
||||||
# include <FDM/flight.hxx>
|
# include <FDM/flight.hxx>
|
||||||
#else
|
#else
|
||||||
# include "simd.hxx"
|
# include "simd.hxx"
|
||||||
|
# include "simd4x4.hxx"
|
||||||
# define SG_METER_TO_FEET 3.2808399
|
# define SG_METER_TO_FEET 3.2808399
|
||||||
# define SG_FEET_TO_METER (1/SG_METER_TO_FEET)
|
# define SG_FEET_TO_METER (1/SG_METER_TO_FEET)
|
||||||
# define SGD_DEGREES_TO_RADIANS 0.0174532925
|
# define SGD_DEGREES_TO_RADIANS 0.0174532925
|
||||||
|
@ -64,7 +65,7 @@ private:
|
||||||
enum { MAX=0, VELOCITY=1, PROPULSION=2 };
|
enum { MAX=0, VELOCITY=1, PROPULSION=2 };
|
||||||
enum { 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 };
|
enum { DRAG=0, SIDE=1, LIFT=2 };
|
||||||
enum { ROLL=0, PITCH=1, YAW=2 };
|
enum { ROLL=0, PITCH=1, YAW=2, THRUST=3 };
|
||||||
enum { ALPHA=0, BETA=1 };
|
enum { ALPHA=0, BETA=1 };
|
||||||
enum { PHI, THETA, PSI };
|
enum { PHI, THETA, PSI };
|
||||||
enum { P=0, Q=1, R=2 };
|
enum { P=0, Q=1, R=2 };
|
||||||
|
@ -92,21 +93,21 @@ public:
|
||||||
|
|
||||||
/* controls */
|
/* controls */
|
||||||
inline void set_rudder_norm(float f) {
|
inline void set_rudder_norm(float f) {
|
||||||
xCDYL[RUDDER][SIDE] = CYdr_n*f;
|
xCDYLT.ptr()[RUDDER][SIDE] = CYdr_n*f;
|
||||||
xClmn[RUDDER][ROLL] = Cldr_n*f;
|
xClmnT.ptr()[RUDDER][ROLL] = Cldr_n*f;
|
||||||
xClmn[RUDDER][YAW] = Cndr_n*f;
|
xClmnT.ptr()[RUDDER][YAW] = Cndr_n*f;
|
||||||
}
|
}
|
||||||
inline void set_elevator_norm(float f) {
|
inline void set_elevator_norm(float f) {
|
||||||
xClmn[ELEVATOR][PITCH] = Cmde_n*f;
|
xClmnT.ptr()[ELEVATOR][PITCH] = Cmde_n*f;
|
||||||
}
|
}
|
||||||
inline void set_aileron_norm(float f) {
|
inline void set_aileron_norm(float f) {
|
||||||
xClmn[AILERON][ROLL] = Clda_n*f;
|
xClmnT.ptr()[AILERON][ROLL] = Clda_n*f;
|
||||||
xClmn[AILERON][YAW] = Cnda_n*f;
|
xClmnT.ptr()[AILERON][YAW] = Cnda_n*f;
|
||||||
}
|
}
|
||||||
inline void set_flaps_norm(float f) {
|
inline void set_flaps_norm(float f) {
|
||||||
xCDYL[FLAPS][LIFT] = CLdf_n*f;
|
xCDYLT.ptr()[FLAPS][LIFT] = CLdf_n*f;
|
||||||
xCDYL[FLAPS][DRAG] = CDdf_n*f;
|
xCDYLT.ptr()[FLAPS][DRAG] = CDdf_n*std::abs(f);
|
||||||
xClmn[FLAPS][PITCH] = Cmdf_n*f;
|
xClmnT.ptr()[FLAPS][PITCH] = Cmdf_n*f;
|
||||||
}
|
}
|
||||||
inline void set_throttle_norm(float f) { th = 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; }
|
||||||
|
@ -122,21 +123,21 @@ public:
|
||||||
inline void set_altitude_agl_ft(float f) { agl = f; }
|
inline void set_altitude_agl_ft(float f) { agl = f; }
|
||||||
|
|
||||||
inline void set_euler_angles_rad(const simd4_t<float,3>& e) {
|
inline void set_euler_angles_rad(const simd4_t<float,3>& e) {
|
||||||
euler_body = e;
|
euler = e;
|
||||||
}
|
}
|
||||||
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_body = simd4_t<float,3>(phi, theta, psi);
|
euler = simd4_t<float,3>(phi, theta, psi);
|
||||||
}
|
}
|
||||||
inline void set_pitch_rad(float f) { euler_body[PHI] = f; }
|
inline void set_pitch_rad(float f) { euler[PHI] = f; }
|
||||||
inline void set_roll_rad(float f) { euler_body[THETA] = f; }
|
inline void set_roll_rad(float f) { euler[THETA] = f; }
|
||||||
inline void set_heading_rad(float f) { euler_body[PSI] = f; }
|
inline void set_heading_rad(float f) { euler[PSI] = f; }
|
||||||
|
|
||||||
void set_velocity_fps(const simd4_t<float,3>& v) { UVW_body = v; }
|
void set_velocity_fps(const simd4_t<float,3>& v) { vUVW = v; }
|
||||||
void set_velocity_fps(float u, float v, float w) {
|
void set_velocity_fps(float u, float v, float w) {
|
||||||
UVW_body = simd4_t<float,3>(u, v, w);
|
vUVW = simd4_t<float,3>(u, v, w);
|
||||||
}
|
}
|
||||||
void set_velocity_fps(float u) {
|
void set_velocity_fps(float u) {
|
||||||
UVW_body = simd4_t<float,3>(u, 0.0f, 0.0f);
|
vUVW = simd4_t<float,3>(u, 0.0f, 0.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void set_wind_ned_fps(const simd4_t<float,3>& w) { wind_ned = w; }
|
inline void set_wind_ned_fps(const simd4_t<float,3>& w) { wind_ned = w; }
|
||||||
|
@ -145,27 +146,28 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void set_alpha_rad(float f) {
|
inline void set_alpha_rad(float f) {
|
||||||
xCDYL[ALPHA][DRAG] = CDa*std::abs(f);
|
xCDYLT.ptr()[ALPHA][DRAG] = CDa*std::abs(f);
|
||||||
xCDYL[ALPHA][LIFT] = CLa*f;
|
xCDYLT.ptr()[ALPHA][LIFT] = -CLa*f;
|
||||||
xClmn[ALPHA][PITCH] = Cma*f;
|
xClmnT.ptr()[ALPHA][PITCH] = Cma*f;
|
||||||
AOA_body[ALPHA] = f;
|
AOA[ALPHA] = f;
|
||||||
}
|
}
|
||||||
inline void set_beta_rad(float f) {
|
inline void set_beta_rad(float f) {
|
||||||
xCDYL[BETA][DRAG] = CDb*std::abs(f);
|
xCDYLT.ptr()[BETA][DRAG] = CDb*std::abs(f);
|
||||||
xCDYL[BETA][SIDE] = CYb*f;
|
xCDYLT.ptr()[BETA][SIDE] = CYb*f;
|
||||||
xClmn[BETA][ROLL] = Clb*f;
|
xClmnT.ptr()[BETA][ROLL] = Clb*f;
|
||||||
xClmn[BETA][YAW] = Cnb*f;
|
xClmnT.ptr()[BETA][YAW] = Cnb*f;
|
||||||
AOA_body[BETA] = f;
|
AOA[BETA] = f;
|
||||||
}
|
}
|
||||||
inline float get_alpha_rad() {
|
inline float get_alpha_rad() {
|
||||||
return AOA_body[ALPHA];
|
return AOA[ALPHA];
|
||||||
}
|
}
|
||||||
inline float get_beta_rad() {
|
inline float get_beta_rad() {
|
||||||
return AOA_body[BETA];
|
return AOA[BETA];
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void update_velocity(float v);
|
void update_velocity(float v);
|
||||||
|
simd4x4_t<float,4> invert_inertia(simd4x4_t<float,4> mtx);
|
||||||
|
|
||||||
/* aircraft normalized controls */
|
/* aircraft normalized controls */
|
||||||
float th; /* throttle command */
|
float th; /* throttle command */
|
||||||
|
@ -173,15 +175,15 @@ private:
|
||||||
|
|
||||||
/* aircraft state */
|
/* aircraft state */
|
||||||
simd4_t<double,3> location_geod; /* lat, lon, altitude */
|
simd4_t<double,3> location_geod; /* lat, lon, altitude */
|
||||||
simd4_t<float,5> NED_distance; /* North, East, Down rel. pos. */
|
simd4_t<float,3> aXYZ; /* local body accelrations */
|
||||||
simd4_t<float,3> NED_body; /* North, East, Down speeds */
|
simd4_t<float,3> NEDdot; /* North, East, Down velocity */
|
||||||
simd4_t<float,3> UVW_body; /* fwd, up, side speed */
|
simd4_t<float,3> vUVW; /* fwd, side, down velocity */
|
||||||
simd4_t<float,3> UVW_dot; /* fwd, up, side accelerations */
|
simd4_t<float,3> vUVWdot; /* fwd, side, down accel. */
|
||||||
simd4_t<float,3> PQR_body; /* P, Q, R rate */
|
simd4_t<float,3> vPQR; /* roll, pitch, yaw rate */
|
||||||
simd4_t<float,3> PQR_dot; /* P, Q, R accelerations */
|
simd4_t<float,3> vPQRdot; /* roll, pitch, yaw accel. */
|
||||||
simd4_t<float,2> AOA_body; /* alpha, beta */
|
simd4_t<float,3> AOA; /* alpha, beta */
|
||||||
simd4_t<float,2> AOA_dot; /* adot, bdot */
|
simd4_t<float,3> AOAdot; /* adot, bdot */
|
||||||
simd4_t<float,3> euler_body; /* phi, theta, psi */
|
simd4_t<float,3> euler; /* phi, theta, psi */
|
||||||
simd4_t<float,3> euler_dot; /* change in phi, theta, psi */
|
simd4_t<float,3> euler_dot; /* change in phi, theta, psi */
|
||||||
simd4_t<float,3> wind_ned; /* wind north, east, down */
|
simd4_t<float,3> wind_ned; /* wind north, east, down */
|
||||||
|
|
||||||
|
@ -191,27 +193,30 @@ private:
|
||||||
/* run 20 to 60 times (or more) per second */
|
/* run 20 to 60 times (or more) per second */
|
||||||
|
|
||||||
/* cache */
|
/* cache */
|
||||||
simd4_t<float,3> UVW_aero; /* airmass relative to the body */
|
simd4_t<float,3> vUVWaero; /* airmass relative to the body */
|
||||||
simd4_t<float,3> FT[AISIM_MAX]; /* thrust force */
|
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> FTM[AISIM_MAX]; /* thrust due to mach force */
|
||||||
simd4_t<float,3> MT[AISIM_MAX]; /* thrust moment */
|
simd4_t<float,3> MT[AISIM_MAX]; /* thrust moment */
|
||||||
simd4_t<float,3> IQR_dot_fact;
|
simd4_t<float,3> b_2U, cbar_2U;
|
||||||
float agl, velocity, mach;
|
simd4_t<float,3> inv_m;
|
||||||
float b_2U, cbar_2U;
|
float velocity, mach;
|
||||||
|
float agl;
|
||||||
bool WoW;
|
bool WoW;
|
||||||
|
|
||||||
/* dynamic coefficients (already multiplied with their value) */
|
/* dynamic coefficients (already multiplied with their value) */
|
||||||
simd4_t<float,3> xCqadot[2], xCpr[2];
|
simd4_t<float,3> xCq, xCadot, xCp, xCr;
|
||||||
simd4_t<float,3> xCDYL[4];
|
simd4x4_t<float,4> xCDYLT;
|
||||||
simd4_t<float,3> xClmn[4];
|
simd4x4_t<float,4> xClmnT;
|
||||||
simd4_t<float,3> C2F, C2M;
|
simd4_t<float,4> Coef2Force;
|
||||||
|
simd4_t<float,4> Coef2Moment;
|
||||||
|
|
||||||
/* ---------------------------------------------------------------- */
|
/* ---------------------------------------------------------------- */
|
||||||
/* aircraft static data */
|
/* aircraft static data */
|
||||||
int no_engines, no_gears;
|
int no_engines, no_gears;
|
||||||
|
simd4x4_t<float,4> mI, mIinv; /* inertia matrix */
|
||||||
simd4_t<float,3> gear_pos[AISIM_MAX]; /* pos 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> cg; /* center of gravity */
|
||||||
simd4_t<float,3> I; /* inertia */
|
simd4_t<float,4> 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 */
|
||||||
|
|
||||||
|
|
Loading…
Add table
Reference in a new issue