X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGState.cpp;h=d97dfb439cabc98b893d099ab6733e93fffcc5d1;hb=bb785797b9dd171f21c2577e0ba1a9c84e107583;hp=51d6deaff67cfa0ed63748365611bf683e5a1b22;hpb=21a5886bcbfadb457074602fbde16c427badcc6c;p=flightgear.git diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 51d6deaff..d97dfb439 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -1,4 +1,4 @@ -/******************************************************************************* +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Module: FGState.cpp Author: Jon Berndt @@ -32,48 +32,40 @@ HISTORY -------------------------------------------------------------------------------- 11/17/98 JSB Created -******************************************************************************** +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES -*******************************************************************************/ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #ifdef FGFS # include -# ifdef FG_HAVE_STD_INCLUDES -# include -# else -# include -# endif +# include #else # include #endif #ifndef M_PI # include -# define M_PI FG_PI +# define M_PI SG_PI #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 = "$Id$"; +static const char *IdHdr = ID_STATE; + +extern short debug_lvl; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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 @@ -83,15 +75,29 @@ MACROS FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), mTl2b(3,3), mTs2b(3,3), - vQtrn(4) + vQtrn(4), + vlastQdot(4), + vQdot(4), + vTmp(4), + vEuler(3) { FDMExec = fdex; - adot = bdot = 0.0; a = 1000.0; sim_time = 0.0; dt = 1.0/120.0; - + ActiveEngine = -1; + + Aircraft = FDMExec->GetAircraft(); + Translation = FDMExec->GetTranslation(); + Rotation = FDMExec->GetRotation(); + Position = FDMExec->GetPosition(); + FCS = FDMExec->GetFCS(); + Output = FDMExec->GetOutput(); + Atmosphere = FDMExec->GetAtmosphere(); + Aerodynamics = FDMExec->GetAerodynamics(); + + RegisterVariable(FG_TIME, " time " ); RegisterVariable(FG_QBAR, " qbar " ); RegisterVariable(FG_WINGAREA, " wing_area " ); RegisterVariable(FG_WINGSPAN, " wingspan " ); @@ -100,9 +106,13 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), RegisterVariable(FG_ALPHADOT, " alphadot " ); RegisterVariable(FG_BETA, " beta " ); RegisterVariable(FG_BETADOT, " betadot " ); + RegisterVariable(FG_PHI, " roll_angle " ); + RegisterVariable(FG_THT, " pitch_angle " ); + RegisterVariable(FG_PSI, " heading_angle " ); RegisterVariable(FG_PITCHRATE, " pitch_rate " ); RegisterVariable(FG_ROLLRATE, " roll_rate " ); RegisterVariable(FG_YAWRATE, " yaw_rate " ); + RegisterVariable(FG_CL_SQRD, " Clift_sqrd " ); RegisterVariable(FG_MACH, " mach " ); RegisterVariable(FG_ALTITUDE, " altitude " ); RegisterVariable(FG_BI2VEL, " BI2Vel " ); @@ -121,86 +131,114 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), RegisterVariable(FG_FLAPS_CMD, " flaps_cmd " ); RegisterVariable(FG_THROTTLE_CMD, " throttle_cmd " ); RegisterVariable(FG_THROTTLE_POS, " throttle_pos " ); + RegisterVariable(FG_ACTIVE_ENGINE, " active_engine " ); RegisterVariable(FG_HOVERB, " height/span " ); RegisterVariable(FG_PITCH_TRIM_CMD, " pitch_trim_cmd " ); + RegisterVariable(FG_LEFT_BRAKE_CMD, " left_brake_cmd " ); + RegisterVariable(FG_RIGHT_BRAKE_CMD," right_brake_cmd "); + RegisterVariable(FG_CENTER_BRAKE_CMD," center_brake_cmd "); + RegisterVariable(FG_SET_LOGGING, " data_logging " ); + + if (debug_lvl & 2) cout << "Instantiated: FGState" << endl; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGState::~FGState(void) {} +FGState::~FGState() +{ + if (debug_lvl & 2) cout << "Destroyed: FGState" << endl; +} -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% float FGState::GetParameter(eParam val_idx) { + float scratch; + switch(val_idx) { + case FG_TIME: + return sim_time; case FG_QBAR: - return FDMExec->GetTranslation()->Getqbar(); + return Translation->Getqbar(); case FG_WINGAREA: - return FDMExec->GetAircraft()->GetWingArea(); + return Aircraft->GetWingArea(); case FG_WINGSPAN: - return FDMExec->GetAircraft()->GetWingSpan(); + return Aircraft->GetWingSpan(); case FG_CBAR: - return FDMExec->GetAircraft()->Getcbar(); + return Aircraft->Getcbar(); case FG_ALPHA: - return FDMExec->GetTranslation()->Getalpha(); + return Translation->Getalpha(); case FG_ALPHADOT: - return Getadot(); + return Translation->Getadot(); case FG_BETA: - return FDMExec->GetTranslation()->Getbeta(); + return Translation->Getbeta(); case FG_BETADOT: - return Getbdot(); + return Translation->Getbdot(); + case FG_PHI: + return Rotation->Getphi(); + case FG_THT: + return Rotation->Gettht(); + case FG_PSI: + return Rotation->Getpsi(); case FG_PITCHRATE: - return (FDMExec->GetRotation()->GetPQR())(2); + return Rotation->GetPQR(eQ); case FG_ROLLRATE: - return (FDMExec->GetRotation()->GetPQR())(1); + return Rotation->GetPQR(eP); case FG_YAWRATE: - return (FDMExec->GetRotation()->GetPQR())(3); + return Rotation->GetPQR(eR); + case FG_CL_SQRD: + if (Translation->Getqbar() > 0.00) + scratch = Aerodynamics->GetvLastFs(eLift)/(Aircraft->GetWingArea()*Translation->Getqbar()); + else + scratch = 0.0; + return scratch*scratch; case FG_ELEVATOR_POS: - return FDMExec->GetFCS()->GetDePos(); + return FCS->GetDePos(); case FG_AILERON_POS: - return FDMExec->GetFCS()->GetDaPos(); + return FCS->GetDaPos(); case FG_RUDDER_POS: - return FDMExec->GetFCS()->GetDrPos(); + return FCS->GetDrPos(); case FG_SPDBRAKE_POS: - return FDMExec->GetFCS()->GetDsbPos(); + return FCS->GetDsbPos(); case FG_SPOILERS_POS: - return FDMExec->GetFCS()->GetDspPos(); + return FCS->GetDspPos(); case FG_FLAPS_POS: - return FDMExec->GetFCS()->GetDfPos(); + return FCS->GetDfPos(); case FG_ELEVATOR_CMD: - return FDMExec->GetFCS()->GetDeCmd(); + return FCS->GetDeCmd(); case FG_AILERON_CMD: - return FDMExec->GetFCS()->GetDaCmd(); + return FCS->GetDaCmd(); case FG_RUDDER_CMD: - return FDMExec->GetFCS()->GetDrCmd(); + return FCS->GetDrCmd(); case FG_SPDBRAKE_CMD: - return FDMExec->GetFCS()->GetDsbCmd(); + return FCS->GetDsbCmd(); case FG_SPOILERS_CMD: - return FDMExec->GetFCS()->GetDspCmd(); + return FCS->GetDspCmd(); case FG_FLAPS_CMD: - return FDMExec->GetFCS()->GetDfCmd(); + return FCS->GetDfCmd(); case FG_MACH: - return FDMExec->GetTranslation()->GetMach(); + return Translation->GetMach(); case FG_ALTITUDE: - return FDMExec->GetPosition()->Geth(); + return Position->Geth(); case FG_BI2VEL: - if(FDMExec->GetTranslation()->GetVt() > 0) - return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt()); + if(Translation->GetVt() > 0) + return Aircraft->GetWingSpan()/(2.0 * Translation->GetVt()); else return 0; case FG_CI2VEL: - if(FDMExec->GetTranslation()->GetVt() > 0) - return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt()); + if(Translation->GetVt() > 0) + return Aircraft->Getcbar()/(2.0 * Translation->GetVt()); else return 0; case FG_THROTTLE_CMD: - return FDMExec->GetFCS()->GetThrottleCmd(0); + if (ActiveEngine < 0) return FCS->GetThrottleCmd(0); + else return FCS->GetThrottleCmd(ActiveEngine); case FG_THROTTLE_POS: - return FDMExec->GetFCS()->GetThrottlePos(0); + if (ActiveEngine < 0) return FCS->GetThrottlePos(0); + else return FCS->GetThrottlePos(ActiveEngine); case FG_HOVERB: - return FDMExec->GetPosition()->GetHOverB(); + return Position->GetHOverB(); case FG_PITCH_TRIM_CMD: - return FDMExec->GetFCS()->GetPitchTrimCmd(); + return FCS->GetPitchTrimCmd(); default: cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl; return 0.0; @@ -208,42 +246,88 @@ float FGState::GetParameter(eParam val_idx) { return 0; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% float FGState::GetParameter(string val_string) { return GetParameter(coeffdef[val_string]); } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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); + FCS->SetDePos(val); break; case FG_AILERON_POS: - FDMExec->GetFCS()->SetDaPos(val); + FCS->SetDaPos(val); break; case FG_RUDDER_POS: - FDMExec->GetFCS()->SetDrPos(val); + FCS->SetDrPos(val); break; case FG_SPDBRAKE_POS: - FDMExec->GetFCS()->SetDsbPos(val); + FCS->SetDsbPos(val); break; case FG_SPOILERS_POS: - FDMExec->GetFCS()->SetDspPos(val); + FCS->SetDspPos(val); break; case FG_FLAPS_POS: - FDMExec->GetFCS()->SetDfPos(val); + FCS->SetDfPos(val); break; case FG_THROTTLE_POS: - FDMExec->GetFCS()->SetThrottlePos(-1,val); + FCS->SetThrottlePos(ActiveEngine,val); + break; + + case FG_ELEVATOR_CMD: + FCS->SetDeCmd(val); + break; + case FG_AILERON_CMD: + FCS->SetDaCmd(val); + break; + case FG_RUDDER_CMD: + FCS->SetDrCmd(val); + break; + case FG_SPDBRAKE_CMD: + FCS->SetDsbCmd(val); + break; + case FG_SPOILERS_CMD: + FCS->SetDspCmd(val); + break; + case FG_FLAPS_CMD: + FCS->SetDfCmd(val); + break; + case FG_THROTTLE_CMD: + FCS->SetThrottleCmd(ActiveEngine,val); + break; + + case FG_ACTIVE_ENGINE: + ActiveEngine = (int)val; + break; + + case FG_LEFT_BRAKE_CMD: + FCS->SetLBrake(val); + break; + case FG_CENTER_BRAKE_CMD: + FCS->SetCBrake(val); + break; + case FG_RIGHT_BRAKE_CMD: + FCS->SetRBrake(val); + break; + + case FG_SET_LOGGING: + if (val < -0.01) Output->Disable(); + else if (val > 0.01) Output->Enable(); + else Output->Toggle(); + break; + + default: + cerr << "Parameter '" << val_idx << "' (" << paramdef[val_idx] << ") not handled" << endl; } } @@ -258,9 +342,13 @@ bool FGState::Reset(string path, string acname, string fname) { float phi, tht, psi; float latitude, longitude, h; - resetDef = path + "/" + acname + "/" + fname; + resetDef = path + "/" + acname + "/" + fname + ".xml"; - ifstream resetfile(resetDef.c_str()); +#if defined ( sgi ) && !defined( __GNUC__ ) + ifstream resetfile(resetDef.c_str(), ios::in ); +#else + ifstream resetfile(resetDef.c_str(), ios::in | ios::binary ); +#endif if (resetfile) { resetfile >> U; @@ -274,9 +362,9 @@ bool FGState::Reset(string path, string acname, string fname) { resetfile >> h; resetfile.close(); - FDMExec->GetPosition()->SetLatitude(latitude*DEGTORAD); - FDMExec->GetPosition()->SetLongitude(longitude*DEGTORAD); - FDMExec->GetPosition()->Seth(h); + Position->SetLatitude(latitude*DEGTORAD); + Position->SetLongitude(longitude*DEGTORAD); + Position->Seth(h); Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD, latitude*DEGTORAD, longitude*DEGTORAD, h); @@ -298,15 +386,15 @@ void FGState::Initialize(float U, float V, float W, float Latitude, float Longitude, float H) { FGColumnVector vUVW(3); FGColumnVector vLocalVelNED(3); - FGColumnVector vEuler(3); + FGColumnVector vLocalEuler(3); float alpha, beta; float qbar, Vt; - 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(); if (W != 0.0) alpha = U*U > 0.0 ? atan2(W, U) : 0.0; @@ -318,28 +406,28 @@ void FGState::Initialize(float U, float V, float W, beta = 0.0; vUVW << U << V << W; - FDMExec->GetTranslation()->SetUVW(vUVW); + Translation->SetUVW(vUVW); - vEuler << phi << tht << psi; - FDMExec->GetRotation()->SetEuler(vEuler); + vLocalEuler << phi << tht << psi; + Rotation->SetEuler(vLocalEuler); - 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); + qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity(); + Translation->Setqbar(qbar); InitMatrices(phi, tht, psi); vLocalVelNED = mTb2l*vUVW; - FDMExec->GetPosition()->SetvVel(vLocalVelNED); + Position->SetvVel(vLocalVelNED); } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGState::Initialize(FGInitialCondition *FGIC) { @@ -358,23 +446,28 @@ void FGState::Initialize(FGInitialCondition *FGIC) { psi = FGIC->GetPsiRadIC(); Initialize(U, V, W, phi, tht, psi, latitude, longitude, h); + + Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() ); + Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + + FGIC->GetTerrainAltitudeFtIC() ); + } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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 << Translation->GetUVW(eU); + datafile << Translation->GetUVW(eV); + datafile << Translation->GetUVW(eW); + datafile << Position->GetLatitude(); + datafile << Position->GetLongitude(); + datafile << Rotation->GetEuler(ePhi); + datafile << Rotation->GetEuler(eTht); + datafile << Rotation->GetEuler(ePsi); + datafile << Position->Geth(); datafile.close(); return true; } else { @@ -383,7 +476,7 @@ bool FGState::StoreData(string fname) { } } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGState::InitMatrices(float phi, float tht, float psi) { float thtd2, psid2, phid2; @@ -419,7 +512,7 @@ void FGState::InitMatrices(float phi, float tht, float psi) { CalcMatrices(); } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGState::CalcMatrices(void) { float Q0Q0, Q1Q1, Q2Q2, Q3Q3; @@ -451,17 +544,13 @@ void FGState::CalcMatrices(void) { mTb2l.T(); } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { - static FGColumnVector vlastQdot(4); - static FGColumnVector vQdot(4); - 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.Normalize(); @@ -469,11 +558,9 @@ void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { vlastQdot = vQdot; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FGColumnVector FGState::CalcEuler(void) { - static FGColumnVector vEuler(3); - if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001; if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001; @@ -486,7 +573,7 @@ FGColumnVector FGState::CalcEuler(void) { return vEuler; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FGMatrix FGState::GetTs2b(float alpha, float beta) { float ca, cb, sa, sb; @@ -509,5 +596,10 @@ FGMatrix FGState::GetTs2b(float alpha, float beta) { return mTs2b; } -/******************************************************************************/ +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGState::Debug(void) +{ + //TODO: Add your source code here +}