X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2Fflight.cxx;h=74bc1b5d98d6f9d8a0f8fea9b7178f8d427c9172;hb=7041ed907891f7be0205cafe13b218f9f2767efa;hp=10ce9948a18722f84d4b596a6726c574d70b5f5a;hpb=8b57675aef4930245a964816469be922e3004287;p=flightgear.git diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 10ce9948a..74bc1b5d9 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -28,8 +28,9 @@ #include #include -#include #include +#include +#include #include #include
@@ -44,8 +45,6 @@ static inline void assign(double* ptr, const SGVec3d& vec) ptr[2] = vec[2]; } -FGInterface *cur_fdm_state = 0; - // Constructor FGInterface::FGInterface() { @@ -66,14 +65,16 @@ int FGInterface::_calc_multiloop (double dt) { // Since some time the simulation time increments we get here are - // already a multiple of the basic update freqency. + // already a multiple of the basic update frequency. // So, there is no need to do our own multiloop rounding with all bad // roundoff problems when we already have nearly accurate values. // Only the speedup thing must be still handled here int hz = fgGetInt("/sim/model-hz"); - int multiloop = SGMiscd::roundToInt(dt*hz); - int speedup = fgGetInt("/sim/speed-up"); - return multiloop * speedup; + double speedup = fgGetDouble("/sim/speed-up"); + double loops = dt * hz * speedup + delta_loops; + int iloops = SGMiscd::roundToInt(loops); + delta_loops = loops-iloops; // delta_loops required for speed-ups < 1 (to do one iteration every n-th step) + return iloops; } @@ -88,39 +89,41 @@ FGInterface::_setup () inited = false; bound = false; - d_cg_rp_body_v = SGVec3d::zeros(); - v_dot_local_v = SGVec3d::zeros(); - v_dot_body_v = SGVec3d::zeros(); - a_cg_body_v = SGVec3d::zeros(); - a_pilot_body_v = SGVec3d::zeros(); - n_cg_body_v = SGVec3d::zeros(); - v_local_v = SGVec3d::zeros(); - v_local_rel_ground_v = SGVec3d::zeros(); - v_local_airmass_v = SGVec3d::zeros(); - v_wind_body_v = SGVec3d::zeros(); - omega_body_v = SGVec3d::zeros(); - euler_rates_v = SGVec3d::zeros(); - geocentric_rates_v = SGVec3d::zeros(); - geodetic_position_v = SGGeod::fromRadM(0, 0, 0); - cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v); - geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); - euler_angles_v = SGVec3d::zeros(); + _state.d_cg_rp_body_v = SGVec3d::zeros(); + _state.v_dot_local_v = SGVec3d::zeros(); + _state.v_dot_body_v = SGVec3d::zeros(); + _state.a_cg_body_v = SGVec3d::zeros(); + _state.a_pilot_body_v = SGVec3d::zeros(); + _state.n_cg_body_v = SGVec3d::zeros(); + _state.v_local_v = SGVec3d::zeros(); + _state.v_local_rel_ground_v = SGVec3d::zeros(); + _state.v_local_airmass_v = SGVec3d::zeros(); + _state.v_body_v = SGVec3d::zeros(); + _state.omega_body_v = SGVec3d::zeros(); + _state.euler_rates_v = SGVec3d::zeros(); + _state.geocentric_rates_v = SGVec3d::zeros(); + _state.geodetic_position_v = SGGeod::fromRadM(0, 0, 0); + _state.cartesian_position_v = SGVec3d::fromGeod(_state.geodetic_position_v); + _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v); + _state.euler_angles_v = SGVec3d::zeros(); + + _state.nlf=0; + _state.v_rel_wind=_state.v_true_kts=0; + _state.v_ground_speed=_state.v_equiv_kts=0; + _state.v_calibrated_kts=0; + _state.alpha=_state.beta=0; + _state.gamma_vert_rad=0; + _state.density=_state.mach_number=0; + _state.static_pressure=_state.total_pressure=0; + _state.dynamic_pressure=0; + _state.static_temperature=_state.total_temperature=0; + _state.sea_level_radius=_state.earth_position_angle=0; + _state.runway_altitude=0; + _state.climb_rate=0; + _state.altitude_agl=0; + _state.track=0; - nlf=0; - v_rel_wind=v_true_kts=0; - v_ground_speed=v_equiv_kts=0; - v_calibrated_kts=0; - alpha=beta=0; - gamma_vert_rad=0; - density=mach_number=0; - static_pressure=total_pressure=0; - dynamic_pressure=0; - static_temperature=total_temperature=0; - sea_level_radius=earth_position_angle=0; - runway_altitude=0; - climb_rate=0; - altitude_agl=0; - track=0; + delta_loops = 0.0; } void @@ -161,6 +164,8 @@ FGInterface::common_init () double ground_elev_m = get_groundlevel_m(lat, lon, alt_m); double ground_elev_ft = ground_elev_m * SG_METER_TO_FEET; _set_Runway_altitude ( ground_elev_ft ); + + // Set aircraft altitude if ( fgGetBool("/sim/presets/onground") || alt_ft < ground_elev_ft ) { fgSetDouble("/position/altitude-ft", ground_elev_ft + 0.1); set_Altitude( ground_elev_ft + 0.1); @@ -178,21 +183,30 @@ FGInterface::common_init () SG_LOG( SG_FLIGHT, SG_INFO, " lat = " << fgGetDouble("/sim/presets/latitude-deg") << " alt = " << get_Altitude() ); - double slr = SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v); + double slr = SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v); _set_Sea_level_radius( slr * SG_METER_TO_FEET ); + // Set initial Euler angles + SG_LOG( SG_FLIGHT, SG_INFO, "...initializing Euler angles..." ); + set_Euler_Angles( fgGetDouble("/sim/presets/roll-deg") + * SGD_DEGREES_TO_RADIANS, + fgGetDouble("/sim/presets/pitch-deg") + * SGD_DEGREES_TO_RADIANS, + fgGetDouble("/sim/presets/heading-deg") + * SGD_DEGREES_TO_RADIANS ); + // Set initial velocities SG_LOG( SG_FLIGHT, SG_INFO, "...initializing velocities..." ); if ( !fgHasNode("/sim/presets/speed-set") ) { set_V_calibrated_kts(0.0); } else { - const string speedset = fgGetString("/sim/presets/speed-set"); + const std::string speedset = fgGetString("/sim/presets/speed-set"); if ( speedset == "knots" || speedset == "KNOTS" ) { set_V_calibrated_kts( fgGetDouble("/sim/presets/airspeed-kt") ); } else if ( speedset == "mach" || speedset == "MACH" ) { set_Mach_number( fgGetDouble("/sim/presets/mach") ); } else if ( speedset == "UVW" || speedset == "uvw" ) { - set_Velocities_Wind_Body( + set_Velocities_Body( fgGetDouble("/sim/presets/uBody-fps"), fgGetDouble("/sim/presets/vBody-fps"), fgGetDouble("/sim/presets/wBody-fps") ); @@ -209,14 +223,14 @@ FGInterface::common_init () } } - // Set initial Euler angles - SG_LOG( SG_FLIGHT, SG_INFO, "...initializing Euler angles..." ); - set_Euler_Angles( fgGetDouble("/sim/presets/roll-deg") - * SGD_DEGREES_TO_RADIANS, - fgGetDouble("/sim/presets/pitch-deg") - * SGD_DEGREES_TO_RADIANS, - fgGetDouble("/sim/presets/heading-deg") - * SGD_DEGREES_TO_RADIANS ); + if ( fgHasNode("/sim/presets/glideslope-deg") ) + set_Gamma_vert_rad( fgGetDouble("/sim/presets/glideslope-deg") + * SGD_DEGREES_TO_RADIANS ); + else if ( fgHasNode("/sim/presets/speed-set") && + fgHasNode( "/sim/presets/vertical-speed-fps") ) + { + set_Climb_Rate( fgGetDouble("/sim/presets/vertical-speed-fps") ); + } SG_LOG( SG_FLIGHT, SG_INFO, "End common FDM init" ); } @@ -236,152 +250,174 @@ FGInterface::bind () { bound = true; - // Aircraft position - fgTie("/position/latitude-deg", this, - &FGInterface::get_Latitude_deg, - &FGInterface::set_Latitude_deg, - false); + _tiedProperties.setRoot(globals->get_props()); + // Aircraft position + _tiedProperties.Tie("/position/latitude-deg", this, + &FGInterface::get_Latitude_deg, + &FGInterface::set_Latitude_deg, + false); fgSetArchivable("/position/latitude-deg"); - fgTie("/position/longitude-deg", this, - &FGInterface::get_Longitude_deg, - &FGInterface::set_Longitude_deg, - false); + _tiedProperties.Tie("/position/longitude-deg", this, + &FGInterface::get_Longitude_deg, + &FGInterface::set_Longitude_deg, + false); fgSetArchivable("/position/longitude-deg"); - fgTie("/position/altitude-ft", this, - &FGInterface::get_Altitude, - &FGInterface::set_Altitude, - false); + _tiedProperties.Tie("/position/altitude-ft", this, + &FGInterface::get_Altitude, + &FGInterface::set_Altitude, + false); fgSetArchivable("/position/altitude-ft"); - fgTie("/position/altitude-agl-ft", this, - &FGInterface::get_Altitude_AGL); // read-only + _tiedProperties.Tie("/position/altitude-agl-ft", this, + &FGInterface::get_Altitude_AGL, &FGInterface::set_AltitudeAGL, false); fgSetArchivable("/position/ground-elev-ft"); - fgTie("/position/ground-elev-ft", this, - &FGInterface::get_Runway_altitude); // read-only + _tiedProperties.Tie("/position/ground-elev-ft", this, + &FGInterface::get_Runway_altitude); // read-only fgSetArchivable("/position/ground-elev-m"); - fgTie("/position/ground-elev-m", this, - &FGInterface::get_Runway_altitude_m); // read-only - fgTie("/environment/ground-elevation-m", this, - &FGInterface::get_Runway_altitude_m); // read-only + _tiedProperties.Tie("/position/ground-elev-m", this, + &FGInterface::get_Runway_altitude_m); // read-only + _tiedProperties.Tie("/environment/ground-elevation-m", this, + &FGInterface::get_Runway_altitude_m); // read-only fgSetArchivable("/position/sea-level-radius-ft"); - fgTie("/position/sea-level-radius-ft", this, - &FGInterface::get_Sea_level_radius); // read-only - - // Orientation - fgTie("/orientation/roll-deg", this, - &FGInterface::get_Phi_deg, - &FGInterface::set_Phi_deg); + _tiedProperties.Tie("/position/sea-level-radius-ft", this, + &FGInterface::get_Sea_level_radius, + &FGInterface::_set_Sea_level_radius, false); + + // Orientation + _tiedProperties.Tie("/orientation/roll-deg", this, + &FGInterface::get_Phi_deg, + &FGInterface::set_Phi_deg, false); fgSetArchivable("/orientation/roll-deg"); - fgTie("/orientation/pitch-deg", this, - &FGInterface::get_Theta_deg, - &FGInterface::set_Theta_deg); + _tiedProperties.Tie("/orientation/pitch-deg", this, + &FGInterface::get_Theta_deg, + &FGInterface::set_Theta_deg, false); fgSetArchivable("/orientation/pitch-deg"); - fgTie("/orientation/heading-deg", this, - &FGInterface::get_Psi_deg, - &FGInterface::set_Psi_deg); + _tiedProperties.Tie("/orientation/heading-deg", this, + &FGInterface::get_Psi_deg, + &FGInterface::set_Psi_deg, false); fgSetArchivable("/orientation/heading-deg"); - fgTie("/orientation/track-deg", this, - &FGInterface::get_Track); + _tiedProperties.Tie("/orientation/track-deg", this, + &FGInterface::get_Track); // read-only // Body-axis "euler rates" (rotation speed, but in a funny // representation). - fgTie("/orientation/roll-rate-degps", this, - &FGInterface::get_Phi_dot_degps); - fgTie("/orientation/pitch-rate-degps", this, - &FGInterface::get_Theta_dot_degps); - fgTie("/orientation/yaw-rate-degps", this, - &FGInterface::get_Psi_dot_degps); - - // Ground speed knots - fgTie("/velocities/groundspeed-kt", this, - &FGInterface::get_V_ground_speed_kt); - - // Calibrated airspeed - fgTie("/velocities/airspeed-kt", this, - &FGInterface::get_V_calibrated_kts, - &FGInterface::set_V_calibrated_kts, - false); - - // Mach number - fgTie("/velocities/mach", this, - &FGInterface::get_Mach_number, - &FGInterface::set_Mach_number, - false); - - // Local velocities -// fgTie("/velocities/speed-north-fps", this, -// &FGInterface::get_V_north, -// &FGInterface::set_V_north); + _tiedProperties.Tie("/orientation/roll-rate-degps", this, + &FGInterface::get_Phi_dot_degps, + &FGInterface::set_Phi_dot_degps, false); + _tiedProperties.Tie("/orientation/pitch-rate-degps", this, + &FGInterface::get_Theta_dot_degps, + &FGInterface::set_Theta_dot_degps, false); + _tiedProperties.Tie("/orientation/yaw-rate-degps", this, + &FGInterface::get_Psi_dot_degps, + &FGInterface::set_Psi_dot_degps, false); + + _tiedProperties.Tie("/orientation/p-body", this, &FGInterface::get_P_body); // read-only + _tiedProperties.Tie("/orientation/q-body", this, &FGInterface::get_Q_body); // read-only + _tiedProperties.Tie("/orientation/r-body", this, &FGInterface::get_R_body); // read-only + + // Ground speed knots + _tiedProperties.Tie("/velocities/groundspeed-kt", this, + &FGInterface::get_V_ground_speed_kt, + &FGInterface::set_V_ground_speed_kt); // read-only + + // Calibrated airspeed + _tiedProperties.Tie("/velocities/airspeed-kt", this, + &FGInterface::get_V_calibrated_kts, + &FGInterface::set_V_calibrated_kts, + false); + + _tiedProperties.Tie("/velocities/equivalent-kt", this, + &FGInterface::get_V_equiv_kts); // read-only + + // Mach number + _tiedProperties.Tie("/velocities/mach", this, + &FGInterface::get_Mach_number, + &FGInterface::set_Mach_number, + false); + + // Local velocities +// _tiedProperties.Tie("/velocities/speed-north-fps", this, +// &FGInterface::get_V_north, +// &FGInterface::set_V_north); // fgSetArchivable("/velocities/speed-north-fps"); -// fgTie("/velocities/speed-east-fps", this, -// &FGInterface::get_V_east, -// &FGInterface::set_V_east); +// _tiedProperties.Tie("/velocities/speed-east-fps", this, +// &FGInterface::get_V_east, +// &FGInterface::set_V_east); // fgSetArchivable("/velocities/speed-east-fps"); -// fgTie("/velocities/speed-down-fps", this, -// &FGInterface::get_V_down, -// &FGInterface::set_V_down); +// _tiedProperties.Tie("/velocities/speed-down-fps", this, +// &FGInterface::get_V_down, +// &FGInterface::set_V_down); // fgSetArchivable("/velocities/speed-down-fps"); - // FIXME: Temporarily read-only, until the - // incompatibilities between JSBSim and - // LaRCSim are fixed (LaRCSim adds the - // earth's rotation to the east velocity). - fgTie("/velocities/speed-north-fps", this, - &FGInterface::get_V_north); - fgTie("/velocities/speed-east-fps", this, - &FGInterface::get_V_east); - fgTie("/velocities/speed-down-fps", this, - &FGInterface::get_V_down); - - // Relative wind - // FIXME: temporarily archivable, until - // the NED problem is fixed. - fgTie("/velocities/uBody-fps", this, - &FGInterface::get_uBody, - &FGInterface::set_uBody, - false); + + // FIXME: Temporarily read-only, until the + // incompatibilities between JSBSim and + // LaRCSim are fixed (LaRCSim adds the + // earth's rotation to the east velocity). + _tiedProperties.Tie("/velocities/speed-north-fps", this, + &FGInterface::get_V_north, &FGInterface::set_V_north, false); + _tiedProperties.Tie("/velocities/speed-east-fps", this, + &FGInterface::get_V_east, &FGInterface::set_V_east, false); + _tiedProperties.Tie("/velocities/speed-down-fps", this, + &FGInterface::get_V_down, &FGInterface::set_V_down, false); + + _tiedProperties.Tie("/velocities/north-relground-fps", this, + &FGInterface::get_V_north_rel_ground); // read-only + _tiedProperties.Tie("/velocities/east-relground-fps", this, + &FGInterface::get_V_east_rel_ground); // read-only + _tiedProperties.Tie("/velocities/down-relground-fps", this, + &FGInterface::get_V_down_rel_ground); // read-only + + // ECEF velocity in body axis + // FIXME: temporarily archivable, until the NED problem is fixed. + _tiedProperties.Tie("/velocities/uBody-fps", this, + &FGInterface::get_uBody, + &FGInterface::set_uBody, + false); fgSetArchivable("/velocities/uBody-fps"); - fgTie("/velocities/vBody-fps", this, - &FGInterface::get_vBody, - &FGInterface::set_vBody, - false); + _tiedProperties.Tie("/velocities/vBody-fps", this, + &FGInterface::get_vBody, + &FGInterface::set_vBody, + false); fgSetArchivable("/velocities/vBody-fps"); - fgTie("/velocities/wBody-fps", this, - &FGInterface::get_wBody, - &FGInterface::set_wBody, - false); + _tiedProperties.Tie("/velocities/wBody-fps", this, + &FGInterface::get_wBody, + &FGInterface::set_wBody, + false); fgSetArchivable("/velocities/wBody-fps"); - // Climb and slip (read-only) - fgTie("/velocities/vertical-speed-fps", this, - &FGInterface::get_Climb_Rate, - &FGInterface::set_Climb_Rate ); - fgTie("/velocities/glideslope", this, - &FGInterface::get_Gamma_vert_rad, - &FGInterface::set_Gamma_vert_rad ); - fgTie("/orientation/side-slip-rad", this, - &FGInterface::get_Beta); // read-only - fgTie("/orientation/side-slip-deg", this, - &FGInterface::get_Beta_deg); // read-only - fgTie("/orientation/alpha-deg", this, - &FGInterface::get_Alpha_deg); // read-only - fgTie("/accelerations/nlf", this, - &FGInterface::get_Nlf); // read-only - - // NED accelerations - fgTie("/accelerations/ned/north-accel-fps_sec", - this, &FGInterface::get_V_dot_north); - fgTie("/accelerations/ned/east-accel-fps_sec", - this, &FGInterface::get_V_dot_east); - fgTie("/accelerations/ned/down-accel-fps_sec", - this, &FGInterface::get_V_dot_down); - - // Pilot accelerations - fgTie("/accelerations/pilot/x-accel-fps_sec", - this, &FGInterface::get_A_X_pilot); - fgTie("/accelerations/pilot/y-accel-fps_sec", - this, &FGInterface::get_A_Y_pilot); - fgTie("/accelerations/pilot/z-accel-fps_sec", - this, &FGInterface::get_A_Z_pilot); - + // Climb and slip (read-only) + _tiedProperties.Tie("/velocities/vertical-speed-fps", this, + &FGInterface::get_Climb_Rate, + &FGInterface::set_Climb_Rate, false ); + _tiedProperties.Tie("/velocities/glideslope", this, + &FGInterface::get_Gamma_vert_rad, + &FGInterface::set_Gamma_vert_rad, false ); + _tiedProperties.Tie("/orientation/side-slip-rad", this, + &FGInterface::get_Beta, &FGInterface::_set_Beta, false); + _tiedProperties.Tie("/orientation/side-slip-deg", this, + &FGInterface::get_Beta_deg); // read-only + _tiedProperties.Tie("/orientation/alpha-deg", this, + &FGInterface::get_Alpha_deg, &FGInterface::set_Alpha_deg, false); + _tiedProperties.Tie("/accelerations/nlf", this, + &FGInterface::get_Nlf); // read-only + + // NED accelerations + _tiedProperties.Tie("/accelerations/ned/north-accel-fps_sec", + this, &FGInterface::get_V_dot_north); // read-only + _tiedProperties.Tie("/accelerations/ned/east-accel-fps_sec", + this, &FGInterface::get_V_dot_east); // read-only + _tiedProperties.Tie("/accelerations/ned/down-accel-fps_sec", + this, &FGInterface::get_V_dot_down); // read-only + + // Pilot accelerations + _tiedProperties.Tie("/accelerations/pilot/x-accel-fps_sec", + this, &FGInterface::get_A_X_pilot, &FGInterface::set_A_X_pilot, false); + _tiedProperties.Tie("/accelerations/pilot/y-accel-fps_sec", + this, &FGInterface::get_A_Y_pilot, &FGInterface::set_A_Y_pilot, false); + _tiedProperties.Tie("/accelerations/pilot/z-accel-fps_sec", + this, &FGInterface::get_A_Z_pilot, &FGInterface::set_A_Z_pilot, false); + + _tiedProperties.Tie("/accelerations/n-z-cg-fps_sec", + this, &FGInterface::get_N_Z_cg); // read-only } @@ -394,44 +430,8 @@ FGInterface::bind () void FGInterface::unbind () { + _tiedProperties.Untie(); bound = false; - - fgUntie("/position/latitude-deg"); - fgUntie("/position/longitude-deg"); - fgUntie("/position/altitude-ft"); - fgUntie("/position/altitude-agl-ft"); - fgUntie("/position/ground-elev-ft"); - fgUntie("/position/ground-elev-m"); - fgUntie("/environment/ground-elevation-m"); - fgUntie("/position/sea-level-radius-ft"); - fgUntie("/orientation/roll-deg"); - fgUntie("/orientation/pitch-deg"); - fgUntie("/orientation/heading-deg"); - fgUntie("/orientation/track-deg"); - fgUntie("/orientation/roll-rate-degps"); - fgUntie("/orientation/pitch-rate-degps"); - fgUntie("/orientation/yaw-rate-degps"); - fgUntie("/orientation/side-slip-rad"); - fgUntie("/orientation/side-slip-deg"); - fgUntie("/orientation/alpha-deg"); - fgUntie("/velocities/airspeed-kt"); - fgUntie("/velocities/groundspeed-kt"); - fgUntie("/velocities/mach"); - fgUntie("/velocities/speed-north-fps"); - fgUntie("/velocities/speed-east-fps"); - fgUntie("/velocities/speed-down-fps"); - fgUntie("/velocities/uBody-fps"); - fgUntie("/velocities/vBody-fps"); - fgUntie("/velocities/wBody-fps"); - fgUntie("/velocities/vertical-speed-fps"); - fgUntie("/velocities/glideslope"); - fgUntie("/accelerations/nlf"); - fgUntie("/accelerations/pilot/x-accel-fps_sec"); - fgUntie("/accelerations/pilot/y-accel-fps_sec"); - fgUntie("/accelerations/pilot/z-accel-fps_sec"); - fgUntie("/accelerations/ned/north-accel-fps_sec"); - fgUntie("/accelerations/ned/east-accel-fps_sec"); - fgUntie("/accelerations/ned/down-accel-fps_sec"); } /** @@ -443,38 +443,73 @@ FGInterface::update (double dt) SG_LOG(SG_FLIGHT, SG_ALERT, "dummy update() ... SHOULDN'T BE CALLED!"); } +bool FGInterface::readState(SGIOChannel* io) +{ + FlightState buf; + int length = sizeof(FlightState); + + if ( io->get_type() == sgFileType ) { + if ( io->read( (char *)(& buf), length ) == length ) { + SG_LOG( SG_IO, SG_DEBUG, "Success reading data." ); + } else { + return false; + } + } else { + while ( io->read( (char *)(& buf), length ) == length ) { + SG_LOG( SG_IO, SG_DEBUG, "Success reading data." ); + } + } + + _state = buf; // copy the read state over + return true; +} + +bool FGInterface::writeState(SGIOChannel* io) +{ + if (!bound || !inited) { + return false; + } + + int length = sizeof(FlightState); + if ( ! io->write( (char *)(& _state), length ) ) { + SG_LOG( SG_IO, SG_ALERT, "Error writing data." ); + return false; + } + + return true; +} void FGInterface::_updatePositionM(const SGVec3d& cartPos) { - TrackComputer tracker( track, geodetic_position_v ); - cartesian_position_v = cartPos; - geodetic_position_v = SGGeod::fromCart(cartesian_position_v); - geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); - _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET ); + TrackComputer tracker( _state.track, _state.geodetic_position_v ); + _state.cartesian_position_v = cartPos; + _state.geodetic_position_v = SGGeod::fromCart(_state.cartesian_position_v); + _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v); + _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v)*SG_METER_TO_FEET ); _update_ground_elev_at_pos(); } void FGInterface::_updatePosition(const SGGeod& geod) { - TrackComputer tracker( track, geodetic_position_v ); - geodetic_position_v = geod; - cartesian_position_v = SGVec3d::fromGeod(geodetic_position_v); - geocentric_position_v = SGGeoc::fromCart(cartesian_position_v); + TrackComputer tracker( _state.track, _state.geodetic_position_v ); + _state.geodetic_position_v = geod; + _state.cartesian_position_v = SGVec3d::fromGeod(_state.geodetic_position_v); + _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v); - _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET ); + _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v)*SG_METER_TO_FEET ); _update_ground_elev_at_pos(); } void FGInterface::_updatePosition(const SGGeoc& geoc) { - TrackComputer tracker( track, geodetic_position_v ); - geocentric_position_v = geoc; - cartesian_position_v = SGVec3d::fromGeoc(geocentric_position_v); - geodetic_position_v = SGGeod::fromCart(cartesian_position_v); + TrackComputer tracker( _state.track, _state.geodetic_position_v ); + _state.geocentric_position_v = geoc; + _state.cartesian_position_v = SGVec3d::fromGeoc(_state.geocentric_position_v); + _state.geodetic_position_v = SGGeod::fromCart(_state.cartesian_position_v); - _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(geodetic_position_v)*SG_METER_TO_FEET ); + _set_Sea_level_radius( SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v)*SG_METER_TO_FEET ); _update_ground_elev_at_pos(); } @@ -486,129 +521,129 @@ void FGInterface::_updateGeodeticPosition( double lat, double lon, double alt ) void FGInterface::_updateGeocentricPosition( double lat, double lon, - double alt ) + double alt ) { _updatePosition(SGGeoc::fromRadFt(lon, lat, get_Sea_level_radius() + alt)); } void FGInterface::_update_ground_elev_at_pos( void ) { - double groundlevel_m = get_groundlevel_m(geodetic_position_v); + double groundlevel_m = get_groundlevel_m(_state.geodetic_position_v); _set_Runway_altitude( groundlevel_m * SG_METER_TO_FEET ); } // Positions void FGInterface::set_Latitude(double lat) { - geodetic_position_v.setLatitudeRad(lat); + _state.geodetic_position_v.setLatitudeRad(lat); } void FGInterface::set_Longitude(double lon) { - geodetic_position_v.setLongitudeRad(lon); + _state.geodetic_position_v.setLongitudeRad(lon); } void FGInterface::set_Altitude(double alt) { - geodetic_position_v.setElevationFt(alt); + _state.geodetic_position_v.setElevationFt(alt); } void FGInterface::set_AltitudeAGL(double altagl) { - altitude_agl=altagl; + _state.altitude_agl=altagl; } // Velocities void FGInterface::set_V_calibrated_kts(double vc) { - v_calibrated_kts = vc; + _state.v_calibrated_kts = vc; } void FGInterface::set_Mach_number(double mach) { - mach_number = mach; + _state.mach_number = mach; } void FGInterface::set_Velocities_Local( double north, - double east, - double down ){ - v_local_v[0] = north; - v_local_v[1] = east; - v_local_v[2] = down; + double east, + double down ){ + _state.v_local_v[0] = north; + _state.v_local_v[1] = east; + _state.v_local_v[2] = down; } -void FGInterface::set_Velocities_Wind_Body( double u, - double v, - double w){ - v_wind_body_v[0] = u; - v_wind_body_v[1] = v; - v_wind_body_v[2] = w; +void FGInterface::set_Velocities_Body( double u, + double v, + double w){ + _state.v_body_v[0] = u; + _state.v_body_v[1] = v; + _state.v_body_v[2] = w; } // Euler angles void FGInterface::set_Euler_Angles( double phi, - double theta, - double psi ) { - euler_angles_v[0] = phi; - euler_angles_v[1] = theta; - euler_angles_v[2] = psi; + double theta, + double psi ) { + _state.euler_angles_v[0] = phi; + _state.euler_angles_v[1] = theta; + _state.euler_angles_v[2] = psi; } // Flight Path void FGInterface::set_Climb_Rate( double roc) { - climb_rate = roc; + _state.climb_rate = roc; } void FGInterface::set_Gamma_vert_rad( double gamma) { - gamma_vert_rad = gamma; + _state.gamma_vert_rad = gamma; } -void FGInterface::set_Static_pressure(double p) { static_pressure = p; } -void FGInterface::set_Static_temperature(double T) { static_temperature = T; } -void FGInterface::set_Density(double rho) { density = rho; } +void FGInterface::set_Static_pressure(double p) { _state.static_pressure = p; } +void FGInterface::set_Static_temperature(double T) { _state.static_temperature = T; } +void FGInterface::set_Density(double rho) { _state.density = rho; } void FGInterface::set_Velocities_Local_Airmass (double wnorth, - double weast, - double wdown ) { - v_local_airmass_v[0] = wnorth; - v_local_airmass_v[1] = weast; - v_local_airmass_v[2] = wdown; -} - - -void FGInterface::_busdump(void) { - - SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rp_body_v: " << d_cg_rp_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_local_v: " << v_dot_local_v); - SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_body_v: " << v_dot_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"a_cg_body_v: " << a_cg_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"a_pilot_body_v: " << a_pilot_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"n_cg_body_v: " << n_cg_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_v: " << v_local_v); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_rel_ground_v: " << v_local_rel_ground_v); - SG_LOG(SG_FLIGHT,SG_INFO,"v_local_airmass_v: " << v_local_airmass_v); - SG_LOG(SG_FLIGHT,SG_INFO,"v_wind_body_v: " << v_wind_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"omega_body_v: " << omega_body_v); - SG_LOG(SG_FLIGHT,SG_INFO,"euler_rates_v: " << euler_rates_v); - SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_rates_v: " << geocentric_rates_v); - SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_position_v: " << geocentric_position_v); - SG_LOG(SG_FLIGHT,SG_INFO,"geodetic_position_v: " << geodetic_position_v); - SG_LOG(SG_FLIGHT,SG_INFO,"euler_angles_v: " << euler_angles_v); - - SG_LOG(SG_FLIGHT,SG_INFO,"nlf: " << nlf ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_rel_wind: " << v_rel_wind ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_true_kts: " << v_true_kts ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_ground_speed: " << v_ground_speed ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_equiv_kts: " << v_equiv_kts ); - SG_LOG(SG_FLIGHT,SG_INFO,"v_calibrated_kts: " << v_calibrated_kts ); - SG_LOG(SG_FLIGHT,SG_INFO,"alpha: " << alpha ); - SG_LOG(SG_FLIGHT,SG_INFO,"beta: " << beta ); - SG_LOG(SG_FLIGHT,SG_INFO,"gamma_vert_rad: " << gamma_vert_rad ); - SG_LOG(SG_FLIGHT,SG_INFO,"density: " << density ); - SG_LOG(SG_FLIGHT,SG_INFO,"mach_number: " << mach_number ); - SG_LOG(SG_FLIGHT,SG_INFO,"static_pressure: " << static_pressure ); - SG_LOG(SG_FLIGHT,SG_INFO,"total_pressure: " << total_pressure ); - SG_LOG(SG_FLIGHT,SG_INFO,"dynamic_pressure: " << dynamic_pressure ); - SG_LOG(SG_FLIGHT,SG_INFO,"static_temperature: " << static_temperature ); - SG_LOG(SG_FLIGHT,SG_INFO,"total_temperature: " << total_temperature ); - SG_LOG(SG_FLIGHT,SG_INFO,"sea_level_radius: " << sea_level_radius ); - SG_LOG(SG_FLIGHT,SG_INFO,"earth_position_angle: " << earth_position_angle ); - SG_LOG(SG_FLIGHT,SG_INFO,"runway_altitude: " << runway_altitude ); - SG_LOG(SG_FLIGHT,SG_INFO,"climb_rate: " << climb_rate ); - SG_LOG(SG_FLIGHT,SG_INFO,"altitude_agl: " << altitude_agl ); + double weast, + double wdown ) { + _state.v_local_airmass_v[0] = wnorth; + _state.v_local_airmass_v[1] = weast; + _state.v_local_airmass_v[2] = wdown; +} + + +void FGInterface::_busdump(void) +{ + SG_LOG(SG_FLIGHT,SG_INFO,"d_cg_rp_body_v: " << _state.d_cg_rp_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_local_v: " << _state.v_dot_local_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_dot_body_v: " << _state.v_dot_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"a_cg_body_v: " << _state.a_cg_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"a_pilot_body_v: " << _state.a_pilot_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"n_cg_body_v: " << _state.n_cg_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_local_v: " << _state.v_local_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_local_rel_ground_v: " << _state.v_local_rel_ground_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_local_airmass_v: " << _state.v_local_airmass_v); + SG_LOG(SG_FLIGHT,SG_INFO,"v_body_v: " << _state.v_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"omega_body_v: " << _state.omega_body_v); + SG_LOG(SG_FLIGHT,SG_INFO,"euler_rates_v: " << _state.euler_rates_v); + SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_rates_v: " << _state.geocentric_rates_v); + SG_LOG(SG_FLIGHT,SG_INFO,"geocentric_position_v: " << _state.geocentric_position_v); + SG_LOG(SG_FLIGHT,SG_INFO,"geodetic_position_v: " << _state.geodetic_position_v); + SG_LOG(SG_FLIGHT,SG_INFO,"euler_angles_v: " << _state.euler_angles_v); + + SG_LOG(SG_FLIGHT,SG_INFO,"nlf: " << _state.nlf ); + SG_LOG(SG_FLIGHT,SG_INFO,"v_rel_wind: " << _state.v_rel_wind ); + SG_LOG(SG_FLIGHT,SG_INFO,"v_true_kts: " << _state.v_true_kts ); + SG_LOG(SG_FLIGHT,SG_INFO,"v_ground_speed: " << _state.v_ground_speed ); + SG_LOG(SG_FLIGHT,SG_INFO,"v_equiv_kts: " << _state.v_equiv_kts ); + SG_LOG(SG_FLIGHT,SG_INFO,"v_calibrated_kts: " << _state.v_calibrated_kts ); + SG_LOG(SG_FLIGHT,SG_INFO,"alpha: " << _state.alpha ); + SG_LOG(SG_FLIGHT,SG_INFO,"beta: " << _state.beta ); + SG_LOG(SG_FLIGHT,SG_INFO,"gamma_vert_rad: " << _state.gamma_vert_rad ); + SG_LOG(SG_FLIGHT,SG_INFO,"density: " << _state.density ); + SG_LOG(SG_FLIGHT,SG_INFO,"mach_number: " << _state.mach_number ); + SG_LOG(SG_FLIGHT,SG_INFO,"static_pressure: " << _state.static_pressure ); + SG_LOG(SG_FLIGHT,SG_INFO,"total_pressure: " << _state.total_pressure ); + SG_LOG(SG_FLIGHT,SG_INFO,"dynamic_pressure: " << _state.dynamic_pressure ); + SG_LOG(SG_FLIGHT,SG_INFO,"static_temperature: " << _state.static_temperature ); + SG_LOG(SG_FLIGHT,SG_INFO,"total_temperature: " << _state.total_temperature ); + SG_LOG(SG_FLIGHT,SG_INFO,"sea_level_radius: " << _state.sea_level_radius ); + SG_LOG(SG_FLIGHT,SG_INFO,"earth_position_angle: " << _state.earth_position_angle ); + SG_LOG(SG_FLIGHT,SG_INFO,"runway_altitude: " << _state.runway_altitude ); + SG_LOG(SG_FLIGHT,SG_INFO,"climb_rate: " << _state.climb_rate ); + SG_LOG(SG_FLIGHT,SG_INFO,"altitude_agl: " << _state.altitude_agl ); } bool @@ -698,7 +733,7 @@ bool FGInterface::get_agl_m(double t, const double pt[3], double max_altoff, double contact[3], double normal[3], double linearVel[3], double angularVel[3], - SGMaterial const*& material, simgear::BVHNode::Id& id) + simgear::BVHMaterial const*& material, simgear::BVHNode::Id& id) { SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down(); SGVec3d _contact, _normal, _linearVel, _angularVel; @@ -721,7 +756,7 @@ bool FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff, double contact[3], double normal[3], double linearVel[3], double angularVel[3], - SGMaterial const*& material, simgear::BVHNode::Id& id) + simgear::BVHMaterial const*& material, simgear::BVHNode::Id& id) { // Convert units and do the real work. SGVec3d pt_m = SGVec3d(pt) - max_altoff*ground_cache.get_down(); @@ -747,10 +782,11 @@ bool FGInterface::get_nearest_m(double t, const double pt[3], double maxDist, double contact[3], double normal[3], double linearVel[3], double angularVel[3], - SGMaterial const*& material, + simgear::BVHMaterial const*& material, simgear::BVHNode::Id& id) { SGVec3d _contact, _linearVel, _angularVel; + material = 0; if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel, _angularVel, id, material)) return false; @@ -765,10 +801,11 @@ bool FGInterface::get_nearest_ft(double t, const double pt[3], double maxDist, double contact[3], double normal[3], double linearVel[3], double angularVel[3], - SGMaterial const*& material, + simgear::BVHMaterial const*& material, simgear::BVHNode::Id& id) { SGVec3d _contact, _linearVel, _angularVel; + material = 0; if (!ground_cache.get_nearest(t, SG_FEET_TO_METER*SGVec3d(pt), SG_FEET_TO_METER*maxDist, _contact, _linearVel, _angularVel, id, material)) @@ -830,7 +867,7 @@ FGInterface::get_groundlevel_m(const SGGeod& geod) } double contact[3], normal[3], vel[3], angvel[3]; - const SGMaterial* material; + const simgear::BVHMaterial* material; simgear::BVHNode::Id id; // Ignore the return value here, since it just tells us if // the returns stem from the groundcache or from the coarse @@ -893,6 +930,3 @@ FGInterface::release_wire(void) ground_cache.release_wire(); } -void fgToggleFDMdataLogging(void) { - cur_fdm_state->ToggleDataLogging(); -}