From 0d0b5c616c26012100aa6e6493447d086645b918 Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Sun, 29 May 2011 00:05:33 +0200 Subject: [PATCH] Bertrand Coconnier: bug fix for #184, JSBSim: command line arguments 1. The atmospheric properties of FG are not yet initialized when JSBSim is initialized. -> patch is quite basic and there may exist smarter ways to initialize properly the environment before the FDM. 2. The Euler angles were initialized after the velocities. 3. The glide slope and rate of climb were ignored. Fixes all FDMs (YASim, UIUC, JSBSim, etc.) 4. Some properties were instructed to re-use their previous value while they should not. 5. Some bugs existed in JSBSim trim code. -> This bug has already been fixed in JSBSim but the corresponding patch has not yet been applied to FG. --- src/Environment/environment_mgr.cxx | 7 ++ src/FDM/JSBSim/JSBSim.cxx | 30 ++++---- .../initialization/FGInitialCondition.cpp | 35 +++++---- .../initialization/FGInitialCondition.h | 5 +- src/FDM/flight.cxx | 76 +++++++++++-------- 5 files changed, 89 insertions(+), 64 deletions(-) diff --git a/src/Environment/environment_mgr.cxx b/src/Environment/environment_mgr.cxx index e48039029..42023ea84 100644 --- a/src/Environment/environment_mgr.cxx +++ b/src/Environment/environment_mgr.cxx @@ -97,6 +97,13 @@ FGEnvironmentMgr::init () SG_LOG( SG_GENERAL, SG_INFO, "Initializing environment subsystem"); SGSubsystemGroup::init(); fgClouds->Init(); + + // Initialize the longitude, latitude and altitude to the initial position + // of the aircraft so that the atmospheric properties (pressure, temperature + // and density) can be initialized accordingly. + _altitudeNode->setDoubleValue(fgGetDouble("/sim/presets/altitude-ft")); + _longitude_n->setDoubleValue(fgGetDouble("/sim/presets/longitude-deg")); + _latitude_n->setDoubleValue(fgGetDouble("/sim/presets/latitude-deg")); } void diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index 806401cf9..54ed59872 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/src/FDM/JSBSim/JSBSim.cxx @@ -181,7 +181,7 @@ FGJSBsim::FGJSBsim( double dt ) MassBalance = fdmex->GetMassBalance(); Propulsion = fdmex->GetPropulsion(); Aircraft = fdmex->GetAircraft(); - Propagate = fdmex->GetPropagate(); + Propagate = fdmex->GetPropagate(); Auxiliary = fdmex->GetAuxiliary(); Inertial = fdmex->GetInertial(); Aerodynamics = fdmex->GetAerodynamics(); @@ -369,9 +369,9 @@ void FGJSBsim::init() Atmosphere->UseInternal(); } - fgic->SetVNorthFpsIC( -wind_from_north->getDoubleValue() ); - fgic->SetVEastFpsIC( -wind_from_east->getDoubleValue() ); - fgic->SetVDownFpsIC( -wind_from_down->getDoubleValue() ); + fgic->SetWindNEDFpsIC( -wind_from_north->getDoubleValue(), + -wind_from_east->getDoubleValue(), + -wind_from_down->getDoubleValue() ); //Atmosphere->SetExTemperature(get_Static_temperature()); //Atmosphere->SetExPressure(get_Static_pressure()); @@ -393,20 +393,20 @@ void FGJSBsim::init() } // end of egt_degf deprecation patch - if (fgGetBool("/sim/presets/running")) { - for (unsigned int i=0; i < Propulsion->GetNumEngines(); i++) { - SGPropertyNode * node = fgGetNode("engines/engine", i, true); - node->setBoolValue("running", true); - Propulsion->GetEngine(i)->SetRunning(true); - } - } - FCS->SetDfPos( ofNorm, globals->get_controls()->get_flaps() ); common_init(); copy_to_JSBsim(); fdmex->RunIC(); //loop JSBSim once w/o integrating + if (fgGetBool("/sim/presets/running")) { + Propulsion->InitRunning(-1); + for (unsigned int i = 0; i < Propulsion->GetNumEngines(); i++) { + FGPiston* eng = (FGPiston*)Propulsion->GetEngine(i); + globals->get_controls()->set_magnetos(i, eng->GetMagnetos()); + globals->get_controls()->set_mixture(i, FCS->GetMixtureCmd(i)); + } + } copy_from_JSBsim(); //update the bus SG_LOG( SG_FLIGHT, SG_INFO, " Initialized JSBSim with:" ); @@ -1282,7 +1282,7 @@ void FGJSBsim::do_trim(void) { fgtrim = new FGTrim(fdmex,tGround); } else { - fgtrim = new FGTrim(fdmex,tLongitudinal); + fgtrim = new FGTrim(fdmex,tFull); } if ( !fgtrim->DoTrim() ) { @@ -1296,7 +1296,7 @@ void FGJSBsim::do_trim(void) pitch_trim->setDoubleValue( FCS->GetPitchTrimCmd() ); throttle_trim->setDoubleValue( FCS->GetThrottleCmd(0) ); aileron_trim->setDoubleValue( FCS->GetDaCmd() ); - rudder_trim->setDoubleValue( FCS->GetDrCmd() ); + rudder_trim->setDoubleValue( -FCS->GetDrCmd() ); globals->get_controls()->set_elevator_trim(FCS->GetPitchTrimCmd()); globals->get_controls()->set_elevator(FCS->GetDeCmd()); @@ -1304,7 +1304,7 @@ void FGJSBsim::do_trim(void) globals->get_controls()->set_throttle(i, FCS->GetThrottleCmd(i)); globals->get_controls()->set_aileron(FCS->GetDaCmd()); - globals->get_controls()->set_rudder( FCS->GetDrCmd()); + globals->get_controls()->set_rudder( -FCS->GetDrCmd()); SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" ); } diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp index 0dbcf88ce..00a3840f1 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp @@ -61,7 +61,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.59 2011/04/03 13:18:51 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.61 2011/05/20 00:47:03 bcoconni Exp $"; static const char *IdHdr = ID_INITIALCONDITION; //****************************************************************************** @@ -112,7 +112,7 @@ void FGInitialCondition::ResetIC(double u0, double v0, double w0, FGQuaternion Quat(phi, theta, psi); Quat.Normalize(); Tl2b = Quat.GetT(); - Tb2l = Quat.GetTInv(); + Tb2l = Tl2b.Transposed(); vUVW_NED = Tb2l * FGColumnVector3(u0, v0, w0); vt = vUVW_NED.Magnitude(); @@ -322,20 +322,18 @@ void FGInitialCondition::SetClimbRateFpsIC(double hdot) FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); FGColumnVector3 _WIND_NED = _vt_NED - vUVW_NED; - double hdot0 = _vt_NED(eW); + double hdot0 = -_vt_NED(eW); if (fabs(hdot0) < vt) { double scale = sqrt((vt*vt-hdot*hdot)/(vt*vt-hdot0*hdot0)); _vt_NED(eU) *= scale; _vt_NED(eV) *= scale; } - _vt_NED(eW) = hdot; + _vt_NED(eW) = -hdot; vUVW_NED = _vt_NED - _WIND_NED; - // The AoA is not modified here but the function SetAlphaRadIC is updating the - // same angles than SetClimbRateFpsIC needs to update. - // TODO : create a subroutine that only shares the relevant code. - SetAlphaRadIC(alpha); + // Updating the angles theta and beta to keep the true airspeed amplitude + calcThetaBeta(alpha, _vt_NED); } //****************************************************************************** @@ -346,13 +344,22 @@ void FGInitialCondition::SetClimbRateFpsIC(double hdot) void FGInitialCondition::SetAlphaRadIC(double alfa) { FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + calcThetaBeta(alfa, _vt_NED); +} + +//****************************************************************************** +// When the AoA is modified, we need to update the angles theta and beta to +// keep the true airspeed amplitude, the climb rate and the heading unchanged. +// Beta will be modified if the aircraft roll angle is not null. +void FGInitialCondition::calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED) +{ double calpha = cos(alfa), salpha = sin(alfa); double cpsi = cos(psi), spsi = sin(psi); double cphi = cos(phi), sphi = sin(phi); FGMatrix33 Tpsi( cpsi, spsi, 0., - -spsi, cpsi, 0., - 0., 0., 1.); + -spsi, cpsi, 0., + 0., 0., 1.); FGMatrix33 Tphi(1., 0., 0., 0., cphi, sphi, 0.,-sphi, cphi); @@ -398,11 +405,11 @@ void FGInitialCondition::SetAlphaRadIC(double alfa) Tl2b = Quat.GetT(); Tb2l = Quat.GetTInv(); - FGColumnVector3 v2 = Talpha * Quat.GetT() * _vt_NED; + FGColumnVector3 v2 = Talpha * Tl2b * _vt_NED; alpha = alfa; beta = atan2(v2(eV), v2(eU)); - double cbeta=0.0, sbeta=0.0; + double cbeta=1.0, sbeta=0.0; if (vt != 0.0) { cbeta = v2(eU) / vt; sbeta = v2(eV) / vt; @@ -687,6 +694,8 @@ void FGInitialCondition::SetAltitudeASLFtIC(double alt) double ve0 = vt * sqrt(rho/rhoSL); altitudeASL=alt; + position.SetRadius(alt + sea_level_radius); + temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); soundSpeed = sqrt(SHRatio*Reng*temperature); rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL); @@ -703,8 +712,6 @@ void FGInitialCondition::SetAltitudeASLFtIC(double alt) default: // Make the compiler stop complaining about missing enums break; } - - position.SetRadius(alt + sea_level_radius); } //****************************************************************************** diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.h b/src/FDM/JSBSim/initialization/FGInitialCondition.h index 308122450..e110ebbb3 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.h +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.h @@ -54,7 +54,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.26 2011/01/16 16:10:59 bcoconni Exp $" +#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.27 2011/05/20 00:47:03 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -213,7 +213,7 @@ CLASS DOCUMENTATION @property ic/r-rad_sec (read/write) Yaw rate initial condition in radians/second @author Tony Peden - @version "$Id: FGInitialCondition.h,v 1.26 2011/01/16 16:10:59 bcoconni Exp $" + @version "$Id: FGInitialCondition.h,v 1.27 2011/05/20 00:47:03 bcoconni Exp $" */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -666,6 +666,7 @@ private: double getMachFromVcas(double vcas); double calcVcas(double Mach) const; void calcAeroAngles(const FGColumnVector3& _vt_BODY); + void calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED); void bind(void); void Debug(int from); diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index d501d1507..431605a9f 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -179,6 +179,15 @@ FGInterface::common_init () double slr = SGGeodesy::SGGeodToSeaLevelRadius(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") ) { @@ -207,14 +216,11 @@ 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( "/velocities/vertical-speed-fps") ) + set_Climb_Rate( fgGetDouble("/velocities/vertical-speed-fps") ); SG_LOG( SG_FLIGHT, SG_INFO, "End common FDM init" ); } @@ -251,7 +257,7 @@ FGInterface::bind () false); fgSetArchivable("/position/altitude-ft"); fgTie("/position/altitude-agl-ft", this, - &FGInterface::get_Altitude_AGL, &FGInterface::set_AltitudeAGL); + &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 @@ -263,7 +269,7 @@ FGInterface::bind () fgSetArchivable("/position/sea-level-radius-ft"); fgTie("/position/sea-level-radius-ft", this, &FGInterface::get_Sea_level_radius, - &FGInterface::_set_Sea_level_radius); + &FGInterface::_set_Sea_level_radius, false); // Orientation fgTie("/orientation/roll-deg", this, @@ -279,24 +285,27 @@ FGInterface::bind () &FGInterface::set_Psi_deg, false); fgSetArchivable("/orientation/heading-deg"); fgTie("/orientation/track-deg", this, - &FGInterface::get_Track); + &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, &FGInterface::set_Phi_dot_degps); + &FGInterface::get_Phi_dot_degps, + &FGInterface::set_Phi_dot_degps, false); fgTie("/orientation/pitch-rate-degps", this, - &FGInterface::get_Theta_dot_degps, &FGInterface::set_Theta_dot_degps); + &FGInterface::get_Theta_dot_degps, + &FGInterface::set_Theta_dot_degps, false); fgTie("/orientation/yaw-rate-degps", this, - &FGInterface::get_Psi_dot_degps, &FGInterface::set_Psi_dot_degps); + &FGInterface::get_Psi_dot_degps, + &FGInterface::set_Psi_dot_degps, false); - fgTie("/orientation/p-body", this, &FGInterface::get_P_body); - fgTie("/orientation/q-body", this, &FGInterface::get_Q_body); - fgTie("/orientation/r-body", this, &FGInterface::get_R_body); + fgTie("/orientation/p-body", this, &FGInterface::get_P_body); // read-only + fgTie("/orientation/q-body", this, &FGInterface::get_Q_body); // read-only + fgTie("/orientation/r-body", this, &FGInterface::get_R_body); // read-only // Ground speed knots fgTie("/velocities/groundspeed-kt", this, - &FGInterface::get_V_ground_speed_kt); + &FGInterface::get_V_ground_speed_kt); // read-only // Calibrated airspeed fgTie("/velocities/airspeed-kt", this, @@ -305,7 +314,7 @@ FGInterface::bind () false); fgTie("/velocities/equivalent-kt", this, - &FGInterface::get_V_equiv_kts); + &FGInterface::get_V_equiv_kts); // read-only // Mach number fgTie("/velocities/mach", this, @@ -338,11 +347,11 @@ FGInterface::bind () &FGInterface::get_V_down, &FGInterface::set_V_down, false); fgTie("/velocities/north-relground-fps", this, - &FGInterface::get_V_north_rel_ground); + &FGInterface::get_V_north_rel_ground); // read-only fgTie("/velocities/east-relground-fps", this, - &FGInterface::get_V_east_rel_ground); + &FGInterface::get_V_east_rel_ground); // read-only fgTie("/velocities/down-relground-fps", this, - &FGInterface::get_V_down_rel_ground); + &FGInterface::get_V_down_rel_ground); // read-only // Relative wind @@ -367,36 +376,37 @@ FGInterface::bind () // Climb and slip (read-only) fgTie("/velocities/vertical-speed-fps", this, &FGInterface::get_Climb_Rate, - &FGInterface::set_Climb_Rate ); + &FGInterface::set_Climb_Rate, false ); fgTie("/velocities/glideslope", this, &FGInterface::get_Gamma_vert_rad, - &FGInterface::set_Gamma_vert_rad ); + &FGInterface::set_Gamma_vert_rad, false ); fgTie("/orientation/side-slip-rad", this, - &FGInterface::get_Beta, &FGInterface::_set_Beta); + &FGInterface::get_Beta, &FGInterface::_set_Beta, false); fgTie("/orientation/side-slip-deg", this, &FGInterface::get_Beta_deg); // read-only fgTie("/orientation/alpha-deg", this, - &FGInterface::get_Alpha_deg, &FGInterface::set_Alpha_deg); // read-only + &FGInterface::get_Alpha_deg, &FGInterface::set_Alpha_deg, false); fgTie("/accelerations/nlf", this, &FGInterface::get_Nlf); // read-only // NED accelerations fgTie("/accelerations/ned/north-accel-fps_sec", - this, &FGInterface::get_V_dot_north); + this, &FGInterface::get_V_dot_north); // read-only fgTie("/accelerations/ned/east-accel-fps_sec", - this, &FGInterface::get_V_dot_east); + this, &FGInterface::get_V_dot_east); // read-only fgTie("/accelerations/ned/down-accel-fps_sec", - this, &FGInterface::get_V_dot_down); + this, &FGInterface::get_V_dot_down); // read-only // Pilot accelerations fgTie("/accelerations/pilot/x-accel-fps_sec", - this, &FGInterface::get_A_X_pilot, &FGInterface::set_A_X_pilot); + this, &FGInterface::get_A_X_pilot, &FGInterface::set_A_X_pilot, false); fgTie("/accelerations/pilot/y-accel-fps_sec", - this, &FGInterface::get_A_Y_pilot, &FGInterface::set_A_Y_pilot); + this, &FGInterface::get_A_Y_pilot, &FGInterface::set_A_Y_pilot, false); fgTie("/accelerations/pilot/z-accel-fps_sec", - this, &FGInterface::get_A_Z_pilot, &FGInterface::set_A_Z_pilot); + this, &FGInterface::get_A_Z_pilot, &FGInterface::set_A_Z_pilot, false); - fgTie("/accelerations/n-z-cg-fps_sec", this, &FGInterface::get_N_Z_cg); + fgTie("/accelerations/n-z-cg-fps_sec", + this, &FGInterface::get_N_Z_cg); // read-only } -- 2.39.5