From a95aaab0f401ac3d13cf1f32cd90c8daa06bde30 Mon Sep 17 00:00:00 2001
From: Erik Hofman <erik@ehofman.com>
Date: Fri, 4 Sep 2020 16:03:04 +0200
Subject: [PATCH] use in-place declarations and clean up the code a bit
---
src/FDM/SP/AISim.cpp | 136 +++++++++++++++++++------------------------
src/FDM/SP/AISim.hpp | 46 ++++++++-------
2 files changed, 84 insertions(+), 98 deletions(-)
diff --git a/src/FDM/SP/AISim.cpp b/src/FDM/SP/AISim.cpp
index 2fd2de4ae..e8b78f97d 100644
--- a/src/FDM/SP/AISim.cpp
+++ b/src/FDM/SP/AISim.cpp
@@ -47,31 +47,11 @@
#endif
-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)
+FGAISim::FGAISim(double dt)
{
simd4x4::zeros(xCDYLT);
simd4x4::zeros(xClmnT);
- xCq = xCadot = 0.0f;
- xCp = xCr = 0.0f;
-
for (int i=0; i<AISIM_MAX; ++i) {
FT[i] = FTM[i] = MT[i] = 0.0f;
@@ -98,7 +78,7 @@ FGAISim::FGAISim(double dt) :
set_alpha_rad(0.0f);
set_beta_rad(0.0f);
- /* useful constants */
+ /* useful constants assigned to a vector */
xCp[SIDE] = CYp;
xCr[SIDE] = CYr;
xCp[ROLL] = Clp;
@@ -115,6 +95,7 @@ FGAISim::FGAISim(double dt) :
xCDYLT.ptr()[MIN][LIFT] = -CLmin;
xCDYLT.ptr()[MIN][DRAG] = -CDmin;
+ /* m is assigned in the load function */
inv_m = 1.0f/m;
// calculate the initial c.g. position above ground level to position
@@ -139,12 +120,12 @@ FGAISim::FGAISim(double dt) :
simd4x4::translate(mcg, cg);
mI = aiMtx4( 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);
+ 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);
-// mI *= mcg;
-// mIinv *= matrix_inverse(mcg);
+ mI *= mcg;
+ mIinv *= matrix_inverse(mcg);
}
FGAISim::~FGAISim()
@@ -246,7 +227,6 @@ FGAISim::update(double ddt)
float CL = CDYL[LIFT];
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
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);
@@ -415,6 +395,25 @@ FGAISim::copy_to_AISim()
bool
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
_set_Geodetic_Position( location_geod[LATITUDE], location_geod[LONGITUDE]);
@@ -425,31 +424,14 @@ FGAISim::copy_from_AISim()
_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_Altitude_AGL( location_geod[ALTITUDE] - get_Runway_altitude() - gear_pos[0][Z]);
-// _set_Euler_Angles( roll, pitch, heading );
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_Alpha( AOA[ALPHA] );
_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;
}
#endif
@@ -615,28 +597,28 @@ FGAISim::load(std::string path)
// gear ground contact points relative tot aero ref. pt. at (0,0,0)
no_gears = 3;
/* nose */
- gear_pos[0][X] = -50.0f;
+ gear_pos[0][X] = -93.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_damp[0] = 600.0f;
/* left */
- gear_pos[1][X] = 15.0f;
- gear_pos[1][Y] = -43.0f;
- gear_pos[1][Z] = -74.9f;
+ gear_pos[1][X] = -5.4f;
+ gear_pos[1][Y] = -38.0f;
+ gear_pos[1][Z] = -38.9f;
Cg_spring[1] = 5400.0f;
Cg_damp[1] = 1600.0f;
/* right */
- gear_pos[2][X] = 15.0f;
- gear_pos[2][Y] = 43.0f;
- gear_pos[2][Z] = -74.9f;
+ gear_pos[2][X] = -5.4f;
+ gear_pos[2][Y] = 38.0f;
+ gear_pos[2][Z] = -38.9f;
Cg_spring[2] = 5400.0f;
Cg_damp[2] = 1600.0f;
- 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;
+ 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;
/* thuster / propulsion */
// (FWD,RIGHT,DOWN)
@@ -654,14 +636,14 @@ FGAISim::load(std::string path)
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
+ MT[0] = dir * (-Ixx*(2.0f*RPS*float(SG_PI)));// Thrus moment
// if propeller driven
/* aerodynamic coefficients */
- CLmin = 0.007f;
+ CLmin = 0.25f;
CLa = 4.72f;
- CLadot = 1.70f;
- CLq = 3.90f;
+ CLadot = 1.7f;
+ CLq = 3.9f;
CLdf_n = 0.705f*df_max;
CDmin = 0.036f;
@@ -675,23 +657,23 @@ FGAISim::load(std::string path)
CYr = 0.21f;
CYdr_n = 0.187f*dr_max;
- Clb = -0.322; // -0.057f;
- Clp = -0.4840; // -0.613f;
- Clr = 2.0f; // 0.079f;
- Clda_n = 0.229f*da_max; // 0.170f*da_max;
- Cldr_n = 0.0147f*dr_max; // 0.01f*dr_max;
+ Clb = -0.057f;
+ Clp = -0.613f;
+ Clr = 0.079f;
+ Clda_n = 0.17f*da_max;
+ Cldr_n = 0.0147f*dr_max;
- Cma = -1.8f; // -1.0f;
- Cmadot = -5.2f; // -4.42f;
- Cmq = -12.4f; // -10.5f;
- Cmde_n = -1.28f*de_max; // -1.05f*de_max;
- Cmdf_n = -0.2177f*df_max; // -0.059f*df_max;
+ Cma = -1.8f;
+ Cmadot = -5.2f;
+ Cmq = -12.4f;
+ Cmde_n = -1.05f*de_max;
+// Cmdf_n = -0.2177f*df_max;
- Cnb = 0.065f; // 0.0630f;
- Cnp = -0.03f; // -0.0028f;
- Cnr = -0.099f; // -0.0681f;
+ Cnb = 0.063f;
+ Cnp = -0.0028f;
+ Cnr = -0.0937f; // -0.0681f;
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;
}
diff --git a/src/FDM/SP/AISim.hpp b/src/FDM/SP/AISim.hpp
index 8a4568d44..28afb14e7 100644
--- a/src/FDM/SP/AISim.hpp
+++ b/src/FDM/SP/AISim.hpp
@@ -102,7 +102,7 @@ public:
inline void set_rudder_norm(float f) {
xCDYLT.ptr()[RUDDER][SIDE] = CYdr_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) {
xClmnT.ptr()[ELEVATOR][PITCH] = Cmde_n*f;
@@ -178,22 +178,22 @@ private:
aiMtx4 invert_inertia(aiMtx4 mtx);
/* aircraft normalized controls */
- float th; /* throttle command */
- float br = 0.0f; /* brake command */
+ float th; /* throttle command */
+ float br = 0.0f; /* brake command */
/* aircraft state */
- aiVec3d location_geod; /* lat, lon, altitude */
- aiVec3 aXYZ; /* local body accelrations */
- aiVec3 NEDdot; /* North, East, Down velocity */
- aiVec3 vUVW; /* fwd, side, down velocity */
- aiVec3 vUVWdot; /* fwd, side, down accel. */
- aiVec3 vPQR; /* roll, pitch, yaw rate */
- aiVec3 vPQRdot; /* roll, pitch, yaw accel. */
- aiVec3 AOA; /* alpha, beta */
- aiVec3 AOAdot; /* adot, bdot */
- aiVec3 euler; /* phi, theta, psi */
- aiVec3 euler_dot; /* change in phi, theta, psi */
- aiVec3 wind_ned; /* wind north, east, down */
+ aiVec3d location_geod = 0.0; /* lat, lon, altitude */
+ aiVec3 aXYZ = 0.0f; /* local body accelrations */
+ aiVec3 NEDdot = 0.0f; /* North, East, Down velocity */
+ aiVec3 vUVW = 0.0f; /* fwd, side, down velocity */
+ aiVec3 vUVWdot = 0.0f; /* fwd, side, down accel. */
+ aiVec3 vPQR = 0.0f; /* roll, pitch, yaw rate */
+ aiVec3 vPQRdot = 0.0f; /* roll, pitch, yaw accel. */
+ aiVec3 AOA = 0.0f; /* alpha, beta */
+ aiVec3 AOAdot = 0.0f; /* adot, bdot */
+ aiVec3 euler = 0.0f; /* phi, theta, psi */
+ aiVec3 euler_dot = 0.0f; /* change in phi, theta, psi */
+ aiVec3 wind_ned = 0.0f; /* wind north, east, down */
/* ---------------------------------------------------------------- */
/* This should reduce the time spent in update() since controls */
@@ -201,11 +201,12 @@ private:
/* run 20 to 60 times (or more) per second */
/* cache */
- aiVec3 vUVWaero; /* airmass relative to the body */
+ aiVec3 vUVWaero = 0.0f; /* airmass relative to the body */
aiVec3 FT[AISIM_MAX]; /* thrust force */
aiVec3 FTM[AISIM_MAX]; /* thrust due to mach force */
aiVec3 MT[AISIM_MAX]; /* thrust moment */
- aiVec3 b_2U, cbar_2U;
+ aiVec3 b_2U = 0.0f;
+ aiVec3 cbar_2U = 0.0f;
aiVec3 inv_m;
float velocity = 0.0f;
float mach = 0.0f;
@@ -213,7 +214,10 @@ private:
bool WoW = true;
/* 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 xClmnT;
aiVec4 Coef2Force;
@@ -225,8 +229,8 @@ private:
int no_gears = 0;
aiMtx4 mI, mIinv; /* inertia matrix */
aiVec3 gear_pos[AISIM_MAX]; /* pos in structural frame */
- aiVec3 cg; /* center of gravity */
- aiVec4 I; /* inertia */
+ aiVec3 cg = 0.0f; /* center of gravity */
+ aiVec4 I = 0.0f; /* inertia */
float Sw = 0.0f; /* wing area */
float cbar = 0.0f; /* mean average chord */
float b = 0.0f; /* wing span */
@@ -245,7 +249,7 @@ private:
/* environment data */
static float density[101], vsound[101];
- aiVec3 gravity_ned;
+ aiVec3 gravity_ned = { 0.0f, 0.0f, AISIM_G };
float rho = 0.0f;
float qbar = 0.0f;
float sigma = 0.0f;