fly/flightgear
1
0
Fork 0
Code Activity

use in-place declarations and clean up the code a bit

Browse source
This commit is contained in:
Erik Hofman 2020-09-04 16:03:04 +02:00
parent c6cb845f90
commit a95aaab0f4
2 changed files with 84 additions and 98 deletions

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

@ -47,31 +47,11 @@
#endif #endif
FGAISim::FGAISim(double dt) : FGAISim::FGAISim(double dt)
location_geod(0.0),
NEDdot(0.0f),
vUVW(0.0f),
vUVWdot(0.0f),
vPQR(0.0f),
vPQRdot(0.0f),
AOA(0.0f),
AOAdot(0.0f),
euler(0.0f),
euler_dot(0.0f),
wind_ned(0.0f),
vUVWaero(0.0f),
b_2U(0.0f),
cbar_2U(0.0f),
cg(0.0f),
I(0.0f),
gravity_ned(0.0f, 0.0f, AISIM_G)
{ {
simd4x4::zeros(xCDYLT); simd4x4::zeros(xCDYLT);
simd4x4::zeros(xClmnT); simd4x4::zeros(xClmnT);
xCq = xCadot = 0.0f;
xCp = xCr = 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;
@ -98,7 +78,7 @@ FGAISim::FGAISim(double dt) :
set_alpha_rad(0.0f); set_alpha_rad(0.0f);
set_beta_rad(0.0f); set_beta_rad(0.0f);
/* useful constants */ /* useful constants assigned to a vector */
xCp[SIDE] = CYp; xCp[SIDE] = CYp;
xCr[SIDE] = CYr; xCr[SIDE] = CYr;
xCp[ROLL] = Clp; xCp[ROLL] = Clp;
@ -115,6 +95,7 @@ FGAISim::FGAISim(double dt) :
xCDYLT.ptr()[MIN][LIFT] = -CLmin; xCDYLT.ptr()[MIN][LIFT] = -CLmin;
xCDYLT.ptr()[MIN][DRAG] = -CDmin; xCDYLT.ptr()[MIN][DRAG] = -CDmin;
/* m is assigned in the load function */
inv_m = 1.0f/m; 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
@ -139,12 +120,12 @@ FGAISim::FGAISim(double dt) :
simd4x4::translate(mcg, cg); simd4x4::translate(mcg, cg);
mI = aiMtx4( I[XX], 0.0f, -I[XZ], 0.0f, mI = aiMtx4( I[XX], 0.0f, -I[XZ], 0.0f,
0.0f, I[YY], 0.0f, 0.0f, 0.0f, I[YY], 0.0f, 0.0f,
-I[XZ], 0.0f, I[ZZ], 0.0f, -I[XZ], 0.0f, I[ZZ], 0.0f,
0.0f, 0.0f, 0.0f, 0.0f); 0.0f, 0.0f, 0.0f, 0.0f);
mIinv = invert_inertia(mI); mIinv = invert_inertia(mI);
// mI *= mcg; mI *= mcg;
// mIinv *= matrix_inverse(mcg); mIinv *= matrix_inverse(mcg);
} }
FGAISim::~FGAISim() FGAISim::~FGAISim()
@ -246,7 +227,6 @@ FGAISim::update(double ddt)
float CL = CDYL[LIFT]; float CL = CDYL[LIFT];
CDYL += aiVec3(CDi*CL*CL, 0.0f, 0.0f); CDYL += aiVec3(CDi*CL*CL, 0.0f, 0.0f);
printf("CDYL: %7.2f, %7.2f, %7.2f\n", CDYL[DRAG], CDYL[SIDE], CDYL[LIFT]);
#if 0 #if 0
printf(" CLa: %6.3f, CLadot: %6.3f, CLq: %6.3f\n", xCDYLT.ptr()[ALPHA][LIFT],CLadot*adot,CLq*q); 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(" CDa: %6.3f, CDb: %6.3f, CDi: %6.3f\n", xCDYLT.ptr()[ALPHA][DRAG],xCDYLT.ptr()[BETA][DRAG],CDi*CL*CL);
@ -415,6 +395,25 @@ FGAISim::copy_to_AISim()
bool bool
FGAISim::copy_from_AISim() FGAISim::copy_from_AISim()
{ {
// Mass properties and geometry values
// _set_Inertias( m, I[XX], I[YY], I[ZZ], I[XZ] );
_set_CG_Position( cg[X], cg[Y], cg[Z]);
// Accelerations
_set_Accels_Body( vUVWdot[U], vUVWdot[V], vUVWdot[W] );
// Velocities
_set_V_equiv_kts( velocity*std::sqrt(sigma) * SG_FPS_TO_KT );
_set_V_calibrated_kts( std::sqrt( 2.0f*qbar*sigma/rho) * SG_FPS_TO_KT );
_set_V_ground_speed( simd4::magnitude(NEDdot) * SG_FPS_TO_KT );
_set_Mach_number( mach );
_set_Velocities_Local( NEDdot[NORTH], NEDdot[EAST], NEDdot[DOWN] );
_set_Velocities_Local_Airmass( vUVWaero[U], vUVWaero[V], vUVWaero[W] );
_set_Velocities_Body( vUVW[U], vUVW[V], vUVW[W] );
_set_Omega_Body( vPQR[P], vPQR[Q], vPQR[R] );
_set_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
// Positions // Positions
_set_Geodetic_Position( location_geod[LATITUDE], location_geod[LONGITUDE]); _set_Geodetic_Position( location_geod[LATITUDE], location_geod[LONGITUDE]);
@ -425,31 +424,14 @@ FGAISim::copy_from_AISim()
_update_ground_elev_at_pos(); _update_ground_elev_at_pos();
_set_Sea_level_radius( sl_radius * SG_METER_TO_FEET); _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET);
_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() - gear_pos[0][Z]);
// _set_Euler_Angles( roll, pitch, heading );
float heading = euler[PSI]; float heading = euler[PSI];
if (heading < 0) heading += SGD_2PI; if (heading < 0) heading += (2.0f*SG_PI);
_set_Euler_Angles( euler[PHI], euler[THETA], heading ); _set_Euler_Angles( euler[PHI], euler[THETA], heading );
_set_Alpha( AOA[ALPHA] ); _set_Alpha( AOA[ALPHA] );
_set_Beta( AOA[BETA] ); _set_Beta( AOA[BETA] );
// Velocities
_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(NEDdot) * SG_FPS_TO_KT );
_set_Mach_number( mach );
_set_Velocities_Local( NEDdot[NORTH], NEDdot[EAST], NEDdot[DOWN] );
// _set_Velocities_Local_Airmass( vUVWaero[U], vUVWaero[V], vUVWaero[W] );
_set_Velocities_Body( vUVW[U], vUVW[V], vUVW[W] );
_set_Omega_Body( vPQR[P], vPQR[Q], vPQR[R] );
_set_Euler_Rates( euler_dot[PHI], euler_dot[THETA], euler_dot[PSI] );
// Accelerations
// set_Accels_Omega( vPQRdot[P], vPQRdot[Q], vPQRdot[R] );
_set_Accels_Body( vUVWdot[U], vUVWdot[V], vUVWdot[W] );
return true; return true;
} }
#endif #endif
@ -615,28 +597,28 @@ FGAISim::load(std::string path)
// gear ground contact points relative tot aero ref. pt. at (0,0,0) // gear ground contact points relative tot aero ref. pt. at (0,0,0)
no_gears = 3; no_gears = 3;
/* nose */ /* nose */
gear_pos[0][X] = -50.0f; gear_pos[0][X] = -93.0f;
gear_pos[0][Y] = 0.0f; gear_pos[0][Y] = 0.0f;
gear_pos[0][Z] = -78.9f; gear_pos[0][Z] = -38.9f;
Cg_spring[0] = 1800.0f; Cg_spring[0] = 1800.0f;
Cg_damp[0] = 600.0f; Cg_damp[0] = 600.0f;
/* left */ /* left */
gear_pos[1][X] = 15.0f; gear_pos[1][X] = -5.4f;
gear_pos[1][Y] = -43.0f; gear_pos[1][Y] = -38.0f;
gear_pos[1][Z] = -74.9f; gear_pos[1][Z] = -38.9f;
Cg_spring[1] = 5400.0f; Cg_spring[1] = 5400.0f;
Cg_damp[1] = 1600.0f; Cg_damp[1] = 1600.0f;
/* right */ /* right */
gear_pos[2][X] = 15.0f; gear_pos[2][X] = -5.4f;
gear_pos[2][Y] = 43.0f; gear_pos[2][Y] = 38.0f;
gear_pos[2][Z] = -74.9f; gear_pos[2][Z] = -38.9f;
Cg_spring[2] = 5400.0f; Cg_spring[2] = 5400.0f;
Cg_damp[2] = 1600.0f; Cg_damp[2] = 1600.0f;
float de_max = 24.0f*SGD_DEGREES_TO_RADIANS; float de_max = 24.0f*SG_DEGREES_TO_RADIANS;
float dr_max = 16.0f*SGD_DEGREES_TO_RADIANS; float dr_max = 16.0f*SG_DEGREES_TO_RADIANS;
float da_max = 17.5f*SGD_DEGREES_TO_RADIANS; float da_max = 17.5f*SG_DEGREES_TO_RADIANS;
float df_max = 30.0f*SGD_DEGREES_TO_RADIANS; float df_max = 30.0f*SG_DEGREES_TO_RADIANS;
/* thuster / propulsion */ /* thuster / propulsion */
// (FWD,RIGHT,DOWN) // (FWD,RIGHT,DOWN)
@ -654,14 +636,14 @@ FGAISim::load(std::string path)
simd4::normalize(dir); simd4::normalize(dir);
FT[0] = dir * (CTmax * RPS*RPS * D*D*D*D); // Thrust force 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 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 MT[0] = dir * (-Ixx*(2.0f*RPS*float(SG_PI)));// Thrus moment
// if propeller driven // if propeller driven
/* aerodynamic coefficients */ /* aerodynamic coefficients */
CLmin = 0.007f; CLmin = 0.25f;
CLa = 4.72f; CLa = 4.72f;
CLadot = 1.70f; CLadot = 1.7f;
CLq = 3.90f; CLq = 3.9f;
CLdf_n = 0.705f*df_max; CLdf_n = 0.705f*df_max;
CDmin = 0.036f; CDmin = 0.036f;
@ -675,23 +657,23 @@ FGAISim::load(std::string path)
CYr = 0.21f; CYr = 0.21f;
CYdr_n = 0.187f*dr_max; CYdr_n = 0.187f*dr_max;
Clb = -0.322; // -0.057f; Clb = -0.057f;
Clp = -0.4840; // -0.613f; Clp = -0.613f;
Clr = 2.0f; // 0.079f; Clr = 0.079f;
Clda_n = 0.229f*da_max; // 0.170f*da_max; Clda_n = 0.17f*da_max;
Cldr_n = 0.0147f*dr_max; // 0.01f*dr_max; Cldr_n = 0.0147f*dr_max;
Cma = -1.8f; // -1.0f; Cma = -1.8f;
Cmadot = -5.2f; // -4.42f; Cmadot = -5.2f;
Cmq = -12.4f; // -10.5f; Cmq = -12.4f;
Cmde_n = -1.28f*de_max; // -1.05f*de_max; Cmde_n = -1.05f*de_max;
Cmdf_n = -0.2177f*df_max; // -0.059f*df_max; // Cmdf_n = -0.2177f*df_max;
Cnb = 0.065f; // 0.0630f; Cnb = 0.063f;
Cnp = -0.03f; // -0.0028f; Cnp = -0.0028f;
Cnr = -0.099f; // -0.0681f; Cnr = -0.0937f; // -0.0681f;
Cnda_n = -0.0053f*da_max; // -0.0100f*da_max; Cnda_n = -0.0053f*da_max; // -0.0100f*da_max;
Cndr_n = -0.0657f*dr_max; // -0.0398f*dr_max; Cndr_n = -0.043f*dr_max;
return true; return true;
} }

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

