X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGState.cpp;h=999af7da55af9ef0a0e056f44949702fed14b616;hb=95440173caef3ce92ee7308fd58a24dabe4c5f8a;hp=f383f72b347f20e7a7fe9a0d1ede506f65832ab5;hpb=972bf2263094c9ff25ced601b0d6aac200916443;p=flightgear.git diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index f383f72b3..999af7da5 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -1,4 +1,4 @@ -/******************************************************************************* +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Module: FGState.cpp Author: Jon Berndt @@ -32,61 +32,41 @@ HISTORY -------------------------------------------------------------------------------- 11/17/98 JSB Created -******************************************************************************** +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES -*******************************************************************************/ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #ifdef FGFS # include -# ifdef FG_HAVE_STD_INCLUDES -# include -# else +# include +#else +# if defined(sgi) && !defined(__GNUC__) # include +# else +# include # endif -#else -# include #endif -#ifndef M_PI -# include -# define M_PI FG_PI +#ifdef _WIN32 +#define snprintf _snprintf #endif #include "FGState.h" -#include "FGFDMExec.h" -#include "FGAtmosphere.h" -#include "FGFCS.h" -#include "FGAircraft.h" -#include "FGTranslation.h" -#include "FGRotation.h" -#include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" - -static const char *IdSrc = "$Header$"; + +namespace JSBSim { + +static const char *IdSrc = "$Id$"; static const char *IdHdr = ID_STATE; -/******************************************************************************* +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MACROS -*******************************************************************************/ - -#define RegisterVariable(ID,DEF) coeffdef[#ID] = ID; paramdef[ID] = DEF +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -/******************************************************************************* -************************************ CODE ************************************** -*******************************************************************************/ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -/******************************************************************************/ -// -// For every term registered here there must be a corresponding handler in -// GetParameter() below that retrieves that parameter. Also, there must be an -// entry in the enum eParam definition in FGDefs.h. The ID is what must be used -// in any config file entry which references that item. - -FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), - mTl2b(3,3), - mTs2b(3,3), - vQtrn(4) +FGState::FGState(FGFDMExec* fdex) { FDMExec = fdex; @@ -94,200 +74,31 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), sim_time = 0.0; dt = 1.0/120.0; - RegisterVariable(FG_QBAR, " qbar " ); - RegisterVariable(FG_WINGAREA, " wing_area " ); - RegisterVariable(FG_WINGSPAN, " wingspan " ); - RegisterVariable(FG_CBAR, " cbar " ); - RegisterVariable(FG_ALPHA, " alpha " ); - RegisterVariable(FG_ALPHADOT, " alphadot " ); - RegisterVariable(FG_BETA, " beta " ); - RegisterVariable(FG_BETADOT, " betadot " ); - RegisterVariable(FG_PITCHRATE, " pitch_rate " ); - RegisterVariable(FG_ROLLRATE, " roll_rate " ); - RegisterVariable(FG_YAWRATE, " yaw_rate " ); - RegisterVariable(FG_MACH, " mach " ); - RegisterVariable(FG_ALTITUDE, " altitude " ); - RegisterVariable(FG_BI2VEL, " BI2Vel " ); - RegisterVariable(FG_CI2VEL, " CI2Vel " ); - RegisterVariable(FG_ELEVATOR_POS, " elevator_pos " ); - RegisterVariable(FG_AILERON_POS, " aileron_pos " ); - RegisterVariable(FG_RUDDER_POS, " rudder_pos " ); - RegisterVariable(FG_SPDBRAKE_POS, " speedbrake_pos " ); - RegisterVariable(FG_SPOILERS_POS, " spoiler_pos " ); - RegisterVariable(FG_FLAPS_POS, " flaps_pos " ); - RegisterVariable(FG_ELEVATOR_CMD, " elevator_cmd " ); - RegisterVariable(FG_AILERON_CMD, " aileron_cmd " ); - RegisterVariable(FG_RUDDER_CMD, " rudder_cmd " ); - RegisterVariable(FG_SPDBRAKE_CMD, " speedbrake_cmd " ); - RegisterVariable(FG_SPOILERS_CMD, " spoiler_cmd " ); - RegisterVariable(FG_FLAPS_CMD, " flaps_cmd " ); - RegisterVariable(FG_THROTTLE_CMD, " throttle_cmd " ); - RegisterVariable(FG_THROTTLE_POS, " throttle_pos " ); - RegisterVariable(FG_HOVERB, " height/span " ); - RegisterVariable(FG_PITCH_TRIM_CMD, " pitch_trim_cmd " ); -} - -/******************************************************************************/ - -FGState::~FGState(void) {} - -/******************************************************************************/ - -float FGState::GetParameter(eParam val_idx) { - switch(val_idx) { - case FG_QBAR: - return FDMExec->GetTranslation()->Getqbar(); - case FG_WINGAREA: - return FDMExec->GetAircraft()->GetWingArea(); - case FG_WINGSPAN: - return FDMExec->GetAircraft()->GetWingSpan(); - case FG_CBAR: - return FDMExec->GetAircraft()->Getcbar(); - case FG_ALPHA: - return FDMExec->GetTranslation()->Getalpha(); - case FG_ALPHADOT: - return FDMExec->GetTranslation()->Getadot(); - case FG_BETA: - return FDMExec->GetTranslation()->Getbeta(); - case FG_BETADOT: - return FDMExec->GetTranslation()->Getbdot(); - case FG_PITCHRATE: - return (FDMExec->GetRotation()->GetPQR())(2); - case FG_ROLLRATE: - return (FDMExec->GetRotation()->GetPQR())(1); - case FG_YAWRATE: - return (FDMExec->GetRotation()->GetPQR())(3); - case FG_ELEVATOR_POS: - return FDMExec->GetFCS()->GetDePos(); - case FG_AILERON_POS: - return FDMExec->GetFCS()->GetDaPos(); - case FG_RUDDER_POS: - return FDMExec->GetFCS()->GetDrPos(); - case FG_SPDBRAKE_POS: - return FDMExec->GetFCS()->GetDsbPos(); - case FG_SPOILERS_POS: - return FDMExec->GetFCS()->GetDspPos(); - case FG_FLAPS_POS: - return FDMExec->GetFCS()->GetDfPos(); - case FG_ELEVATOR_CMD: - return FDMExec->GetFCS()->GetDeCmd(); - case FG_AILERON_CMD: - return FDMExec->GetFCS()->GetDaCmd(); - case FG_RUDDER_CMD: - return FDMExec->GetFCS()->GetDrCmd(); - case FG_SPDBRAKE_CMD: - return FDMExec->GetFCS()->GetDsbCmd(); - case FG_SPOILERS_CMD: - return FDMExec->GetFCS()->GetDspCmd(); - case FG_FLAPS_CMD: - return FDMExec->GetFCS()->GetDfCmd(); - case FG_MACH: - return FDMExec->GetTranslation()->GetMach(); - case FG_ALTITUDE: - return FDMExec->GetPosition()->Geth(); - case FG_BI2VEL: - if(FDMExec->GetTranslation()->GetVt() > 0) - return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt()); - else - return 0; - case FG_CI2VEL: - if(FDMExec->GetTranslation()->GetVt() > 0) - return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt()); - else - return 0; - case FG_THROTTLE_CMD: - return FDMExec->GetFCS()->GetThrottleCmd(0); - case FG_THROTTLE_POS: - return FDMExec->GetFCS()->GetThrottlePos(0); - case FG_HOVERB: - return FDMExec->GetPosition()->GetHOverB(); - case FG_PITCH_TRIM_CMD: - return FDMExec->GetFCS()->GetPitchTrimCmd(); - default: - cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl; - return 0.0; - } - return 0; -} - -/******************************************************************************/ - -float FGState::GetParameter(string val_string) { - return GetParameter(coeffdef[val_string]); + Aircraft = FDMExec->GetAircraft(); + Translation = FDMExec->GetTranslation(); + Rotation = FDMExec->GetRotation(); + Position = FDMExec->GetPosition(); + FCS = FDMExec->GetFCS(); + Output = FDMExec->GetOutput(); + Atmosphere = FDMExec->GetAtmosphere(); + Aerodynamics = FDMExec->GetAerodynamics(); + GroundReactions = FDMExec->GetGroundReactions(); + Propulsion = FDMExec->GetPropulsion(); + PropertyManager = FDMExec->GetPropertyManager(); + + for(int i=0;i<4;i++) vQdot_prev[i].InitMatrix(); + + bind(); + + Debug(0); } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -eParam FGState::GetParameterIndex(string val_string) { - return coeffdef[val_string]; -} - -/******************************************************************************/ - -void FGState::SetParameter(eParam val_idx, float val) { - switch(val_idx) { - case FG_ELEVATOR_POS: - FDMExec->GetFCS()->SetDePos(val); - break; - case FG_AILERON_POS: - FDMExec->GetFCS()->SetDaPos(val); - break; - case FG_RUDDER_POS: - FDMExec->GetFCS()->SetDrPos(val); - break; - case FG_SPDBRAKE_POS: - FDMExec->GetFCS()->SetDsbPos(val); - break; - case FG_SPOILERS_POS: - FDMExec->GetFCS()->SetDspPos(val); - break; - case FG_FLAPS_POS: - FDMExec->GetFCS()->SetDfPos(val); - break; - case FG_THROTTLE_POS: - FDMExec->GetFCS()->SetThrottlePos(-1,val); - } -} - -//*************************************************************************** -// -// Reset: Assume all angles READ FROM FILE IN DEGREES !! -// - -bool FGState::Reset(string path, string acname, string fname) { - string resetDef; - float U, V, W; - float phi, tht, psi; - float latitude, longitude, h; - - resetDef = path + "/" + acname + "/" + fname; - - ifstream resetfile(resetDef.c_str()); - - if (resetfile) { - resetfile >> U; - resetfile >> V; - resetfile >> W; - resetfile >> latitude; - resetfile >> longitude; - resetfile >> phi; - resetfile >> tht; - resetfile >> psi; - resetfile >> h; - resetfile.close(); - - FDMExec->GetPosition()->SetLatitude(latitude*DEGTORAD); - FDMExec->GetPosition()->SetLongitude(longitude*DEGTORAD); - FDMExec->GetPosition()->Seth(h); - - Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD, - latitude*DEGTORAD, longitude*DEGTORAD, h); - - return true; - } else { - cerr << "Unable to load reset file " << fname << endl; - return false; - } +FGState::~FGState() +{ + unbind(); + Debug(1); } //*************************************************************************** @@ -295,60 +106,65 @@ bool FGState::Reset(string path, string acname, string fname) { // Initialize: Assume all angles GIVEN IN RADIANS !! // -void FGState::Initialize(float U, float V, float W, - float phi, float tht, float psi, - float Latitude, float Longitude, float H) { - FGColumnVector vUVW(3); - FGColumnVector vLocalVelNED(3); - FGColumnVector vEuler(3); - float alpha, beta; - float qbar, Vt; +void FGState::Initialize(double U, double V, double W, + double phi, double tht, double psi, + double Latitude, double Longitude, double H, + double wnorth, double weast, double wdown) +{ + double alpha, beta; + double qbar, Vt; + FGColumnVector3 vAeroUVW; - FDMExec->GetPosition()->SetLatitude(Latitude); - FDMExec->GetPosition()->SetLongitude(Longitude); - FDMExec->GetPosition()->Seth(H); + Position->SetLatitude(Latitude); + Position->SetLongitude(Longitude); + Position->Seth(H); - FDMExec->GetAtmosphere()->Run(); + Atmosphere->Run(); + + vLocalEuler << phi << tht << psi; + Rotation->SetEuler(vLocalEuler); - if (W != 0.0) - alpha = U*U > 0.0 ? atan2(W, U) : 0.0; + InitMatrices(phi, tht, psi); + + vUVW << U << V << W; + Translation->SetUVW(vUVW); + + Atmosphere->SetWindNED(wnorth, weast, wdown); + + vAeroUVW = vUVW + mTl2b*Atmosphere->GetWindNED(); + + if (vAeroUVW(eW) != 0.0) + alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0; else alpha = 0.0; - if (V != 0.0) - beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0; + if (vAeroUVW(eV) != 0.0) + beta = vAeroUVW(eU)*vAeroUVW(eU)+vAeroUVW(eW)*vAeroUVW(eW) > 0.0 ? atan2(vAeroUVW(eV), (fabs(vAeroUVW(eU))/vAeroUVW(eU))*sqrt(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eW)*vAeroUVW(eW))) : 0.0; else beta = 0.0; - vUVW << U << V << W; - FDMExec->GetTranslation()->SetUVW(vUVW); - - vEuler << phi << tht << psi; - FDMExec->GetRotation()->SetEuler(vEuler); - - FDMExec->GetTranslation()->SetAB(alpha, beta); + Translation->SetAB(alpha, beta); Vt = sqrt(U*U + V*V + W*W); - FDMExec->GetTranslation()->SetVt(Vt); + Translation->SetVt(Vt); - FDMExec->GetTranslation()->SetMach(Vt/FDMExec->GetAtmosphere()->GetSoundSpeed()); + Translation->SetMach(Vt/Atmosphere->GetSoundSpeed()); - qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->GetDensity(); - FDMExec->GetTranslation()->Setqbar(qbar); - - InitMatrices(phi, tht, psi); + qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity(); + Translation->Setqbar(qbar); vLocalVelNED = mTb2l*vUVW; - FDMExec->GetPosition()->SetvVel(vLocalVelNED); + Position->SetvVel(vLocalVelNED); } -/******************************************************************************/ - -void FGState::Initialize(FGInitialCondition *FGIC) { - - float tht,psi,phi; - float U, V, W, h; - float latitude, longitude; +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +void FGState::Initialize(FGInitialCondition *FGIC) +{ + double tht,psi,phi; + double U, V, W, h; + double latitude, longitude; + double wnorth,weast, wdown; + latitude = FGIC->GetLatitudeRadIC(); longitude = FGIC->GetLongitudeRadIC(); h = FGIC->GetAltitudeFtIC(); @@ -358,43 +174,29 @@ void FGState::Initialize(FGInitialCondition *FGIC) { tht = FGIC->GetThetaRadIC(); phi = FGIC->GetPhiRadIC(); psi = FGIC->GetPsiRadIC(); - - Initialize(U, V, W, phi, tht, psi, latitude, longitude, h); + wnorth = FGIC->GetWindNFpsIC(); + weast = FGIC->GetWindEFpsIC(); + wdown = FGIC->GetWindDFpsIC(); + + Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() ); + Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + + FGIC->GetTerrainAltitudeFtIC() ); + + // need to fix the wind speed args, here. + Initialize(U, V, W, phi, tht, psi, latitude, longitude, h, wnorth, weast, wdown); } -/******************************************************************************/ - -bool FGState::StoreData(string fname) { - ofstream datafile(fname.c_str()); - - if (datafile) { - datafile << (FDMExec->GetTranslation()->GetUVW())(1); - datafile << (FDMExec->GetTranslation()->GetUVW())(2); - datafile << (FDMExec->GetTranslation()->GetUVW())(3); - datafile << FDMExec->GetPosition()->GetLatitude(); - datafile << FDMExec->GetPosition()->GetLongitude(); - datafile << (FDMExec->GetRotation()->GetEuler())(1); - datafile << (FDMExec->GetRotation()->GetEuler())(2); - datafile << (FDMExec->GetRotation()->GetEuler())(3); - datafile << FDMExec->GetPosition()->Geth(); - datafile.close(); - return true; - } else { - cerr << "Could not open dump file " << fname << endl; - return false; - } -} +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -/******************************************************************************/ - -void FGState::InitMatrices(float phi, float tht, float psi) { - float thtd2, psid2, phid2; - float Sthtd2, Spsid2, Sphid2; - float Cthtd2, Cpsid2, Cphid2; - float Cphid2Cthtd2; - float Cphid2Sthtd2; - float Sphid2Sthtd2; - float Sphid2Cthtd2; +void FGState::InitMatrices(double phi, double tht, double psi) +{ + double thtd2, psid2, phid2; + double Sthtd2, Spsid2, Sphid2; + double Cthtd2, Cpsid2, Cphid2; + double Cphid2Cthtd2; + double Cphid2Sthtd2; + double Sphid2Sthtd2; + double Sphid2Cthtd2; thtd2 = tht/2.0; psid2 = psi/2.0; @@ -421,12 +223,13 @@ void FGState::InitMatrices(float phi, float tht, float psi) { CalcMatrices(); } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGState::CalcMatrices(void) { - float Q0Q0, Q1Q1, Q2Q2, Q3Q3; - float Q0Q1, Q0Q2, Q0Q3, Q1Q2; - float Q1Q3, Q2Q3; +void FGState::CalcMatrices(void) +{ + double Q0Q0, Q1Q1, Q2Q2, Q3Q3; + double Q0Q1, Q0Q2, Q0Q3, Q1Q2; + double Q1Q3, Q2Q3; Q0Q0 = vQtrn(1)*vQtrn(1); Q1Q1 = vQtrn(2)*vQtrn(2); @@ -453,29 +256,24 @@ void FGState::CalcMatrices(void) { mTb2l.T(); } -/******************************************************************************/ - -void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { - static FGColumnVector vlastQdot(4); - static FGColumnVector vQdot(4); +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +void FGState::IntegrateQuat(FGColumnVector3 vPQR, int rate) +{ vQdot(1) = -0.5*(vQtrn(2)*vPQR(eP) + vQtrn(3)*vPQR(eQ) + vQtrn(4)*vPQR(eR)); vQdot(2) = 0.5*(vQtrn(1)*vPQR(eP) + vQtrn(3)*vPQR(eR) - vQtrn(4)*vPQR(eQ)); vQdot(3) = 0.5*(vQtrn(1)*vPQR(eQ) + vQtrn(4)*vPQR(eP) - vQtrn(2)*vPQR(eR)); vQdot(4) = 0.5*(vQtrn(1)*vPQR(eR) + vQtrn(2)*vPQR(eQ) - vQtrn(3)*vPQR(eP)); - vQtrn += 0.5*dt*rate*(vlastQdot + vQdot); + vQtrn += Integrate(TRAPZ, dt*rate, vQdot, vQdot_prev); vQtrn.Normalize(); - - vlastQdot = vQdot; } -/******************************************************************************/ - -FGColumnVector FGState::CalcEuler(void) { - static FGColumnVector vEuler(3); +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FGColumnVector3& FGState::CalcEuler(void) +{ if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001; if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001; @@ -488,28 +286,197 @@ FGColumnVector FGState::CalcEuler(void) { return vEuler; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGMatrix FGState::GetTs2b(float alpha, float beta) { - float ca, cb, sa, sb; +FGMatrix33& FGState::GetTs2b(void) +{ + double ca, cb, sa, sb; + + double alpha = Translation->Getalpha(); + double beta = Translation->Getbeta(); ca = cos(alpha); sa = sin(alpha); cb = cos(beta); sb = sin(beta); - mTs2b(1,1) = -ca*cb; + mTs2b(1,1) = ca*cb; mTs2b(1,2) = -ca*sb; - mTs2b(1,3) = sa; - mTs2b(2,1) = -sb; + mTs2b(1,3) = -sa; + mTs2b(2,1) = sb; mTs2b(2,2) = cb; mTs2b(2,3) = 0.0; - mTs2b(3,1) = -sa*cb; + mTs2b(3,1) = sa*cb; mTs2b(3,2) = -sa*sb; - mTs2b(3,3) = -ca; + mTs2b(3,3) = ca; return mTs2b; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33& FGState::GetTb2s(void) +{ + float alpha,beta; + float ca, cb, sa, sb; + + alpha = Translation->Getalpha(); + beta = Translation->Getbeta(); + + ca = cos(alpha); + sa = sin(alpha); + cb = cos(beta); + sb = sin(beta); + + mTb2s(1,1) = ca*cb; + mTb2s(1,2) = sb; + mTb2s(1,3) = sa*cb; + mTb2s(2,1) = -ca*sb; + mTb2s(2,2) = cb; + mTb2s(2,3) = -sa*sb; + mTb2s(3,1) = -sa; + mTb2s(3,2) = 0.0; + mTb2s(3,3) = ca; + + return mTb2s; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGState::ReportState(void) +{ +#if !defined(__BORLANDCPP__) + char out[80], flap[10], gear[12]; + + cout << endl << " JSBSim State" << endl; + snprintf(out,80," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n", + FDMExec->GetMassBalance()->GetWeight(), + FDMExec->GetMassBalance()->GetXYZcg(1), + FDMExec->GetMassBalance()->GetXYZcg(2), + FDMExec->GetMassBalance()->GetXYZcg(3)); + cout << out; + if ( FCS->GetDfPos() <= 0.01) + snprintf(flap,10,"Up"); + else + snprintf(flap,10,"%2.0f",FCS->GetDfPos()); + if (FCS->GetGearPos() < 0.01) + snprintf(gear,12,"Up"); + else if (FCS->GetGearPos() > 0.99) + snprintf(gear,12,"Down"); + else + snprintf(gear,12,"In Transit"); + + snprintf(out,80, " Flaps: %3s Gear: %12s\n",flap,gear); + cout << out; + snprintf(out,80, " Speed: %4.0f KCAS Mach: %5.2f\n", + FDMExec->GetAuxiliary()->GetVcalibratedKTS(), + Translation->GetMach() ); + cout << out; + snprintf(out,80, " Altitude: %7.0f ft. AGL Altitude: %7.0f ft.\n", + Position->Geth(), + Position->GetDistanceAGL() ); + cout << out; + snprintf(out,80, " Angle of Attack: %6.2f deg Pitch Angle: %6.2f deg\n", + Translation->Getalpha()*radtodeg, + Rotation->Gettht()*radtodeg ); + cout << out; + snprintf(out,80, " Flight Path Angle: %6.2f deg Climb Rate: %5.0f ft/min\n", + Position->GetGamma()*radtodeg, + Position->Gethdot()*60 ); + cout << out; + snprintf(out,80, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n", + Aircraft->GetNlf(), + Rotation->GetPQR(2)*radtodeg ); + cout << out; + snprintf(out,80, " Heading: %3.0f deg true Sideslip: %5.2f deg Yaw Rate: %5.2f deg/s\n", + Rotation->Getpsi()*radtodeg, + Translation->Getbeta()*radtodeg, + Rotation->GetPQR(3)*radtodeg ); + cout << out; + snprintf(out,80, " Bank Angle: %5.2f deg Roll Rate: %5.2f deg/s\n", + Rotation->Getphi()*radtodeg, + Rotation->GetPQR(1)*radtodeg ); + cout << out; + snprintf(out,80, " Elevator: %5.2f deg Left Aileron: %5.2f deg Rudder: %5.2f deg\n", + FCS->GetDePos(ofRad)*radtodeg, + FCS->GetDaLPos(ofRad)*radtodeg, + FCS->GetDrPos(ofRad)*radtodeg ); + cout << out; + snprintf(out,80, " Throttle: %5.2f%c\n", + FCS->GetThrottlePos(0)*100,'%' ); + cout << out; + + snprintf(out,80, " Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n", + FDMExec->GetAuxiliary()->GetHeadWind()*fpstokts, + FDMExec->GetAuxiliary()->GetCrossWind()*fpstokts ); + cout << out; + + snprintf(out,80, " Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n", + Position->GetVground()*fpstokts, + Position->GetGroundTrack()*radtodeg ); + cout << out; +#endif +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGState::bind(void) +{ + PropertyManager->Tie("sim-time-sec",this, + &FGState::Getsim_time); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGState::unbind(void) +{ + PropertyManager->Untie("sim-time-sec"); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// The bitmasked value choices are as follows: +// unset: In this case (the default) JSBSim would only print +// out the normally expected messages, essentially echoing +// the config files as they are read. If the environment +// variable is not set, debug_lvl is set to 1 internally +// 0: This requests JSBSim not to output any messages +// whatsoever. +// 1: This value explicity requests the normal JSBSim +// startup messages +// 2: This value asks for a message to be printed out when +// a class is instantiated +// 4: When this value is set, a message is displayed when a +// FGModel object executes its Run() method +// 8: When this value is set, various runtime state variables +// are printed out periodically +// 16: When set various parameters are sanity checked and +// a message is printed out when they go out of bounds + +void FGState::Debug(int from) +{ + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGState" << endl; + if (from == 1) cout << "Destroyed: FGState" << endl; + } + if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects + } + if (debug_lvl & 8 ) { // Runtime state variables + } + if (debug_lvl & 16) { // Sanity checking + } + if (debug_lvl & 64) { + if (from == 0) { // Constructor + cout << IdSrc << endl; + cout << IdHdr << endl; + } + } +} +}