diff --git a/src/FDM/SP/AISim.cpp b/src/FDM/SP/AISim.cpp index b89fe5d2e..6434894a4 100644 --- a/src/FDM/SP/AISim.cpp +++ b/src/FDM/SP/AISim.cpp @@ -20,55 +20,82 @@ // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // +#include "AISim.hpp" + #include #include #include -#ifndef ENABLE_SP_FDM -# define ENABLE_SP_FDM 1 -#endif +#ifdef ENABLE_SP_FDM +# include +# include +# include +# include -#if ENABLE_SP_FDM -#include -#include -#include -#include - -#include -#include
-#include
-#include +# include +# include
+# include
+# include #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; igetStringValue()); #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 (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(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 vector = euler_body; + float angle = simd4::normalize(vector); + simd4x4_t EB_mtx = simd4x4::rotation_matrix(angle, vector); + simd4x4_t BE_mtx = simd4x4::transpose(EB_mtx); - // Earth-to-Body-Axis Transformation Matrix - // body = pitch, roll, yaw - simd4_t angles = euler; - float angle = simd4::normalize(angles); - simd4x4_t EBM = simd4x4::rotation_matrix(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 windb = EBM*wind_ned; + simd4_t 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 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 Ccbar2U = (xCqadot[0]*q + xCqadot[1]*adot)*cbar_2U; - simd4_t Cb2U = (xCpr[0]*p + xCpr[1]*r)*b_2U; + simd4_t Ccbar2U = (xCqadot[0]*q + xCqadot[1]*adot)*cbar_2U; + simd4_t Cb2U = (xCpr[0]*p + xCpr[1]*r )*b_2U; - /* Sum all Drag, Side, Lift, Roll, Pitch, Yaw and Thrust coefficients */ - simd4_t CDYLT(0.0f, Cb2U[SIDE], Ccbar2U[LIFT], 0.0f); - simd4_t ClmnT(Cb2U[ROLL], Ccbar2U[PITCH], Cb2U[YAW], 0.0f); + simd4_t CDYL(0.0f, Cb2U[SIDE], Ccbar2U[LIFT]); + simd4_t 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(CDi*CL*CL, 0.0f, 0.0f, 0.0f); + float CL = CDYL[LIFT]; + CDYL += simd4_t(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 FDYLT = CDYLT*C2F; - simd4_t 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 FDYL = CDYL*C2F; + simd4_t 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 aby = ABY_body; // alpha, beta, gamma - angle = simd4::normalize(aby); + vector = AOA_body; + angle = simd4::normalize(vector); + simd4x4_t WB_mtx = simd4x4::rotation_matrix(angle, vector); + simd4_tFXYZ = WB_mtx*FDYL; - simd4x4_t WBM = simd4x4::rotation_matrix(angle, aby); - simd4_tFXYZ = 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 FLGear = 0.0f; FLGear[Z] = Cgear*gear_comp; -// simd4_t MLGear = 0.0f; - - FXYZ += FLGear; -// MlmnT += MLGear; + /* Thrust */ + for (int i=0; i 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 cg_agl_neu(0.0f, 0.0f, agl); + for (int i=0; i gear_ned = BE_mtx*gear_pos[i]; + simd4_t lg_ground_neu = gear_ned + cg_agl_neu; + if (lg_ground_neu[Z] < 0.0f) { // weight on wheel + simd4_t lg_vrot = simd4::cross(PQR_body, gear_pos[i]); + simd4_t lg_cg_vned = BE_mtx*lg_vrot; + simd4_t 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 Fn_lg(0.0f, 0.0f, Fn); + simd4_t mu_body(-0.02f-0.7f*br, 0.8f, 0.0f); + + simd4_t FLGear = EB_mtx*Fn_lg + UVW_aero*mu_body; + simd4_t 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 qp(PQR_body[Q], PQR_body[P], PQR_body[P]); + simd4_t 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 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 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 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(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 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(-Sqbar, Sqbar, -Sqbar); - C2M = Sbqbar; - C2M[PITCH] = Sqbarcbar; + /* Roll, Pitch, Yaw */ + C2M = simd4_t(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 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; } diff --git a/src/FDM/SP/AISim.hpp b/src/FDM/SP/AISim.hpp index ecf6b6e40..1a9689453 100644 --- a/src/FDM/SP/AISim.hpp +++ b/src/FDM/SP/AISim.hpp @@ -24,36 +24,48 @@ #ifndef _FGAISim_HXX #define _FGAISim_HXX -#include - -#include -#include -//nclude - -#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 #endif -class FGAISim : public FGInterface +#include +#include + +#ifdef ENABLE_SP_FDM +# include +# include +# include +#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& 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(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& 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(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& v) { UVW_body = v; } + void set_velocity_fps(float u, float v, float w) { + UVW_body = simd4_t(u, v, w); + } + void set_velocity_fps(float u) { + UVW_body = simd4_t(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& w) { wind_ned = w; } + inline void set_wind_ned_fps(float n, float e, float d) { + wind_ned = simd4_t(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 NED_cm; /* lat, lon, altitude */ - simd4_t UVW_body; /* fwd, up, side speed */ + simd4_t location_geod; /* lat, lon, altitude */ + simd4_t NED_distance; /* North, East, Down rel. pos. */ + simd4_t NED_body; /* North, East, Down speeds */ + simd4_t UVW_body; /* fwd, up, side speed */ + simd4_t UVW_dot; /* fwd, up, side accelerations */ simd4_t PQR_body; /* P, Q, R rate */ - simd4_t PQR_dot; /* Pdot, Qdot, Rdot accel. */ - simd4_t ABY_body; /* alpha, beta, gamma */ - simd4_t ABY_dot; /* adot, bdot, ydot */ - simd4_t euler; /* phi, theta, psi */ - simd4_t wind_ned; - float agl, velocity; - - /* history, these change between every call to update() */ - simd4_t PQR_body_prev, ABY_body_prev; - + simd4_t PQR_dot; /* P, Q, R accelerations */ + simd4_t AOA_body; /* alpha, beta */ + simd4_t AOA_dot; /* adot, bdot */ + simd4_t euler_body; /* phi, theta, psi */ + simd4_t euler_dot; /* change in phi, theta, psi */ + simd4_t 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 FThrust, MThrust; + /* cache */ + simd4_t UVW_aero; /* airmass relative to the body */ + simd4_t FT[AISIM_MAX]; /* thrust force */ + simd4_t FTM[AISIM_MAX]; /* thrust due to mach force */ + simd4_t MT[AISIM_MAX]; /* thrust moment */ + simd4_t IQR_dot_fact; + float agl, velocity, mach; float b_2U, cbar_2U; + bool WoW; /* dynamic coefficients (already multiplied with their value) */ - simd4_t xCqadot[2], xCpr[2]; - simd4_t xCDYLT[4]; - simd4_t xClmnT[4]; - simd4_t C2F, C2M; + simd4_t xCqadot[2], xCpr[2]; + simd4_t xCDYL[4]; + simd4_t xClmn[4]; + simd4_t C2F, C2M; /* ---------------------------------------------------------------- */ /* aircraft static data */ - int no_engines; - simd4_t gear; /* NED_cms in structural frame */ + int no_engines, no_gears; + simd4_t gear_pos[AISIM_MAX]; /* pos in structural frame */ + simd4_t cg; /* center of gravity */ simd4_t 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 gravity; - float dt2_2m; + /* environment data */ + static float density[101], vsound[101]; + simd4_t gravity_ned; + float rho, qbar, sigma; }; #endif // _FGAISim_HXX