@ -102,7 +102,7 @@ public:
inline void set_rudder_norm(float f) { inline void set_rudder_norm(float f) {
xCDYLT.ptr()[RUDDER][SIDE] = CYdr_n*f; xCDYLT.ptr()[RUDDER][SIDE] = CYdr_n*f;
xClmnT.ptr()[RUDDER][ROLL] = Cldr_n*f; xClmnT.ptr()[RUDDER][ROLL] = Cldr_n*f;
xClmnT.ptr()[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) {
xClmnT.ptr()[ELEVATOR][PITCH] = Cmde_n*f; xClmnT.ptr()[ELEVATOR][PITCH] = Cmde_n*f;
@ -178,22 +178,22 @@ private:
aiMtx4 invert_inertia(aiMtx4 mtx); aiMtx4 invert_inertia(aiMtx4 mtx);
/* aircraft normalized controls */ /* aircraft normalized controls */
float th; /* throttle command */ float th; /* throttle command */
float br = 0.0f; /* brake command */ float br = 0.0f; /* brake command */
/* aircraft state */ /* aircraft state */
aiVec3d location_geod; /* lat, lon, altitude */ aiVec3d location_geod = 0.0; /* lat, lon, altitude */
aiVec3 aXYZ; /* local body accelrations */ aiVec3 aXYZ = 0.0f; /* local body accelrations */
aiVec3 NEDdot; /* North, East, Down velocity */ aiVec3 NEDdot = 0.0f; /* North, East, Down velocity */
aiVec3 vUVW; /* fwd, side, down velocity */ aiVec3 vUVW = 0.0f; /* fwd, side, down velocity */
aiVec3 vUVWdot; /* fwd, side, down accel. */ aiVec3 vUVWdot = 0.0f; /* fwd, side, down accel. */
aiVec3 vPQR; /* roll, pitch, yaw rate */ aiVec3 vPQR = 0.0f; /* roll, pitch, yaw rate */
aiVec3 vPQRdot; /* roll, pitch, yaw accel. */ aiVec3 vPQRdot = 0.0f; /* roll, pitch, yaw accel. */
aiVec3 AOA; /* alpha, beta */ aiVec3 AOA = 0.0f; /* alpha, beta */
aiVec3 AOAdot; /* adot, bdot */ aiVec3 AOAdot = 0.0f; /* adot, bdot */
aiVec3 euler; /* phi, theta, psi */ aiVec3 euler = 0.0f; /* phi, theta, psi */
aiVec3 euler_dot; /* change in phi, theta, psi */ aiVec3 euler_dot = 0.0f; /* change in phi, theta, psi */
aiVec3 wind_ned; /* wind north, east, down */ aiVec3 wind_ned = 0.0f; /* wind north, east, down */
/* ---------------------------------------------------------------- */ /* ---------------------------------------------------------------- */
/* This should reduce the time spent in update() since controls */ /* This should reduce the time spent in update() since controls */
@ -201,11 +201,12 @@ private:
/* run 20 to 60 times (or more) per second */ /* run 20 to 60 times (or more) per second */
/* cache */ /* cache */
aiVec3 vUVWaero; /* airmass relative to the body */ aiVec3 vUVWaero = 0.0f; /* airmass relative to the body */
aiVec3 FT[AISIM_MAX]; /* thrust force */ aiVec3 FT[AISIM_MAX]; /* thrust force */
aiVec3 FTM[AISIM_MAX]; /* thrust due to mach force */ aiVec3 FTM[AISIM_MAX]; /* thrust due to mach force */
aiVec3 MT[AISIM_MAX]; /* thrust moment */ aiVec3 MT[AISIM_MAX]; /* thrust moment */
aiVec3 b_2U, cbar_2U; aiVec3 b_2U = 0.0f;
aiVec3 cbar_2U = 0.0f;
aiVec3 inv_m; aiVec3 inv_m;
float velocity = 0.0f; float velocity = 0.0f;
float mach = 0.0f; float mach = 0.0f;
@ -213,7 +214,10 @@ private:
bool WoW = true; bool WoW = true;
/* dynamic coefficients (already multiplied with their value) */ /* dynamic coefficients (already multiplied with their value) */
aiVec3 xCq, xCadot, xCp, xCr; aiVec3 xCq = 0.0f;
aiVec3 xCadot = 0.0f;
aiVec3 xCp = 0.0f;
aiVec3 xCr = 0.0f;
aiMtx4 xCDYLT; aiMtx4 xCDYLT;
aiMtx4 xClmnT; aiMtx4 xClmnT;
aiVec4 Coef2Force; aiVec4 Coef2Force;
@ -225,8 +229,8 @@ private:
int no_gears = 0; int no_gears = 0;
aiMtx4 mI, mIinv; /* inertia matrix */ aiMtx4 mI, mIinv; /* inertia matrix */
aiVec3 gear_pos[AISIM_MAX]; /* pos in structural frame */ aiVec3 gear_pos[AISIM_MAX]; /* pos in structural frame */
aiVec3 cg; /* center of gravity */ aiVec3 cg = 0.0f; /* center of gravity */
aiVec4 I; /* inertia */ aiVec4 I = 0.0f; /* inertia */
float Sw = 0.0f; /* wing area */ float Sw = 0.0f; /* wing area */
float cbar = 0.0f; /* mean average chord */ float cbar = 0.0f; /* mean average chord */
float b = 0.0f; /* wing span */ float b = 0.0f; /* wing span */
@ -245,7 +249,7 @@ private:
/* environment data */ /* environment data */
static float density[101], vsound[101]; static float density[101], vsound[101];
aiVec3 gravity_ned; aiVec3 gravity_ned = { 0.0f, 0.0f, AISIM_G };
float rho = 0.0f; float rho = 0.0f;
float qbar = 0.0f; float qbar = 0.0f;
float sigma = 0.0f; float sigma = 0.0f;