fly/flightgear
1
0
Fork 0
Code Activity

Add the latest code for reference in the JSBSim mailing list while bug hunting

Browse source
This commit is contained in:
Erik Hofman 2017-01-14 14:33:28 +01:00
parent 1ef1d5ad43
commit 9f9f1c631f
2 changed files with 258 additions and 179 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -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;
} }

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

@ -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 */