X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim%2FFGState.cpp;h=999af7da55af9ef0a0e056f44949702fed14b616;hb=95440173caef3ce92ee7308fd58a24dabe4c5f8a;hp=3e023aa6276ddb32a4f2ff25ea6f86c94ac80658;hpb=ba10c133fc1ecf5a5879b0a126fe64d091d42aca;p=flightgear.git diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 3e023aa62..999af7da5 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -47,8 +47,14 @@ INCLUDES # endif #endif +#ifdef _WIN32 +#define snprintf _snprintf +#endif + #include "FGState.h" +namespace JSBSim { + static const char *IdSrc = "$Id$"; static const char *IdHdr = ID_STATE; @@ -56,37 +62,17 @@ static const char *IdHdr = ID_STATE; MACROS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define RegisterVariable(ID,DEF) coeffdef[#ID] = ID; paramdef[ID] = DEF - /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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), - vlastQdot(4), - vQdot(4), - vTmp(4), - vEuler(3), - vUVW(3), - vLocalVelNED(3), - vLocalEuler(3) +FGState::FGState(FGFDMExec* fdex) { FDMExec = fdex; a = 1000.0; sim_time = 0.0; dt = 1.0/120.0; - ActiveEngine = -1; Aircraft = FDMExec->GetAircraft(); Translation = FDMExec->GetTranslation(); @@ -96,325 +82,23 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), Output = FDMExec->GetOutput(); Atmosphere = FDMExec->GetAtmosphere(); Aerodynamics = FDMExec->GetAerodynamics(); + GroundReactions = FDMExec->GetGroundReactions(); + Propulsion = FDMExec->GetPropulsion(); + PropertyManager = FDMExec->GetPropertyManager(); - RegisterVariable(FG_TIME, " time " ); - 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_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 " ); - 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_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_ALPHAH, " h-tail alpha " ); - RegisterVariable(FG_ALPHAW, " wing alpha " ); - RegisterVariable(FG_LBARH, " h-tail arm " ); - RegisterVariable(FG_LBARV, " v-tail arm " ); - RegisterVariable(FG_HTAILAREA, " h-tail area " ); - RegisterVariable(FG_VTAILAREA, " v-tail area " ); - RegisterVariable(FG_VBARH, " h-tail volume " ); - RegisterVariable(FG_VBARV, " v-tail volume " ); - RegisterVariable(FG_SET_LOGGING, " data_logging " ); - - if (debug_lvl & 2) cout << "Instantiated: FGState" << endl; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for(int i=0;i<4;i++) vQdot_prev[i].InitMatrix(); -FGState::~FGState() -{ - if (debug_lvl & 2) cout << "Destroyed: FGState" << endl; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -float FGState::GetParameter(eParam val_idx) { - float scratch; + bind(); - switch(val_idx) { - case FG_TIME: - return sim_time; - case FG_QBAR: - return Translation->Getqbar(); - case FG_WINGAREA: - return Aircraft->GetWingArea(); - case FG_WINGSPAN: - return Aircraft->GetWingSpan(); - case FG_CBAR: - return Aircraft->Getcbar(); - case FG_LBARH: - return Aircraft->Getlbarh(); - case FG_LBARV: - return Aircraft->Getvbarh(); - case FG_HTAILAREA: - return Aircraft->GetHTailArea(); - case FG_VTAILAREA: - return Aircraft->GetVTailArea(); - case FG_VBARH: - return Aircraft->Getvbarh(); - case FG_VBARV: - return Aircraft->Getvbarv(); - case FG_ALPHA: - return Translation->Getalpha(); - case FG_ALPHAW: - return Translation->Getalpha() + Aircraft->GetWingIncidence(); - case FG_ALPHADOT: - return Translation->Getadot(); - case FG_BETA: - return Translation->Getbeta(); - case FG_BETADOT: - 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 Rotation->GetPQR(eQ); - case FG_ROLLRATE: - return Rotation->GetPQR(eP); - case FG_YAWRATE: - 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 FCS->GetDePos(); - case FG_AILERON_POS: - return FCS->GetDaPos(); - case FG_RUDDER_POS: - return FCS->GetDrPos(); - case FG_SPDBRAKE_POS: - return FCS->GetDsbPos(); - case FG_SPOILERS_POS: - return FCS->GetDspPos(); - case FG_FLAPS_POS: - return FCS->GetDfPos(); - case FG_ELEVATOR_CMD: - return FCS->GetDeCmd(); - case FG_AILERON_CMD: - return FCS->GetDaCmd(); - case FG_RUDDER_CMD: - return FCS->GetDrCmd(); - case FG_SPDBRAKE_CMD: - return FCS->GetDsbCmd(); - case FG_SPOILERS_CMD: - return FCS->GetDspCmd(); - case FG_FLAPS_CMD: - return FCS->GetDfCmd(); - case FG_MACH: - return Translation->GetMach(); - case FG_ALTITUDE: - return Position->Geth(); - case FG_BI2VEL: - if(Translation->GetVt() > 0) - return Aircraft->GetWingSpan()/(2.0 * Translation->GetVt()); - else - return 0; - case FG_CI2VEL: - if(Translation->GetVt() > 0) - return Aircraft->Getcbar()/(2.0 * Translation->GetVt()); - else - return 0; - case FG_THROTTLE_CMD: - if (ActiveEngine < 0) return FCS->GetThrottleCmd(0); - else return FCS->GetThrottleCmd(ActiveEngine); - case FG_THROTTLE_POS: - if (ActiveEngine < 0) return FCS->GetThrottlePos(0); - else return FCS->GetThrottlePos(ActiveEngine); - case FG_HOVERB: - return Position->GetHOverB(); - case FG_PITCH_TRIM_CMD: - return FCS->GetPitchTrimCmd(); - default: - cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl; - return 0.0; - } - return 0; + Debug(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: - FCS->SetDePos(val); - break; - case FG_AILERON_POS: - FCS->SetDaPos(val); - break; - case FG_RUDDER_POS: - FCS->SetDrPos(val); - break; - case FG_SPDBRAKE_POS: - FCS->SetDsbPos(val); - break; - case FG_SPOILERS_POS: - FCS->SetDspPos(val); - break; - case FG_FLAPS_POS: - FCS->SetDfPos(val); - break; - case FG_THROTTLE_POS: - 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; - } -} - -//*************************************************************************** -// -// Reset: Assume all angles READ FROM FILE IN DEGREES !! -// - -bool FGState::Reset(string path, string acname, string fname) +FGState::~FGState() { - string resetDef; - string token=""; - - float U, V, W; - float phi, tht, psi; - float latitude, longitude, h; - float wdir, wmag, wnorth, weast; - -# ifndef macintosh - resetDef = path + "/" + acname + "/" + fname + ".xml"; -# else - resetDef = path + ";" + acname + ";" + fname + ".xml"; -# endif - - FGConfigFile resetfile(resetDef); - if (!resetfile.IsOpen()) return false; - - resetfile.GetNextConfigLine(); - token = resetfile.GetValue(); - if (token != "initialize") { - cerr << "The reset file " << resetDef - << " does not appear to be a reset file" << endl; - return false; - } - - resetfile.GetNextConfigLine(); - resetfile >> token; - while (token != "/initialize" && token != "EOF") { - if (token == "UBODY") resetfile >> U; - if (token == "VBODY") resetfile >> V; - if (token == "WBODY") resetfile >> W; - if (token == "LATITUDE") resetfile >> latitude; - if (token == "LONGITUDE") resetfile >> longitude; - if (token == "PHI") resetfile >> phi; - if (token == "THETA") resetfile >> tht; - if (token == "PSI") resetfile >> psi; - if (token == "ALTITUDE") resetfile >> h; - if (token == "WINDDIR") resetfile >> wdir; - if (token == "VWIND") resetfile >> wmag; - - resetfile >> token; - } - - - Position->SetLatitude(latitude*DEGTORAD); - Position->SetLongitude(longitude*DEGTORAD); - Position->Seth(h); - - wnorth = wmag*KTSTOFPS*cos(wdir*DEGTORAD); - weast = wmag*KTSTOFPS*sin(wdir*DEGTORAD); - - Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD, - latitude*DEGTORAD, longitude*DEGTORAD, h, wnorth, weast, 0.0); - - return true; + unbind(); + Debug(1); } //*************************************************************************** @@ -422,14 +106,14 @@ 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, - float wnorth, float weast, float wdown) +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; - float qbar, Vt; - FGColumnVector3 vAero; + double alpha, beta; + double qbar, Vt; + FGColumnVector3 vAeroUVW; Position->SetLatitude(Latitude); Position->SetLongitude(Longitude); @@ -447,14 +131,14 @@ void FGState::Initialize(float U, float V, float W, Atmosphere->SetWindNED(wnorth, weast, wdown); - vAero = vUVW + mTl2b*Atmosphere->GetWindNED(); + vAeroUVW = vUVW + mTl2b*Atmosphere->GetWindNED(); - if (vAero(eW) != 0.0) - alpha = vAero(eU)*vAero(eU) > 0.0 ? atan2(vAero(eW), vAero(eU)) : 0.0; + if (vAeroUVW(eW) != 0.0) + alpha = vAeroUVW(eU)*vAeroUVW(eU) > 0.0 ? atan2(vAeroUVW(eW), vAeroUVW(eU)) : 0.0; else alpha = 0.0; - if (vAero(eV) != 0.0) - beta = vAero(eU)*vAero(eU)+vAero(eW)*vAero(eW) > 0.0 ? atan2(vAero(eV), (fabs(vAero(eU))/vAero(eU))*sqrt(vAero(eU)*vAero(eU) + vAero(eW)*vAero(eW))) : 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; @@ -474,12 +158,12 @@ void FGState::Initialize(float U, float V, float W, //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGState::Initialize(FGInitialCondition *FGIC) { - - float tht,psi,phi; - float U, V, W, h; - float latitude, longitude; - float wnorth,weast, wdown; +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(); @@ -504,37 +188,15 @@ void FGState::Initialize(FGInitialCondition *FGIC) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGState::StoreData(string fname) { - ofstream datafile(fname.c_str()); - - if (datafile) { - 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 { - 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; @@ -563,10 +225,11 @@ void FGState::InitMatrices(float phi, float tht, float psi) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -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); @@ -595,21 +258,22 @@ void FGState::CalcMatrices(void) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGState::IntegrateQuat(FGColumnVector3 vPQR, int rate) { +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.Normalize(); + vQtrn += Integrate(TRAPZ, dt*rate, vQdot, vQdot_prev); - vlastQdot = vQdot; + vQtrn.Normalize(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGColumnVector3& FGState::CalcEuler(void) { +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; @@ -624,33 +288,65 @@ FGColumnVector3& FGState::CalcEuler(void) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGMatrix33& FGState::GetTs2b(float alpha, float beta) +FGMatrix33& FGState::GetTs2b(void) { - float ca, cb, sa, sb; + 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) { - char out[80], flap[10], gear[10]; +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", @@ -659,67 +355,128 @@ void FGState::ReportState(void) { FDMExec->GetMassBalance()->GetXYZcg(2), FDMExec->GetMassBalance()->GetXYZcg(3)); cout << out; - if( FCS->GetDfPos() <= 0.01) + if ( FCS->GetDfPos() <= 0.01) snprintf(flap,10,"Up"); else snprintf(flap,10,"%2.0f",FCS->GetDfPos()); - if(Aircraft->GetGearUp() == true) - snprintf(gear,10,"Up"); + + if (FCS->GetGearPos() < 0.01) + snprintf(gear,12,"Up"); + else if (FCS->GetGearPos() > 0.99) + snprintf(gear,12,"Down"); else - snprintf(gear,10,"Down"); - snprintf(out,80, " Flaps: %3s Gear: %4s\n",flap,gear); + 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(), - GetParameter(FG_MACH) ); + 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", - GetParameter(FG_ALPHA)*RADTODEG, - Rotation->Gettht()*RADTODEG ); + 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->GetGamma()*radtodeg, Position->Gethdot()*60 ); cout << out; snprintf(out,80, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n", - Aerodynamics->GetNlf(), - GetParameter(FG_PITCHRATE)*RADTODEG ); + Aircraft->GetNlf(), + Rotation->GetPQR(2)*radtodeg ); cout << out; - snprintf(out,80, " Heading: %3.0f deg true Sideslip: %5.2f deg\n", - Rotation->Getpsi()*RADTODEG, - GetParameter(FG_BETA)*RADTODEG ); + 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\n", - Rotation->Getphi()*RADTODEG ); + 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", - GetParameter(FG_ELEVATOR_POS)*RADTODEG, - GetParameter(FG_AILERON_POS)*RADTODEG, - GetParameter(FG_RUDDER_POS)*RADTODEG ); + 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),'%' ); + 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()*jsbFPSTOKTS, - FDMExec->GetAuxiliary()->GetCrossWind()*jsbFPSTOKTS ); + 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()*jsbFPSTOKTS, - Position->GetGroundTrack()*RADTODEG ); + Position->GetVground()*fpstokts, + Position->GetGroundTrack()*radtodeg ); cout << out; - +#endif } + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGState::Debug(void) +void FGState::bind(void) { - //TODO: Add your source code here + 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; + } + } +} +}