X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGState.cpp;h=999af7da55af9ef0a0e056f44949702fed14b616;hb=95440173caef3ce92ee7308fd58a24dabe4c5f8a;hp=59773edb45fdaf6daacf12f6cc0e45f213ca5db3;hpb=c722481805eeb1c55085eaf4487f5ad909c7c4b8;p=flightgear.git diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 59773edb4..999af7da5 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -1,123 +1,104 @@ -/******************************************************************************* +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Module: FGState.cpp Author: Jon Berndt Date started: 11/17/98 Called by: FGFDMExec and accessed by all models. - + ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) ------------- - + This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. - + This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. - + Further information about the GNU General Public License can also be found on the world wide web at http://www.gnu.org. - + FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- See header file. - + HISTORY -------------------------------------------------------------------------------- 11/17/98 JSB Created - -******************************************************************************** + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES -*******************************************************************************/ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #ifdef FGFS -# include -# ifdef FG_HAVE_STD_INCLUDES -# include -# else +# include +# include +#else +# if defined(sgi) && !defined(__GNUC__) # include +# else +# include # endif -#else -# include +#endif + +#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" -/******************************************************************************* -************************************ CODE ************************************** -*******************************************************************************/ +namespace JSBSim { + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_STATE; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +MACROS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ FGState::FGState(FGFDMExec* fdex) { FDMExec = fdex; - Vt = 0.0; - latitude = longitude = 0.0; - adot = bdot = 0.0; - h = 0.0; a = 1000.0; - qbar = 0.0; sim_time = 0.0; dt = 1.0/120.0; -} - -FGState::~FGState(void) -{ + 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); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -//*************************************************************************** -// -// Reset: Assume all angles READ FROM FILE IN DEGREES !! -// - -bool FGState::Reset(string path, string fname) +FGState::~FGState() { - string resetDef; - float U, V, W; - float phi, tht, psi; - - resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + 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(); - - 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; - } + unbind(); + Debug(1); } //*************************************************************************** @@ -125,65 +106,65 @@ bool FGState::Reset(string path, 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) +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) { - float alpha, beta, gamma; - float Q0, Q1, Q2, Q3; - float T[4][4]; - - latitude = Latitude; - longitude = Longitude; - h = H; - - gamma = 0.0; - if (W != 0.0) - alpha = U*U > 0.0 ? atan2(W, U) : 0.0; + double alpha, beta; + double qbar, Vt; + FGColumnVector3 vAeroUVW; + + Position->SetLatitude(Latitude); + Position->SetLongitude(Longitude); + Position->Seth(H); + + Atmosphere->Run(); + + vLocalEuler << phi << tht << psi; + Rotation->SetEuler(vLocalEuler); + + 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; - FDMExec->GetTranslation()->SetUVW(U, V, W); - FDMExec->GetRotation()->SetEuler(phi, tht, psi); - FDMExec->GetTranslation()->SetABG(alpha, beta, gamma); + Translation->SetAB(alpha, beta); Vt = sqrt(U*U + V*V + W*W); - qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->CalcRho(h); - - Q0 = sin(psi*0.5)*sin(tht*0.5)*sin(phi*0.5) + cos(psi*0.5)*cos(tht*0.5)*cos(phi*0.5); - Q1 = -sin(psi*0.5)*sin(tht*0.5)*cos(phi*0.5) + cos(psi*0.5)*cos(tht*0.5)*sin(phi*0.5); - Q2 = sin(psi*0.5)*cos(tht*0.5)*sin(phi*0.5) + cos(psi*0.5)*sin(tht*0.5)*cos(phi*0.5); - Q3 = sin(psi*0.5)*cos(tht*0.5)*cos(phi*0.5) - cos(psi*0.5)*sin(tht*0.5)*sin(phi*0.5); - - FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3); - - T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3; - T[1][2] = 2*(Q1*Q2 + Q0*Q3); - T[1][3] = 2*(Q1*Q3 - Q0*Q2); - T[2][1] = 2*(Q1*Q2 - Q0*Q3); - T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3; - T[2][3] = 2*(Q2*Q3 + Q0*Q1); - T[3][1] = 2*(Q1*Q3 + Q0*Q2); - T[3][2] = 2*(Q2*Q3 - Q0*Q1); - T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3; - - FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3], - T[2][1], T[2][2], T[2][3], - T[3][1], T[3][2], T[3][3]); - DisplayData(); + Translation->SetVt(Vt); + + Translation->SetMach(Vt/Atmosphere->GetSoundSpeed()); + + qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity(); + Translation->Setqbar(qbar); + + vLocalVelNED = mTb2l*vUVW; + Position->SetvVel(vLocalVelNED); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGState::Initialize(FGInitialCondition *FGIC) { - - float tht,psi,phi; - float U,V,W; - + 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(); @@ -193,104 +174,309 @@ void FGState::Initialize(FGInitialCondition *FGIC) tht = FGIC->GetThetaRadIC(); phi = FGIC->GetPhiRadIC(); psi = FGIC->GetPsiRadIC(); + 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); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - Initialize(U, V, W, phi, tht, psi,latitude, longitude, h); +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; + phid2 = phi/2.0; + + Sthtd2 = sin(thtd2); + Spsid2 = sin(psid2); + Sphid2 = sin(phid2); + + Cthtd2 = cos(thtd2); + Cpsid2 = cos(psid2); + Cphid2 = cos(phid2); + + Cphid2Cthtd2 = Cphid2*Cthtd2; + Cphid2Sthtd2 = Cphid2*Sthtd2; + Sphid2Sthtd2 = Sphid2*Sthtd2; + Sphid2Cthtd2 = Sphid2*Cthtd2; + + vQtrn(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2; + vQtrn(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2; + vQtrn(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2; + vQtrn(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2; + + CalcMatrices(); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGState::StoreData(string fname) +void FGState::CalcMatrices(void) { - ofstream datafile(fname.c_str()); - - if (datafile) { - datafile << FDMExec->GetTranslation()->GetU(); - datafile << FDMExec->GetTranslation()->GetV(); - datafile << FDMExec->GetTranslation()->GetW(); - datafile << latitude; - datafile << longitude; - datafile << FDMExec->GetRotation()->Getphi(); - datafile << FDMExec->GetRotation()->Gettht(); - datafile << FDMExec->GetRotation()->Getpsi(); - datafile << h; - datafile.close(); - return true; - } else { - cerr << "Could not open dump file " << fname << endl; - return false; - } + double Q0Q0, Q1Q1, Q2Q2, Q3Q3; + double Q0Q1, Q0Q2, Q0Q3, Q1Q2; + double Q1Q3, Q2Q3; + + Q0Q0 = vQtrn(1)*vQtrn(1); + Q1Q1 = vQtrn(2)*vQtrn(2); + Q2Q2 = vQtrn(3)*vQtrn(3); + Q3Q3 = vQtrn(4)*vQtrn(4); + Q0Q1 = vQtrn(1)*vQtrn(2); + Q0Q2 = vQtrn(1)*vQtrn(3); + Q0Q3 = vQtrn(1)*vQtrn(4); + Q1Q2 = vQtrn(2)*vQtrn(3); + Q1Q3 = vQtrn(2)*vQtrn(4); + Q2Q3 = vQtrn(3)*vQtrn(4); + + mTl2b(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3; + mTl2b(1,2) = 2*(Q1Q2 + Q0Q3); + mTl2b(1,3) = 2*(Q1Q3 - Q0Q2); + mTl2b(2,1) = 2*(Q1Q2 - Q0Q3); + mTl2b(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3; + mTl2b(2,3) = 2*(Q2Q3 + Q0Q1); + mTl2b(3,1) = 2*(Q1Q3 + Q0Q2); + mTl2b(3,2) = 2*(Q2Q3 - Q0Q1); + mTl2b(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3; + + mTb2l = mTl2b; + mTb2l.T(); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGState::DumpData(string fname) +void FGState::IntegrateQuat(FGColumnVector3 vPQR, int rate) { - ofstream datafile(fname.c_str()); - - if (datafile) { - datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl; - datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl; - datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl; - datafile << "P: " << FDMExec->GetRotation()->GetP() << endl; - datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl; - datafile << "R: " << FDMExec->GetRotation()->GetR() << endl; - datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl; - datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl; - datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl; - datafile << "latitude: " << latitude << endl; - datafile << "longitude: " << longitude << endl; - datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl; - datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl; - datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl; - datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl; - datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl; - datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl; - datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl; - datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl; - datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl; - datafile << "h: " << h << endl; - datafile << "a: " << a << endl; - datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl; - datafile << "qbar: " << qbar << endl; - datafile << "sim_time: " << sim_time << endl; - datafile << "dt: " << dt << endl; - datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl; - datafile.close(); - return true; - } else { - return false; - } + 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 += Integrate(TRAPZ, dt*rate, vQdot, vQdot_prev); + + vQtrn.Normalize(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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; + + vEuler(ePhi) = atan2(mTl2b(2,3), mTl2b(3,3)); + vEuler(eTht) = asin(-mTl2b(1,3)); + vEuler(ePsi) = atan2(mTl2b(1,2), mTl2b(1,1)); + + if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI; + + return vEuler; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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,2) = -ca*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,2) = -sa*sb; + 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"); +} -bool FGState::DisplayData(void) +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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) { - cout << "U: " << FDMExec->GetTranslation()->GetU() << endl; - cout << "V: " << FDMExec->GetTranslation()->GetV() << endl; - cout << "W: " << FDMExec->GetTranslation()->GetW() << endl; - cout << "P: " << FDMExec->GetRotation()->GetP()*RADTODEG << endl; - cout << "Q: " << FDMExec->GetRotation()->GetQ()*RADTODEG << endl; - cout << "R: " << FDMExec->GetRotation()->GetR()*RADTODEG << endl; - cout << "L: " << FDMExec->GetAircraft()->GetL() << endl; - cout << "M: " << FDMExec->GetAircraft()->GetM() << endl; - cout << "N: " << FDMExec->GetAircraft()->GetN() << endl; - cout << "Vt: " << Vt << endl; - cout << "latitude: " << latitude << endl; - cout << "longitude: " << longitude << endl; - cout << "alpha: " << FDMExec->GetTranslation()->Getalpha()*RADTODEG << endl; - cout << "beta: " << FDMExec->GetTranslation()->Getbeta()*RADTODEG << endl; - cout << "gamma: " << FDMExec->GetTranslation()->Getgamma()*RADTODEG << endl; - cout << "phi: " << FDMExec->GetRotation()->Getphi()*RADTODEG << endl; - cout << "tht: " << FDMExec->GetRotation()->Gettht()*RADTODEG << endl; - cout << "psi: " << FDMExec->GetRotation()->Getpsi()*RADTODEG << endl; - cout << "Pdot: " << FDMExec->GetRotation()->GetPdot()*RADTODEG << endl; - cout << "Qdot: " << FDMExec->GetRotation()->GetQdot()*RADTODEG << endl; - cout << "Rdot: " << FDMExec->GetRotation()->GetRdot()*RADTODEG << endl; - cout << "h: " << h << endl; - cout << "a: " << a << endl; - cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl; - cout << "qbar: " << qbar << endl; - cout << "sim_time: " << sim_time << endl; - cout << "dt: " << dt << endl; - cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl; - - return true; + 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; + } + } +} }