From 07720af69381ae1eae661889d011573da0070111 Mon Sep 17 00:00:00 2001 From: Torsten Dreyer Date: Sat, 19 Jan 2013 22:32:55 +0100 Subject: [PATCH] sync with JSBSim --- src/FDM/JSBSim/CMakeLists.txt | 18 + src/FDM/JSBSim/FGFDMExec.cpp | 230 ++-- src/FDM/JSBSim/FGFDMExec.h | 43 +- .../initialization/FGInitialCondition.cpp | 78 +- .../initialization/FGInitialCondition.h | 25 +- .../JSBSim/initialization/FGSimplexTrim.cpp | 270 ++++ src/FDM/JSBSim/initialization/FGSimplexTrim.h | 81 ++ src/FDM/JSBSim/initialization/FGTrim.cpp | 4 +- src/FDM/JSBSim/initialization/FGTrimAxis.cpp | 11 +- src/FDM/JSBSim/initialization/FGTrimmer.cpp | 423 +++++++ src/FDM/JSBSim/initialization/FGTrimmer.h | 64 + src/FDM/JSBSim/input_output/FGOutputFG.cpp | 352 ++++++ src/FDM/JSBSim/input_output/FGOutputFG.h | 87 ++ src/FDM/JSBSim/input_output/FGOutputFile.cpp | 101 ++ src/FDM/JSBSim/input_output/FGOutputFile.h | 131 ++ .../JSBSim/input_output/FGOutputSocket.cpp | 402 ++++++ src/FDM/JSBSim/input_output/FGOutputSocket.h | 121 ++ .../JSBSim/input_output/FGOutputTextFile.cpp | 390 ++++++ .../JSBSim/input_output/FGOutputTextFile.h | 100 ++ src/FDM/JSBSim/input_output/FGOutputType.cpp | 275 ++++ src/FDM/JSBSim/input_output/FGOutputType.h | 222 ++++ .../JSBSim/input_output/FGPropertyManager.cpp | 72 +- .../JSBSim/input_output/FGPropertyManager.h | 35 +- src/FDM/JSBSim/input_output/FGScript.cpp | 10 +- src/FDM/JSBSim/input_output/FGScript.h | 9 +- src/FDM/JSBSim/input_output/FGXMLElement.cpp | 117 +- src/FDM/JSBSim/input_output/FGXMLElement.h | 12 +- src/FDM/JSBSim/input_output/FGXMLFileRead.h | 15 +- src/FDM/JSBSim/input_output/FGXMLParse.cpp | 0 src/FDM/JSBSim/input_output/FGXMLParse.h | 0 src/FDM/JSBSim/input_output/net_fdm.hxx | 54 +- .../JSBSim/input_output/string_utilities.h | 7 +- src/FDM/JSBSim/math/FGCondition.cpp | 39 +- src/FDM/JSBSim/math/FGCondition.h | 4 +- src/FDM/JSBSim/math/FGFunction.cpp | 61 +- src/FDM/JSBSim/math/FGFunction.h | 21 +- src/FDM/JSBSim/math/FGMatrix33.cpp | 38 +- src/FDM/JSBSim/math/FGMatrix33.h | 8 +- src/FDM/JSBSim/math/FGModelFunctions.cpp | 16 +- src/FDM/JSBSim/math/FGModelFunctions.h | 0 src/FDM/JSBSim/math/FGNelderMead.cpp | 376 ++++++ src/FDM/JSBSim/math/FGNelderMead.h | 95 ++ src/FDM/JSBSim/math/FGParameter.h | 0 src/FDM/JSBSim/math/FGPropertyValue.cpp | 0 src/FDM/JSBSim/math/FGPropertyValue.h | 0 src/FDM/JSBSim/math/FGQuaternion.cpp | 37 +- src/FDM/JSBSim/math/FGQuaternion.h | 5 +- src/FDM/JSBSim/math/FGRealValue.cpp | 0 src/FDM/JSBSim/math/FGRealValue.h | 0 src/FDM/JSBSim/math/FGStateSpace.cpp | 172 +++ src/FDM/JSBSim/math/FGStateSpace.h | 881 +++++++++++++ src/FDM/JSBSim/math/LagrangeMultiplier.h | 0 src/FDM/JSBSim/models/FGAccelerations.cpp | 35 +- src/FDM/JSBSim/models/FGAccelerations.h | 16 +- src/FDM/JSBSim/models/FGAerodynamics.cpp | 12 +- src/FDM/JSBSim/models/FGAerodynamics.h | 18 +- src/FDM/JSBSim/models/FGAircraft.cpp | 32 +- src/FDM/JSBSim/models/FGAircraft.h | 9 +- src/FDM/JSBSim/models/FGAtmosphere.cpp | 38 +- src/FDM/JSBSim/models/FGAtmosphere.h | 11 +- src/FDM/JSBSim/models/FGAuxiliary.cpp | 35 +- src/FDM/JSBSim/models/FGAuxiliary.h | 9 +- src/FDM/JSBSim/models/FGExternalForce.cpp | 25 +- src/FDM/JSBSim/models/FGExternalForce.h | 6 +- src/FDM/JSBSim/models/FGFCS.cpp | 197 ++- src/FDM/JSBSim/models/FGFCS.h | 11 +- src/FDM/JSBSim/models/FGFCSChannel.h | 128 ++ src/FDM/JSBSim/models/FGGroundReactions.cpp | 52 +- src/FDM/JSBSim/models/FGGroundReactions.h | 5 +- src/FDM/JSBSim/models/FGInertial.h | 3 +- src/FDM/JSBSim/models/FGInput.cpp | 29 +- src/FDM/JSBSim/models/FGInput.h | 6 +- src/FDM/JSBSim/models/FGLGear.cpp | 92 +- src/FDM/JSBSim/models/FGLGear.h | 17 +- src/FDM/JSBSim/models/FGMassBalance.cpp | 56 +- src/FDM/JSBSim/models/FGMassBalance.h | 9 +- src/FDM/JSBSim/models/FGModel.cpp | 5 +- src/FDM/JSBSim/models/FGOutput.cpp | 1106 ++--------------- src/FDM/JSBSim/models/FGOutput.h | 158 ++- src/FDM/JSBSim/models/FGPropagate.cpp | 46 +- src/FDM/JSBSim/models/FGPropagate.h | 15 +- src/FDM/JSBSim/models/FGPropulsion.cpp | 28 +- src/FDM/JSBSim/models/atmosphere/FGWinds.cpp | 6 +- .../models/flight_control/FGAccelerometer.cpp | 10 +- .../models/flight_control/FGActuator.cpp | 67 +- .../JSBSim/models/flight_control/FGActuator.h | 20 +- .../models/flight_control/FGDeadBand.cpp | 3 +- .../models/flight_control/FGFCSFunction.cpp | 3 +- .../JSBSim/models/flight_control/FGGyro.cpp | 4 +- .../models/flight_control/FGMagnetometer.cpp | 4 +- .../JSBSim/models/flight_control/FGSensor.cpp | 4 +- .../JSBSim/models/flight_control/FGSwitch.cpp | 159 +-- .../JSBSim/models/flight_control/FGSwitch.h | 39 +- src/FDM/JSBSim/models/propulsion/FGEngine.cpp | 5 +- src/FDM/JSBSim/models/propulsion/FGEngine.h | 5 +- .../JSBSim/models/propulsion/FGPropeller.cpp | 24 +- src/FDM/JSBSim/models/propulsion/FGRocket.cpp | 11 +- src/FDM/JSBSim/models/propulsion/FGRocket.h | 13 +- src/FDM/JSBSim/models/propulsion/FGTank.cpp | 34 +- src/FDM/JSBSim/models/propulsion/FGTank.h | 3 +- .../JSBSim/models/propulsion/FGThruster.cpp | 31 +- .../JSBSim/models/propulsion/FGTurbine.cpp | 7 +- 102 files changed, 6218 insertions(+), 1955 deletions(-) create mode 100755 src/FDM/JSBSim/initialization/FGSimplexTrim.cpp create mode 100755 src/FDM/JSBSim/initialization/FGSimplexTrim.h create mode 100755 src/FDM/JSBSim/initialization/FGTrimmer.cpp create mode 100755 src/FDM/JSBSim/initialization/FGTrimmer.h create mode 100644 src/FDM/JSBSim/input_output/FGOutputFG.cpp create mode 100644 src/FDM/JSBSim/input_output/FGOutputFG.h create mode 100644 src/FDM/JSBSim/input_output/FGOutputFile.cpp create mode 100644 src/FDM/JSBSim/input_output/FGOutputFile.h create mode 100644 src/FDM/JSBSim/input_output/FGOutputSocket.cpp create mode 100644 src/FDM/JSBSim/input_output/FGOutputSocket.h create mode 100644 src/FDM/JSBSim/input_output/FGOutputTextFile.cpp create mode 100644 src/FDM/JSBSim/input_output/FGOutputTextFile.h create mode 100644 src/FDM/JSBSim/input_output/FGOutputType.cpp create mode 100644 src/FDM/JSBSim/input_output/FGOutputType.h mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGPropertyManager.cpp mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGScript.cpp mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGXMLElement.cpp mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGXMLElement.h mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGXMLFileRead.h mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGXMLParse.cpp mode change 100644 => 100755 src/FDM/JSBSim/input_output/FGXMLParse.h mode change 100644 => 100755 src/FDM/JSBSim/math/FGFunction.cpp mode change 100644 => 100755 src/FDM/JSBSim/math/FGFunction.h mode change 100644 => 100755 src/FDM/JSBSim/math/FGModelFunctions.cpp mode change 100644 => 100755 src/FDM/JSBSim/math/FGModelFunctions.h create mode 100755 src/FDM/JSBSim/math/FGNelderMead.cpp create mode 100755 src/FDM/JSBSim/math/FGNelderMead.h mode change 100644 => 100755 src/FDM/JSBSim/math/FGParameter.h mode change 100644 => 100755 src/FDM/JSBSim/math/FGPropertyValue.cpp mode change 100644 => 100755 src/FDM/JSBSim/math/FGPropertyValue.h mode change 100644 => 100755 src/FDM/JSBSim/math/FGRealValue.cpp mode change 100644 => 100755 src/FDM/JSBSim/math/FGRealValue.h create mode 100755 src/FDM/JSBSim/math/FGStateSpace.cpp create mode 100755 src/FDM/JSBSim/math/FGStateSpace.h mode change 100644 => 100755 src/FDM/JSBSim/math/LagrangeMultiplier.h mode change 100644 => 100755 src/FDM/JSBSim/models/FGAuxiliary.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/FGExternalForce.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/FGExternalForce.h create mode 100755 src/FDM/JSBSim/models/FGFCSChannel.h mode change 100644 => 100755 src/FDM/JSBSim/models/FGInput.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/FGInput.h mode change 100644 => 100755 src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/flight_control/FGFCSFunction.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/flight_control/FGGyro.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp mode change 100644 => 100755 src/FDM/JSBSim/models/flight_control/FGSensor.cpp diff --git a/src/FDM/JSBSim/CMakeLists.txt b/src/FDM/JSBSim/CMakeLists.txt index 2fca8e5a8..b136ccb7d 100644 --- a/src/FDM/JSBSim/CMakeLists.txt +++ b/src/FDM/JSBSim/CMakeLists.txt @@ -7,6 +7,8 @@ set(HEADERS initialization/FGInitialCondition.h initialization/FGTrim.h initialization/FGTrimAxis.h + initialization/FGSimplexTrim.h + initialization/FGTrimmer.h input_output/FGXMLParse.h input_output/FGXMLFileRead.h input_output/FGPropertyManager.h @@ -16,6 +18,11 @@ set(HEADERS input_output/FGXMLElement.h input_output/net_fdm.hxx input_output/FGGroundCallback.h + input_output/FGOutputFG.h + input_output/FGOutputFile.h + input_output/FGOutputSocket.h + input_output/FGOutputTextFile.h + input_output/FGOutputType.h math/FGParameter.h math/LagrangeMultiplier.h math/FGColumnVector3.h @@ -29,6 +36,8 @@ set(HEADERS math/FGRealValue.h math/FGRungeKutta.h math/FGTable.h + math/FGNelderMead.h + math/FGStateSpace.h models/FGAccelerations.h models/FGAerodynamics.h models/FGAircraft.h @@ -89,12 +98,19 @@ set(SOURCES initialization/FGInitialCondition.cpp initialization/FGTrim.cpp initialization/FGTrimAxis.cpp + initialization/FGSimplexTrim.cpp + initialization/FGTrimmer.cpp input_output/FGGroundCallback.cpp input_output/FGPropertyManager.cpp input_output/FGScript.cpp input_output/FGXMLElement.cpp input_output/FGXMLParse.cpp input_output/FGfdmSocket.cpp + input_output/FGOutputFG.cpp + input_output/FGOutputFile.cpp + input_output/FGOutputSocket.cpp + input_output/FGOutputTextFile.cpp + input_output/FGOutputType.cpp math/FGColumnVector3.cpp math/FGCondition.cpp math/FGFunction.cpp @@ -106,6 +122,8 @@ set(SOURCES math/FGRealValue.cpp math/FGRungeKutta.cpp math/FGTable.cpp + math/FGNelderMead.cpp + math/FGStateSpace.cpp models/FGAccelerations.cpp models/FGAerodynamics.cpp models/FGAircraft.cpp diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index b096f8583..e8bb6a1b3 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -1,3 +1,4 @@ + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Module: FGFDMExec.cpp @@ -63,6 +64,7 @@ INCLUDES #include "models/FGInput.h" #include "models/FGOutput.h" #include "initialization/FGInitialCondition.h" +#include "initialization/FGSimplexTrim.h" #include "input_output/FGPropertyManager.h" #include "input_output/FGScript.h" @@ -70,7 +72,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.133 2012/04/14 18:10:43 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.145 2012/11/11 18:43:07 bcoconni Exp $"; static const char *IdHdr = ID_FDMEXEC; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -96,7 +98,6 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root, unsigned int* fdmctr) : Root(root) holding = false; Terminate = false; StandAlone = false; - firstPass = true; IncrementThenHolding = false; // increment then hold is off by default TimeStepsUntilHold = -1; @@ -147,9 +148,11 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root, unsigned int* fdmctr) : Root(root) Constructing = true; typedef int (FGFDMExec::*iPMF)(void) const; + typedef double (FGFDMExec::*dPMF)(void) const; // typedef unsigned int (FGFDMExec::*uiPMF)(void) const; // instance->Tie("simulation/do_trim_analysis", this, (iPMF)0, &FGFDMExec::DoTrimAnalysis, false); instance->Tie("simulation/do_simple_trim", this, (iPMF)0, &FGFDMExec::DoTrim, false); + instance->Tie("simulation/do_simplex_trim", this, (iPMF)0, &FGFDMExec::DoSimplexTrim); instance->Tie("simulation/reset", this, (iPMF)0, &FGFDMExec::ResetToInitialConditions, false); instance->Tie("simulation/randomseed", this, (iPMF)0, &FGFDMExec::SRand, false); instance->Tie("simulation/terminate", (int *)&Terminate); @@ -157,6 +160,58 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root, unsigned int* fdmctr) : Root(root) instance->Tie("simulation/jsbsim-debug", this, &FGFDMExec::GetDebugLevel, &FGFDMExec::SetDebugLevel); instance->Tie("simulation/frame", (int *)&Frame, false); + // simplex trim properties + instance->SetDouble("trim/solver/rtol",0.0001); + instance->SetDouble("trim/solver/speed",2); + instance->SetDouble("trim/solver/abstol",0.001); + instance->SetDouble("trim/solver/iterMax",2000); + instance->SetInt("trim/solver/debugLevel",0); + instance->SetDouble("trim/solver/random",0); + instance->SetBool("trim/solver/showSimplex",false); +// instance->SetBool("trim/solver/showConvergence",true); + instance->SetBool("trim/solver/pause",false); + + instance->SetDouble("trim/solver/throttleGuess",0.50); + instance->SetDouble("trim/solver/throttleMin",0.0); + instance->SetDouble("trim/solver/throttleMax",1.0); +// instance->SetDouble("trim/solver/throttleInitialStepSize",0.1); + instance->SetDouble("trim/solver/throttleStep",0.1); + + instance->SetDouble("trim/solver/aileronGuess",0); + instance->SetDouble("trim/solver/aileronMin",-1.00); + instance->SetDouble("trim/solver/aileronMax",1.00); +// instance->SetDouble("trim/solver/aileronInitialStepSize",0.1); + instance->SetDouble("trim/solver/aileronStep",0.1); + + instance->SetDouble("trim/solver/rudderGuess",0); + instance->SetDouble("trim/solver/rudderMin",-1.00); + instance->SetDouble("trim/solver/rudderMax",1.00); +// instance->SetDouble("trim/solver/rudderInitialStepSize",0.1); + instance->SetDouble("trim/solver/rudderStep",0.1); + + instance->SetDouble("trim/solver/elevatorGuess",-0.1); + instance->SetDouble("trim/solver/elevatorMin",-1.0); + instance->SetDouble("trim/solver/elevatorMax",1.0); +// instance->SetDouble("trim/solver/elevatorInitialStepSize",0.1); + instance->SetDouble("trim/solver/elevatorStep",0.1); + + instance->SetDouble("trim/solver/alphaGuess",0.05); + instance->SetDouble("trim/solver/alphaMin",-0.1); + instance->SetDouble("trim/solver/alphaMax",.18); +// instance->SetDouble("trim/solver/alphaInitialStepSize",0.1); + instance->SetDouble("trim/solver/alphaStep",0.05); + + instance->SetDouble("trim/solver/betaGuess",0); + instance->SetDouble("trim/solver/betaMin",-0.05); + instance->SetDouble("trim/solver/betaMax",0.05); +// instance->SetDouble("trim/solver/betaInitialStepSize",0.1); + instance->SetDouble("trim/solver/betaStep",0.05); + + instance->SetBool("trim/solver/showConvergeStatus",true); +// instance->SetBool("trim/solver/pause",true); + instance->SetBool("trim/solver/variablePropPitch",false); +// instance->SetBool("trim/solver/debugLevel",0); + Constructing = false; } @@ -213,15 +268,13 @@ bool FGFDMExec::Allocate(void) Models[eSystems] = new FGFCS(this); Models[ePropulsion] = new FGPropulsion(this); Models[eAerodynamics] = new FGAerodynamics (this); - - GetGroundCallback()->SetSeaLevelRadius(((FGInertial*)Models[eInertial])->GetRefRadius()); - Models[eGroundReactions] = new FGGroundReactions(this); Models[eExternalReactions] = new FGExternalReactions(this); Models[eBuoyantForces] = new FGBuoyantForces(this); Models[eMassBalance] = new FGMassBalance(this); Models[eAircraft] = new FGAircraft(this); Models[eAccelerations] = new FGAccelerations(this); + Models[eOutput] = new FGOutput(this); // Assign the Model shortcuts for internal executive use only. Propagate = (FGPropagate*)Models[ePropagate]; @@ -238,12 +291,17 @@ bool FGFDMExec::Allocate(void) MassBalance = (FGMassBalance*)Models[eMassBalance]; Aircraft = (FGAircraft*)Models[eAircraft]; Accelerations = (FGAccelerations*)Models[eAccelerations]; + Output = (FGOutput*)Models[eOutput]; // Initialize planet (environment) constants LoadPlanetConstants(); + GetGroundCallback()->SetSeaLevelRadius(Inertial->GetRefRadius()); // Initialize models for (unsigned int i = 0; i < Models.size(); i++) { + // The Output model must not be initialized prior to IC loading + if (i == eOutput) continue; + LoadInputs(i); Models[i]->InitModel(); } @@ -263,9 +321,6 @@ bool FGFDMExec::DeAllocate(void) for (unsigned int i=0; iRun(); } - if (firstPass && !IntegrationSuspended()) { - // Outputs the initial conditions - for (unsigned int i = 0; i < Outputs.size(); i++) - Outputs[i]->Run(holding); - - firstPass = false; - } - IncrTime(); // returns true if success, false if complete @@ -371,8 +418,6 @@ void FGFDMExec::LoadInputs(unsigned int idx) Auxiliary->in.RPBody = MassBalance->StructuralToBody(Aircraft->GetXYZrp()); Auxiliary->in.vFw = Aerodynamics->GetvFw(); Auxiliary->in.vLocation = Propagate->GetLocation(); - Auxiliary->in.Latitude = Propagate->GetLatitude(); - Auxiliary->in.Longitude = Propagate->GetLongitude(); Auxiliary->in.CosTht = Propagate->GetCosEuler(eTht); Auxiliary->in.SinTht = Propagate->GetSinEuler(eTht); Auxiliary->in.CosPhi = Propagate->GetCosEuler(ePhi); @@ -413,7 +458,6 @@ void FGFDMExec::LoadInputs(unsigned int idx) Propulsion->in.PropFeather = FCS->GetPropFeather(); Propulsion->in.H_agl = Propagate->GetDistanceAGL(); Propulsion->in.PQR = Propagate->GetPQR(); - Propulsion->in.vXYZcg = MassBalance->GetXYZcg(); break; case eAerodynamics: @@ -475,8 +519,6 @@ void FGFDMExec::LoadInputs(unsigned int idx) Aircraft->in.GroundMoment = GroundReactions->GetMoments(); Aircraft->in.ExternalMoment = ExternalReactions->GetMoments(); Aircraft->in.BuoyantMoment = BuoyantForces->GetMoments(); - Aircraft->in.Weight = MassBalance->GetWeight(); - Aircraft->in.Tl2b = Propagate->GetTl2b(); break; case eAccelerations: Accelerations->in.J = MassBalance->GetJ(); @@ -513,7 +555,6 @@ void FGFDMExec::LoadPlanetConstants(void) { Propagate->in.vOmegaPlanet = Inertial->GetOmegaPlanet(); Accelerations->in.vOmegaPlanet = Inertial->GetOmegaPlanet(); - Propagate->in.RefRadius = Inertial->GetRefRadius(); Propagate->in.SemiMajor = Inertial->GetSemimajor(); Propagate->in.SemiMinor = Inertial->GetSemiminor(); Auxiliary->in.SLGravity = Inertial->SLgravity(); @@ -533,7 +574,6 @@ void FGFDMExec::LoadModelConstants(void) Auxiliary->in.Wingspan = Aircraft->GetWingSpan(); Auxiliary->in.Wingchord = Aircraft->Getcbar(); GroundReactions->in.vXYZcg = MassBalance->GetXYZcg(); - Propulsion->in.vXYZcg = MassBalance->GetXYZcg(); LoadPlanetConstants(); } @@ -543,11 +583,18 @@ void FGFDMExec::LoadModelConstants(void) bool FGFDMExec::RunIC(void) { + FGPropulsion* propulsion = (FGPropulsion*)Models[ePropulsion]; + + Models[eOutput]->InitModel(); + SuspendIntegration(); // saves the integration rate, dt, then sets it to 0.0. Initialize(IC); Run(); ResumeIntegration(); // Restores the integration rate to what it was. + for (unsigned int i=0; iGetNumEnginesRunning(); i++) + propulsion->InitRunning(IC->GetEngineRunning(i)); + return true; } @@ -575,11 +622,7 @@ void FGFDMExec::Initialize(FGInitialCondition *FGIC) void FGFDMExec::ResetToInitialConditions(int mode) { - if (mode == 1) { - for (unsigned int i=0; iSetStartNewFile(true); - } - } + if (mode == 1) Output->SetStartNewOutput(); ResetToInitialConditions(); } @@ -590,28 +633,17 @@ void FGFDMExec::ResetToInitialConditions(void) { if (Constructing) return; - vector ::iterator it; - for (it = Models.begin(); it != Models.end(); ++it) (*it)->InitModel(); - - RunIC(); - if (Script) Script->ResetEvents(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for (unsigned int i = 0; i < Models.size(); i++) { + // The Output model will be initialized during the RunIC() execution + if (i == eOutput) continue; -bool FGFDMExec::SetOutputFileName(const string& fname) -{ - if (Outputs.size() > 0) Outputs[0]->SetOutputFileName(fname); - else return false; - return true; -} + LoadInputs(i); + Models[i]->InitModel(); + } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + RunIC(); -string FGFDMExec::GetOutputFileName(void) -{ - if (Outputs.size() > 0) return Outputs[0]->GetOutputFileName(); - else return string(""); + if (Script) Script->ResetEvents(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -632,12 +664,12 @@ vector FGFDMExec::EnumerateFDMs(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGFDMExec::LoadScript(const string& script, double deltaT) +bool FGFDMExec::LoadScript(const string& script, double deltaT, const string initfile) { bool result; Script = new FGScript(this); - result = Script->LoadScript(RootDir + script, deltaT); + result = Script->LoadScript(RootDir + script, deltaT, initfile); return result; } @@ -828,30 +860,23 @@ bool FGFDMExec::LoadModel(const string& model, bool addModelToPath) } // Process the output element[s]. This element is OPTIONAL, and there may be more than one. - unsigned int idx=0; - typedef double (FGOutput::*iOPMF)(void) const; - typedef int (FGFDMExec::*iOPV)(void) const; element = document->FindElement("output"); while (element) { - if (debug_lvl > 0) cout << endl << " Output data set: " << idx << " "; - FGOutput* Output = new FGOutput(this); - result = Output->Load(element); + string output_file_name = aircraftCfgFileName; + + if (!element->GetAttributeValue("file").empty()) { + output_file_name = RootDir + element->GetAttributeValue("file"); + result = ((FGOutput*)Models[eOutput])->SetDirectivesFile(output_file_name); + } + else + result = ((FGOutput*)Models[eOutput])->Load(element); + if (!result) { - cerr << endl << "Aircraft output element has problems in file " << aircraftCfgFileName << endl; - delete Output; + cerr << endl << "Aircraft output element has problems in file " << output_file_name << endl; return result; - } else { - Output->InitModel(); - Schedule(Output); - Outputs.push_back(Output); - string outputProp = CreateIndexedPropertyName("simulation/output",idx); - instance->Tie(outputProp+"/log_rate_hz", Output, (iOPMF)0, &FGOutput::SetRate, false); - idx++; } element = document->FindNextElement("output"); } - if (idx) - instance->Tie("simulation/force-output", this, (iOPV)0, &FGFDMExec::ForceOutput, false); // Lastly, process the child element. This element is OPTIONAL - and NOT YET SUPPORTED. element = document->FindElement("child"); @@ -872,6 +897,10 @@ bool FGFDMExec::LoadModel(const string& model, bool addModelToPath) if (debug_lvl > 0) { LoadInputs(eMassBalance); // Update all input mass properties for the report. Models[eMassBalance]->Run(false); // Update all mass properties for the report. + LoadInputs(ePropulsion); // Update propulsion properties for the report. + Models[ePropulsion]->Run(false); // Update propulsion properties for the report. + LoadInputs(eMassBalance); // Update all (one more time) input mass properties for the report. + Models[eMassBalance]->Run(false); // Update all (one more time) mass properties for the report. ((FGMassBalance*)Models[eMassBalance])->GetMassPropertiesReport(); cout << endl << fgblue << highint @@ -921,7 +950,7 @@ void FGFDMExec::BuildPropertyCatalog(struct PropertyCatalogStructure* pcs) pcsNew->base_string = CreateIndexedPropertyName(pcsNew->base_string, node_idx); } if (pcs->node->getChild(i)->nChildren() == 0) { - if (pcsNew->base_string.substr(0,11) == string("/fdm/jsbsim")) { + if (pcsNew->base_string.substr(0,12) == string("/fdm/jsbsim/")) { pcsNew->base_string = pcsNew->base_string.erase(0,12); } PropertyCatalog.push_back(pcsNew->base_string); @@ -1109,24 +1138,6 @@ FGTrim* FGFDMExec::GetTrim(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGFDMExec::DisableOutput(void) -{ - for (unsigned i=0; iDisable(); - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGFDMExec::EnableOutput(void) -{ - for (unsigned i=0; iEnable(); - } -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - void FGFDMExec::CheckIncrementalHold(void) { // Only check if increment then hold is on @@ -1151,38 +1162,6 @@ void FGFDMExec::CheckIncrementalHold(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGFDMExec::ForceOutput(int idx) -{ - if (idx >= (int)0 && idx < (int)Outputs.size()) Outputs[idx]->Print(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -bool FGFDMExec::SetOutputDirectives(const string& fname) -{ - bool result; - - FGOutput* Output = new FGOutput(this); - Output->SetDirectivesFile(RootDir + fname); - result = Output->Load(0); - - if (result) { - Output->InitModel(); - Schedule(Output); - Output->Run(holding); - Outputs.push_back(Output); - typedef double (FGOutput::*iOPMF)(void) const; - string outputProp = CreateIndexedPropertyName("simulation/output",Outputs.size()-1); - instance->Tie(outputProp+"/log_rate_hz", Output, (iOPMF)0, &FGOutput::SetRate, false); - } - else - delete Output; - - return result; -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - void FGFDMExec::DoTrim(int mode) { double saved_time; @@ -1200,6 +1179,23 @@ void FGFDMExec::DoTrim(int mode) sim_time = saved_time; } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGFDMExec::DoSimplexTrim(int mode) +{ + double saved_time; + if (Constructing) return; + if (mode < 0 || mode > JSBSim::tNone) { + cerr << endl << "Illegal trimming mode!" << endl << endl; + return; + } + saved_time = sim_time; + FGSimplexTrim trim(this, (JSBSim::TrimMode)mode); + sim_time = saved_time; + Setsim_time(saved_time); +} + + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGFDMExec::SRand(int sr) diff --git a/src/FDM/JSBSim/FGFDMExec.h b/src/FDM/JSBSim/FGFDMExec.h index e879deea6..2192e11ec 100644 --- a/src/FDM/JSBSim/FGFDMExec.h +++ b/src/FDM/JSBSim/FGFDMExec.h @@ -50,12 +50,13 @@ INCLUDES #include "input_output/FGXMLFileRead.h" #include "models/FGPropagate.h" #include "math/FGColumnVector3.h" +#include "models/FGOutput.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.76 2012/01/21 16:46:08 jberndt Exp $" +#define ID_FDMEXEC "$Id: FGFDMExec.h,v 1.80 2012/10/25 04:56:57 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -77,8 +78,6 @@ class FGGroundReactions; class FGFCS; class FGInertial; class FGInput; -class FGOutput; -class FGPropagate; class FGPropulsion; class FGMassBalance; @@ -180,7 +179,7 @@ CLASS DOCUMENTATION property actually maps toa function call of DoTrim(). @author Jon S. Berndt - @version $Revision: 1.76 $ + @version $Revision: 1.80 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -240,6 +239,7 @@ public: eMassBalance, eAircraft, eAccelerations, + eOutput, eNumStandardModels }; /** Unbind all tied JSBSim properties. */ @@ -307,12 +307,16 @@ public: bool LoadModel(const string& model, bool addModelToPath = true); /** Loads a script - @param Script the full path name and file name for the script to be loaded. + @param Script The full path name and file name for the script to be loaded. @param deltaT The simulation integration step size, if given. If no value is supplied then 0.0 is used and the value is expected to be supplied in the script file itself. - @return true if successfully loadsd; false otherwise. */ - bool LoadScript(const string& Script, double deltaT=0.0); + @param initfile The initialization file that will override the initialization file + specified in the script file. If no file name is given on the command line, + the file specified in the script will be used. If an initialization file + is not given in either place, an error will result. + @return true if successfully loads; false otherwise. */ + bool LoadScript(const string& Script, double deltaT=0.0, const string initfile=""); /** Sets the path to the engine config file directories. @param path path to the directory under which engine config @@ -432,20 +436,24 @@ public: be logged. @param fname the filename of an output directives file. */ - bool SetOutputDirectives(const string& fname); + bool SetOutputDirectives(const string& fname) + {return Output->SetDirectivesFile(RootDir + fname);} /** Forces the specified output object to print its items once */ - void ForceOutput(int idx=0); + void ForceOutput(int idx=0) { Output->ForceOutput(idx); } + + /** Sets the logging rate for all output objects (if any). */ + void SetLoggingRate(double rate) { Output->SetRate(rate); } /** Sets (or overrides) the output filename @param fname the name of the file to output data to @return true if successful, false if there is no output specified for the flight model */ - bool SetOutputFileName(const string& fname); + bool SetOutputFileName(const string& fname) { return Output->SetOutputName(0, fname); } /** Retrieves the current output filename. @return the name of the output file for the first output specified by the flight model. If none is specified, the empty string is returned. */ - string GetOutputFileName(void); + string GetOutputFileName(void) const { return Output->GetOutputName(0); } /** Executes trimming in the selected mode. * @param mode Specifies how to trim: @@ -457,11 +465,12 @@ public: * - tTurn * - tNone */ void DoTrim(int mode); + void DoSimplexTrim(int mode); /// Disables data logging to all outputs. - void DisableOutput(void); + void DisableOutput(void) { Output->Disable(); } /// Enables data logging to all outputs. - void EnableOutput(void); + void EnableOutput(void) { Output->Enable(); } /// Pauses execution by preventing time from incrementing. void Hold(void) {holding = true;} /// Turn on hold after increment @@ -558,6 +567,10 @@ public: /** Retrieves the current debug level setting. */ int GetDebugLevel(void) const {return debug_lvl;}; + /** Initializes the simulation with initial conditions + @param FGIC The initial conditions that will be passed to the simulation. */ + void Initialize(FGInitialCondition *FGIC); + private: int Error; unsigned int Frame; @@ -572,7 +585,6 @@ private: bool Constructing; bool modelLoaded; bool IsChild; - bool firstPass; string modelName; string AircraftPath; string FullAircraftPath; @@ -597,6 +609,7 @@ private: FGMassBalance* MassBalance; FGAircraft* Aircraft; FGAccelerations* Accelerations; + FGOutput* Output; bool trim_status; int ta_mode; @@ -613,7 +626,6 @@ private: unsigned int* FDMctr; vector PropertyCatalog; - vector Outputs; vector ChildFDMList; vector Models; @@ -627,7 +639,6 @@ private: void LoadModelConstants(void); bool Allocate(void); bool DeAllocate(void); - void Initialize(FGInitialCondition *FGIC); void Debug(int from); }; diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp index 02c93618f..21d4e8b40 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp @@ -63,7 +63,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.81 2012/04/08 15:22:56 jberndt Exp $"; +static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.87 2013/01/19 14:19:43 bcoconni Exp $"; static const char *IdHdr = ID_INITIALCONDITION; //****************************************************************************** @@ -75,9 +75,7 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec) if(FDMExec != NULL ) { PropertyManager=fdmex->GetPropertyManager(); Atmosphere=fdmex->GetAtmosphere(); - Constructing = true; bind(); - Constructing = false; } else { cout << "FGInitialCondition: This class requires a pointer to a valid FGFDMExec object" << endl; } @@ -151,41 +149,7 @@ void FGInitialCondition::InitializeIC(void) lastSpeedSet = setvt; lastAltitudeSet = setasl; -} - -//****************************************************************************** - -void FGInitialCondition::WriteStateFile(int num) -{ - if (Constructing) return; - - string filename = fdmex->GetFullAircraftPath(); - - if (filename.empty()) - filename = "initfile.xml"; - else - filename.append("/initfile.xml"); - - ofstream outfile(filename.c_str()); - FGPropagate* Propagate = fdmex->GetPropagate(); - - if (outfile.is_open()) { - outfile << "" << endl; - outfile << "" << endl; - outfile << " " << Propagate->GetUVW(eU) << " " << endl; - outfile << " " << Propagate->GetUVW(eV) << " " << endl; - outfile << " " << Propagate->GetUVW(eW) << " " << endl; - outfile << " " << Propagate->GetEuler(ePhi)*radtodeg << " " << endl; - outfile << " " << Propagate->GetEuler(eTht)*radtodeg << " " << endl; - outfile << " " << Propagate->GetEuler(ePsi)*radtodeg << " " << endl; - outfile << " " << Propagate->GetLongitudeDeg() << " " << endl; - outfile << " " << Propagate->GetLatitudeDeg() << " " << endl; - outfile << " " << Propagate->GetDistanceAGL() << " " << endl; - outfile << "" << endl; - outfile.close(); - } else { - cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl; - } + enginesRunning.clear(); } //****************************************************************************** @@ -899,9 +863,9 @@ double FGInitialCondition::GetBodyVelFpsIC(int idx) const bool FGInitialCondition::Load(string rstfile, bool useStoredPath) { - string sep = "/"; + string init_file_name; if( useStoredPath ) { - init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml"; + init_file_name = fdmex->GetFullAircraftPath() + "/" + rstfile + ".xml"; } else { init_file_name = rstfile; } @@ -933,19 +897,10 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) result = Load_v1(); } - fdmex->RunIC(); - // Check to see if any engines are specified to be initialized in a running state - FGPropulsion* propulsion = fdmex->GetPropulsion(); Element* running_elements = document->FindElement("running"); while (running_elements) { - int n = int(running_elements->GetDataAsNumber()); - try { - propulsion->InitRunning(n); - } catch (string str) { - cerr << str << endl; - result = false; - } + enginesRunning.push_back(int(running_elements->GetDataAsNumber())); running_elements = document->FindNextElement("running"); } @@ -1067,9 +1022,6 @@ bool FGInitialCondition::Load_v2(void) if (position_el->FindElement("longitude")) position.SetLongitude(position_el->FindElementValueAsNumberConvertTo("longitude", "RAD")); - if (position_el->FindElement("latitude")) - position.SetLatitude(position_el->FindElementValueAsNumberConvertTo("latitude", "RAD")); - if (position_el->FindElement("radius")) { position.SetRadius(position_el->FindElementValueAsNumberConvertTo("radius", "FT")); } else if (position_el->FindElement("altitudeAGL")) { @@ -1082,6 +1034,19 @@ bool FGInitialCondition::Load_v2(void) result = false; } + Element* latitude_el = position_el->FindElement("latitude"); + if (latitude_el) { + string lat_type = latitude_el->GetAttributeValue("type"); + double latitude = position_el->FindElementValueAsNumberConvertTo("latitude", "RAD"); + if (lat_type == "geod" || lat_type == "geodetic") { + double longitude = position.GetLongitude(); + double altitude = position.GetAltitudeASL(); // SetPositionGeodetic() assumes altitude + position.SetPositionGeodetic(longitude, latitude, altitude); // is geodetic, but it's close enough for now. + position.SetAltitudeAGL(altitude, fdmex->GetSimTime()); + } else { + position.SetLatitude(latitude); + } + } } else { position = position_el->FindElementTripletConvertTo("FT"); } @@ -1445,13 +1410,6 @@ void FGInitialCondition::bind(void) &FGInitialCondition::GetRRadpsIC, &FGInitialCondition::SetRRadpsIC, true); - - typedef int (FGInitialCondition::*iPMF)(void) const; - PropertyManager->Tie("simulation/write-state-file", - this, - (iPMF)0, - &FGInitialCondition::WriteStateFile); - } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.h b/src/FDM/JSBSim/initialization/FGInitialCondition.h index 258e1a7f7..a0807421b 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.h +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.h @@ -55,7 +55,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.33 2011/12/10 15:49:21 bcoconni Exp $" +#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.36 2013/01/19 14:19:43 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -216,7 +216,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.33 2011/12/10 15:49:21 bcoconni Exp $" + @version "$Id: FGInitialCondition.h,v 1.36 2013/01/19 14:19:43 bcoconni Exp $" */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -652,13 +652,15 @@ public: @return true if successful */ bool Load(string rstname, bool useStoredPath = true ); - /** Get init-file name - */ - string GetInitFile(void) const {return init_file_name;} - /** Set init-file name - */ - void SetInitFile(string f) { init_file_name = f;} - void WriteStateFile(int num); + /** Get the number of engines running + */ + unsigned int GetNumEnginesRunning(void) const + { return (unsigned int)enginesRunning.size(); } + + /** Gets the running engine identification + @param engine index of running engine instance + @return the identification of running engine instance requested */ + int GetEngineRunning(unsigned int engine) const { return enginesRunning[engine]; } private: FGColumnVector3 vUVW_NED; @@ -674,6 +676,7 @@ private: speedset lastSpeedSet; altitudeset lastAltitudeSet; + vector enginesRunning; FGFDMExec *fdmex; FGPropertyManager *PropertyManager; @@ -682,8 +685,6 @@ private: bool Load_v1(void); bool Load_v2(void); - bool Constructing; - void InitializeIC(void); void SetEulerAngleRadIC(int idx, double angle); void SetBodyVelFpsIC(int idx, double vel); @@ -695,8 +696,6 @@ private: void calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED); void bind(void); void Debug(int from); - - string init_file_name; }; } #endif diff --git a/src/FDM/JSBSim/initialization/FGSimplexTrim.cpp b/src/FDM/JSBSim/initialization/FGSimplexTrim.cpp new file mode 100755 index 000000000..ec6ca6fc0 --- /dev/null +++ b/src/FDM/JSBSim/initialization/FGSimplexTrim.cpp @@ -0,0 +1,270 @@ +/* + * FGSimplexTrim.cpp + * Copyright (C) James Goppert 2010 + * + * FGSimplexTrim.cpp 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. + * + * FGSimplexTrim.cpp 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, see . + */ + +#include "FGSimplexTrim.h" +#include + +namespace JSBSim { + +FGSimplexTrim::FGSimplexTrim(FGFDMExec * fdm, TrimMode mode) +{ + std::clock_t time_start=clock(), time_trimDone, time_linDone; + + // variables + fdm->Setdt(1./120); + FGTrimmer::Constraints constraints; + + std::cout << "\n-----Performing Simplex Based Trim --------------\n" << std::endl; + + // defaults + constraints.velocity = fdm->GetAuxiliary()->GetVt(); + constraints.altitude = fdm->GetPropagate()->GetAltitudeASL(); + std::string aircraft = fdm->GetAircraft()->GetAircraftName(); + double rtol = fdm->GetPropertyManager()->GetDouble("trim/solver/rtol"); + double abstol = fdm->GetPropertyManager()->GetDouble("trim/solver/abstol"); + double speed = fdm->GetPropertyManager()->GetDouble("trim/solver/speed"); // must be > 1, 2 typical + double random = fdm->GetPropertyManager()->GetDouble("trim/solver/random"); + int iterMax = fdm->GetPropertyManager()->GetDouble("trim/solver/iterMax"); + bool showConvergeStatus = fdm->GetPropertyManager()->GetBool("trim/solver/showConvergeStatus"); + bool pause = fdm->GetPropertyManager()->GetBool("trim/solver/pause"); + bool showSimplex = fdm->GetPropertyManager()->GetBool("trim/solver/showSimplex"); + bool variablePropPitch = fdm->GetPropertyManager()->GetBool("trim/solver/variablePropPitch"); + //int debugLevel = fdm->GetPropertyManager()->GetInt("trim/solver/debugLevel"); + + std::string fileName = aircraft; + + // input + //std::cout << "input ( press enter to accept [default] )\n" << std::endl; + + // load model + std::string aircraftName = fdm->GetAircraft()->GetAircraftName(); + //prompt("\tdebug level\t\t",debugLevel); + //fdm->SetDebugLevel(debugLevel); + //std::cout << "model selection" << std::endl; + //while (1) + //{ + //prompt("\taircraft\t\t",aircraft); + //prompt("\toutput file name\t",fileName); + //fdm->LoadModel("../aircraft","../engine","../systems",aircraft); + //aircraftName = fdm->GetAircraft()->GetAircraftName(); + //if (aircraftName == "") + //{ + //std::cout << "\tfailed to load aircraft" << std::endl; + //} + //else + //{ + //std::cout << "\tsuccessfully loaded: " << aircraftName << std::endl; + //break; + //} + //} + + // Turn on propulsion system + fdm->GetPropulsion()->InitRunning(-1); + + // get propulsion pointer to determine type/ etc. + FGEngine * engine0 = fdm->GetPropulsion()->GetEngine(0); + FGThruster * thruster0 = engine0->GetThruster(); + + // flight conditions + //std::cout << "\nflight conditions: " << std::endl; + //prompt("\taltitude, ft\t\t",constraints.altitude); + //prompt("\tvelocity, ft/s\t\t",constraints.velocity); + //prompt("\tgamma, deg\t\t",constraints.gamma); constraints.gamma = constraints.gamma*M_PI/180; + + //double phi = fdm->GetPropagate()->GetEuler(1); + double theta = fdm->GetPropagate()->GetEuler(2); + //double psi = fdm->GetPropagate()->GetEuler(3); + + // TODO check that this works properly + constraints.gamma = theta; + + //if (thruster0->GetType()==FGThruster::ttPropeller) + //prompt("\tvariable prop pitch?\t\t",variablePropPitch); + // FIXME, enable + + constraints.rollRate = fdm->GetIC()->GetPRadpsIC(); + constraints.pitchRate = fdm->GetIC()->GetQRadpsIC(); + constraints.yawRate = fdm->GetIC()->GetRRadpsIC(); + constraints.stabAxisRoll = true; // FIXME, make this an option + + // solver properties + // TODO make these options + //std::cout << "\nsolver properties: " << std::endl; + //std::cout << std::scientific; + //prompt("\tshow converge status?\t",showConvergeStatus); + //prompt("\tshow simplex?\t\t",showSimplex); + //prompt("\tpause?\t\t\t",pause); + //prompt("\trelative tolerance\t",rtol); + //prompt("\tabsolute tolerance\t",abstol); + //prompt("\tmax iterations\t\t",iterMax); + //prompt("\tconvergence speed\t",speed); + //prompt("\trandomization ratio\t",random); + //std::cout << std::fixed; + + // initial solver state + int n = 6; + std::vector initialGuess(n), lowerBound(n), upperBound(n), initialStepSize(n); + + lowerBound[0] = fdm->GetPropertyManager()->GetDouble("trim/solver/throttleMin"); + lowerBound[1] = fdm->GetPropertyManager()->GetDouble("trim/solver/elevatorMin"); + lowerBound[2] = fdm->GetPropertyManager()->GetDouble("trim/solver/alphaMin"); + lowerBound[3] = fdm->GetPropertyManager()->GetDouble("trim/solver/aileronMin"); + lowerBound[4] = fdm->GetPropertyManager()->GetDouble("trim/solver/rudderMin"); + lowerBound[5] = fdm->GetPropertyManager()->GetDouble("trim/solver/betaMin"); + + upperBound[0] = fdm->GetPropertyManager()->GetDouble("trim/solver/throttleMax"); + upperBound[1] = fdm->GetPropertyManager()->GetDouble("trim/solver/elevatorMax"); + upperBound[2] = fdm->GetPropertyManager()->GetDouble("trim/solver/alphaMax"); + upperBound[3] = fdm->GetPropertyManager()->GetDouble("trim/solver/aileronMax"); + upperBound[4] = fdm->GetPropertyManager()->GetDouble("trim/solver/rudderMax"); + upperBound[5] = fdm->GetPropertyManager()->GetDouble("trim/solver/betaMax"); + + initialStepSize[0] = fdm->GetPropertyManager()->GetDouble("trim/solver/throttleStep"); + initialStepSize[1] = fdm->GetPropertyManager()->GetDouble("trim/solver/elevatorStep"); + initialStepSize[2] = fdm->GetPropertyManager()->GetDouble("trim/solver/alphaStep"); + initialStepSize[3] = fdm->GetPropertyManager()->GetDouble("trim/solver/aileronStep"); + initialStepSize[4] = fdm->GetPropertyManager()->GetDouble("trim/solver/rudderStep"); + initialStepSize[5] = fdm->GetPropertyManager()->GetDouble("trim/solver/betaStep"); + + initialGuess[0] = fdm->GetPropertyManager()->GetDouble("trim/solver/throttleGuess"); + initialGuess[1] = fdm->GetPropertyManager()->GetDouble("trim/solver/elevatorGuess"); + initialGuess[2] = fdm->GetPropertyManager()->GetDouble("trim/solver/alphaGuess"); + initialGuess[3] = fdm->GetPropertyManager()->GetDouble("trim/solver/aileronGuess"); + initialGuess[4] = fdm->GetPropertyManager()->GetDouble("trim/solver/rudderGuess"); + initialGuess[5] = fdm->GetPropertyManager()->GetDouble("trim/solver/betaGuess"); + + // solve + FGTrimmer trimmer(fdm, &constraints); + Callback callback(fileName,&trimmer); + FGNelderMead * solver = NULL; + try + { + solver = new FGNelderMead(&trimmer,initialGuess, + lowerBound, upperBound, initialStepSize,iterMax,rtol, + abstol,speed,random,showConvergeStatus,showSimplex,pause,&callback); + while(solver->status()==1) solver->update(); + } + catch (const std::runtime_error & e) + { + std::cout << e.what() << std::endl; + //exit(1); + } + + // output + try + { + trimmer.printSolution(std::cout,solver->getSolution()); // this also loads the solution into the fdm + std::cout << "\nfinal cost: " << std::scientific << std::setw(10) << trimmer.eval(solver->getSolution()) << std::endl; + } + catch(std::runtime_error & e) + { + std::cout << "caught std::runtime error" << std::endl; + std::cout << "exception: " << e.what() << std::endl; + exit(1); + } + + time_trimDone = std::clock(); + std::cout << "\ntrim computation time: " << (time_trimDone - time_start)/double(CLOCKS_PER_SEC) << "s \n" << std::endl; + + //std::cout << "\nsimulating flight to determine trim stability" << std::endl; + + //std::cout << "\nt = 5 seconds" << std::endl; + //for (int i=0;i<5*120;i++) fdm->Run(); + //trimmer.printState(); + + //std::cout << "\nt = 10 seconds" << std::endl; + //for (int i=0;i<5*120;i++) fdm->Run(); + //trimmer.printState(); + + std::cout << "\nlinearization: " << std::endl; + FGStateSpace ss(fdm); + + ss.x.add(new FGStateSpace::Vt); + ss.x.add(new FGStateSpace::Alpha); + ss.x.add(new FGStateSpace::Theta); + ss.x.add(new FGStateSpace::Q); + + if (thruster0->GetType()==FGThruster::ttPropeller) + { + ss.x.add(new FGStateSpace::Rpm0); + if (variablePropPitch) ss.x.add(new FGStateSpace::PropPitch); + int numEngines = fdm->GetPropulsion()->GetNumEngines(); + if (numEngines>1) ss.x.add(new FGStateSpace::Rpm1); + if (numEngines>2) ss.x.add(new FGStateSpace::Rpm2); + if (numEngines>3) ss.x.add(new FGStateSpace::Rpm3); + } + ss.x.add(new FGStateSpace::Beta); + ss.x.add(new FGStateSpace::Phi); + ss.x.add(new FGStateSpace::P); + ss.x.add(new FGStateSpace::Psi); + ss.x.add(new FGStateSpace::R); + ss.x.add(new FGStateSpace::Latitude); + ss.x.add(new FGStateSpace::Longitude); + ss.x.add(new FGStateSpace::Alt); + + ss.u.add(new FGStateSpace::ThrottleCmd); + ss.u.add(new FGStateSpace::DaCmd); + ss.u.add(new FGStateSpace::DeCmd); + ss.u.add(new FGStateSpace::DrCmd); + + // state feedback + ss.y = ss.x; + + std::vector< std::vector > A,B,C,D; + std::vector x0 = ss.x.get(), u0 = ss.u.get(); + std::vector y0 = x0; // state feedback + std::cout << ss << std::endl; + + ss.linearize(x0,u0,y0,A,B,C,D); + + int width=10; + std::cout.precision(3); + std::cout + << std::fixed + << std::right + << "\nA=\n" << std::setw(width) << A + << "\nB=\n" << std::setw(width) << B + << "\nC=\n" << std::setw(width) << C + << "\nD=\n" << std::setw(width) << D + << std::endl; + + // write scicoslab file + std::ofstream scicos(std::string(aircraft+"_lin.sce").c_str()); + scicos.precision(10); + width=20; + scicos + << std::scientific + << aircraft << ".x0=..\n" << std::setw(width) << x0 << ";\n" + << aircraft << ".u0=..\n" << std::setw(width) << u0 << ";\n" + << aircraft << ".sys = syslin('c',..\n" + << std::setw(width) << A << ",..\n" + << std::setw(width) << B << ",..\n" + << std::setw(width) << C << ",..\n" + << std::setw(width) << D << ");\n" + << aircraft << ".tfm = ss2tf(" << aircraft << ".sys);\n" + << std::endl; + + time_linDone = std::clock(); + std::cout << "\nlinearization computation time: " << (time_linDone - time_trimDone)/double(CLOCKS_PER_SEC) << " s\n" << std::endl; + + if (solver) delete solver; +} + +} // JSBSim + +// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/initialization/FGSimplexTrim.h b/src/FDM/JSBSim/initialization/FGSimplexTrim.h new file mode 100755 index 000000000..3ee0dd3f0 --- /dev/null +++ b/src/FDM/JSBSim/initialization/FGSimplexTrim.h @@ -0,0 +1,81 @@ +/* + * FGSimplexTrim.h + * Copyright (C) James Goppert 2010 + * + * FGSimplexTrim.h 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. + * + * FGSimplexTrim.h 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, see . + */ + +#ifndef FGSimplexTrim_H_ +#define FGSimplexTrim_H_ + +#include "initialization/FGTrimmer.h" +#include "math/FGStateSpace.h" +#include +#include +#include "models/FGAircraft.h" +#include "models/propulsion/FGEngine.h" +#include "models/propulsion/FGTurbine.h" +#include "models/propulsion/FGTurboProp.h" +#include "math/FGNelderMead.h" +#include +#include +#include + +namespace JSBSim { + +class FGSimplexTrim +{ +public: + FGSimplexTrim(FGFDMExec * fdmPtr, TrimMode mode); +private: + template + void prompt(const std::string & str, varType & var) + { + std::cout << str + " [" << std::setw(10) << var << "]\t: "; + if (std::cin.peek() != '\n') + { + std::cin >> var; + std::cin.ignore(1000, '\n'); + } + else std::cin.get(); + } + + class Callback : public JSBSim::FGNelderMead::Callback + { + private: + std::ofstream _outputFile; + JSBSim::FGTrimmer * _trimmer; + public: + Callback(std::string fileName, JSBSim::FGTrimmer * trimmer) : + _outputFile((fileName + std::string("_simplexTrim.log")).c_str()), + _trimmer(trimmer) { + } + virtual ~Callback() { + _outputFile.close(); + } + void eval(const std::vector &v) + { + _outputFile << _trimmer->eval(v) << std::endl;; + //std::cout << "v: "; + //for (int i=0;iDisableOutput(); fdmex->SetTrimStatus(true); + fdmex->SuspendIntegration(); fgic->SetPRadpsIC(0.0); fgic->SetQRadpsIC(0.0); @@ -350,6 +351,7 @@ bool FGTrim::DoTrim(void) { fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true); } fdmex->SetTrimStatus(false); + fdmex->ResumeIntegration(); fdmex->EnableOutput(); return !trim_failed; } diff --git a/src/FDM/JSBSim/initialization/FGTrimAxis.cpp b/src/FDM/JSBSim/initialization/FGTrimAxis.cpp index d2baa66df..9d8ee207a 100644 --- a/src/FDM/JSBSim/initialization/FGTrimAxis.cpp +++ b/src/FDM/JSBSim/initialization/FGTrimAxis.cpp @@ -56,7 +56,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGTrimAxis.cpp,v 1.13 2011/07/28 12:48:19 jberndt Exp $"; +static const char *IdSrc = "$Id: FGTrimAxis.cpp,v 1.14 2012/09/05 21:49:19 bcoconni Exp $"; static const char *IdHdr = ID_TRIMAXIS; /*****************************************************************************/ @@ -337,7 +337,8 @@ bool FGTrimAxis::initTheta(void) { while(!level && (i < 100)) { theta+=radtodeg*atan(zDiff/xDiff); fgic->SetThetaDegIC(theta); - fdmex->RunIC(); + fdmex->Initialize(fgic); + fdmex->Run(); zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3); zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3); zDiff = zForward - zAft; @@ -404,7 +405,8 @@ void FGTrimAxis::Run(void) { while(!stable) { i++; last_state_value=state_value; - fdmex->RunIC(); + fdmex->Initialize(fgic); + fdmex->Run(); getState(); if(i > 1) { if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) ) @@ -430,7 +432,8 @@ void FGTrimAxis::setThrottlesPct(void) { fdmex->GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin)); fdmex->GetPropulsion()->in.ThrottlePos[i] = tMin +control_value*(tMax - tMin); - fdmex->RunIC(); //apply throttle change + fdmex->Initialize(fgic); + fdmex->Run(); //apply throttle change fdmex->GetPropulsion()->GetSteadyState(); } } diff --git a/src/FDM/JSBSim/initialization/FGTrimmer.cpp b/src/FDM/JSBSim/initialization/FGTrimmer.cpp new file mode 100755 index 000000000..3cc77bad4 --- /dev/null +++ b/src/FDM/JSBSim/initialization/FGTrimmer.cpp @@ -0,0 +1,423 @@ +/* + * FGTrimmer.cpp + * Copyright (C) James Goppert 2010 + * + * FGTrimmer.cpp 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. + * + * FGTrimmer.cpp 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, see . + */ + +#include "FGTrimmer.h" +#include "models/FGFCS.h" +#include "models/FGPropulsion.h" +#include "models/FGAccelerations.h" +#include "models/propulsion/FGEngine.h" +#include "models/propulsion/FGThruster.h" +#include "models/propulsion/FGTank.h" +#include "models/FGMassBalance.h" +#include "models/FGAuxiliary.h" +#include "models/FGAircraft.h" +#include +#include +#include +#include "simgear/misc/stdint.hxx" + +namespace JSBSim +{ + +FGTrimmer::FGTrimmer(FGFDMExec * fdm, Constraints * constraints) : + m_fdm(fdm), m_constraints(constraints) +{ +} + +std::vector FGTrimmer::constrain(const std::vector & v) +{ + // unpack design vector + double throttle = v[0]; + double elevator = v[1]; + double alpha = v[2]; + double aileron = v[3]; + double rudder = v[4]; + double beta = v[5]; + + // initialize constraints + double vt = m_constraints->velocity; + double altitude = m_constraints->altitude; + double phi = 0.0, theta = 0.0, psi = 0.0*M_PI/180.0; + double p = 0.0, q = 0.0, r= 0.0; + + // precomputation + double sGam = sin(m_constraints->gamma); + double sBeta = sin(beta); + double cBeta = cos(beta); + double tAlpha = tan(alpha); + double cAlpha = cos(alpha); + + // rate of climb constraint + double a = cAlpha*cBeta; + double b = sin(phi)*sBeta+cos(phi)*sin(alpha)*cBeta; + theta = atan((a*b+sGam*sqrt(a*a-sGam*sGam+b*b))/(a*a-sGam*sGam)); + + // turn coordination constraint, lewis pg. 190 + double gd = m_fdm->GetInertial()->gravity(); + double gc = m_constraints->yawRate*vt/gd; + a = 1 - gc*tAlpha*sBeta; + b = sGam/cBeta; + double c = 1 + gc*gc*cBeta*cBeta; + phi = atan((gc*cBeta*((a-b*b)+ + b*tAlpha*sqrt(c*(1-b*b)+gc*gc*sBeta*sBeta)))/ + (cAlpha*(a*a-b*b*(1+c*tAlpha*tAlpha)))); + + // turn rates + if (m_constraints->rollRate != 0.0) // rolling + { + p = m_constraints->rollRate; + q = 0.0; + if (m_constraints->stabAxisRoll) // stability axis roll + { + r = m_constraints->rollRate*tan(alpha); + } + else // body axis roll + { + r = m_constraints->rollRate; + } + } + else if (m_constraints->yawRate != 0.0) // yawing + { + p = -m_constraints->yawRate*sin(theta); + q = m_constraints->yawRate*sin(phi)*cos(theta); + r = m_constraints->yawRate*cos(phi)*cos(theta); + } + else if (m_constraints->pitchRate != 0.0) // pitching + { + p = 0.0; + q = m_constraints->pitchRate; + r = 0.0; + } + + // state + m_fdm->GetIC()->SetVtrueFpsIC(vt); + m_fdm->GetIC()->SetAlphaRadIC(alpha); + m_fdm->GetIC()->SetThetaRadIC(theta); + m_fdm->GetIC()->SetFlightPathAngleRadIC(m_constraints->gamma); + m_fdm->GetIC()->SetQRadpsIC(q); + // thrust handled below + m_fdm->GetIC()->SetBetaRadIC(beta); + m_fdm->GetIC()->SetPhiRadIC(phi); + m_fdm->GetIC()->SetPRadpsIC(p); + m_fdm->GetIC()->SetRRadpsIC(r); + + // actuator states handled below + + // nav state + m_fdm->GetIC()->SetAltitudeASLFtIC(altitude); + m_fdm->GetIC()->SetPsiRadIC(psi); + //m_fdm->GetIC()->SetLatitudeRadIC(0); + //m_fdm->GetIC()->SetLongitudeRadIC(0); + + // apply state + m_fdm->RunIC(); + + // set controls + m_fdm->GetFCS()->SetDeCmd(elevator); + m_fdm->GetFCS()->SetDePos(ofNorm,elevator); + + m_fdm->GetFCS()->SetDaCmd(aileron); + m_fdm->GetFCS()->SetDaLPos(ofNorm,aileron); + m_fdm->GetFCS()->SetDaRPos(ofNorm,aileron); + + m_fdm->GetFCS()->SetDrPos(ofNorm,rudder); + m_fdm->GetFCS()->SetDrCmd(rudder); + + for (unsigned int i=0; iGetPropulsion()->GetNumEngines(); i++) + { + //FGEngine * engine = m_fdm->GetPropulsion()->GetEngine(i); + m_fdm->GetPropulsion()->GetEngine(i)->InitRunning(); + m_fdm->GetFCS()->SetThrottleCmd(i,throttle); + m_fdm->GetFCS()->SetThrottlePos(i,throttle); + } + + + // wait for steady-state + //double thrust0 = m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust(); + //double dThrustMag0 = 0; + //for(int i=0;;i++) { + //m_fdm->RunIC(); + //m_fdm->Run(); + //double thrust = m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust(); + //double dThrustMag = std::abs(thrust - thrust0); + //double d2Thrust = dThrustMag - dThrustMag0; + //thrust0= thrust; + //dThrustMag0 = dThrustMag; + //if (d2Thrust < std::numeric_limits::epsilon() ) { + //// thrust difference has converged to minimum + //// if d2Thrust > 0 clearly, more interations won't help + //// so not using abs(d2Thrust) + //break; + //} else if (i> 1000) { + //std::cout << "thrust failed to converge" << std::endl; + //std::cout << "difference: " << dThrustMag << std::endl; + //throw std::runtime_error("thrust failed to converge"); + //break; + //} + //} + //m_fdm->RunIC(); + + std::vector data; + data.push_back(phi); + data.push_back(theta); + return data; +} + +void FGTrimmer::printSolution(std::ostream & stream, const std::vector & v) +{ + eval(v); + + double dt = m_fdm->GetDeltaT(); + double thrust = m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust(); + double elevator = m_fdm->GetFCS()->GetDePos(ofNorm); + double aileron = m_fdm->GetFCS()->GetDaLPos(ofNorm); + double rudder = m_fdm->GetFCS()->GetDrPos(ofNorm); + double throttle = m_fdm->GetFCS()->GetThrottlePos(0); + double lat = m_fdm->GetPropagate()->GetLatitudeDeg(); + double lon = m_fdm->GetPropagate()->GetLongitudeDeg(); + double vt = m_fdm->GetAuxiliary()->GetVt(); + + // run a step to compute derivatives + //for (int i=0;i<10000;i++) { + //while (old - m_fdm->GetPropulsion()->GetEngine(i)->CalcFuelNeed() < 1e-5) { + //m_fdm->RunIC(); + //m_fdm->Run(); + //} + //} + + double dthrust = (m_fdm->GetPropulsion()->GetEngine(0)-> + GetThruster()->GetThrust()-thrust)/dt; + double delevator = (m_fdm->GetFCS()->GetDePos(ofNorm)-elevator)/dt; + double daileron = (m_fdm->GetFCS()->GetDaLPos(ofNorm)-aileron)/dt; + double drudder = (m_fdm->GetFCS()->GetDrPos(ofNorm)-rudder)/dt; + double dthrottle = (m_fdm->GetFCS()->GetThrottlePos(0)-throttle)/dt; + double dlat = (m_fdm->GetPropagate()->GetLatitudeDeg()-lat)/dt; + double dlon = (m_fdm->GetPropagate()->GetLongitudeDeg()-lon)/dt; + double dvt = (m_fdm->GetAuxiliary()->GetVt()-vt)/dt; + + // reinitialize with correct state + eval(v); + + // state + stream << std::setw(10) + + // aircraft state + << "\naircraft state" + << "\n\tvt\t\t:\t" << vt + << "\n\talpha, deg\t:\t" << m_fdm->GetIC()->GetAlphaDegIC() + << "\n\ttheta, deg\t:\t" << m_fdm->GetIC()->GetThetaDegIC() + << "\n\tq, rad/s\t:\t" << m_fdm->GetIC()->GetQRadpsIC() + << "\n\tthrust, lbf\t:\t" << m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust() + << "\n\tbeta, deg\t:\t" << m_fdm->GetIC()->GetBetaDegIC() + << "\n\tphi, deg\t:\t" << m_fdm->GetIC()->GetPhiDegIC() + << "\n\tp, rad/s\t:\t" << m_fdm->GetIC()->GetPRadpsIC() + << "\n\tr, rad/s\t:\t" << m_fdm->GetIC()->GetRRadpsIC() + << "\n\tmass (lbm)\t:\t" << m_fdm->GetMassBalance()->GetWeight() + + // actuator states + << "\n\nactuator state" + << "\n\tthrottle, %\t:\t" << 100*throttle + << "\n\televator, %\t:\t" << 100*elevator + << "\n\taileron, %\t:\t" << 100*aileron + << "\n\trudder, %\t:\t" << 100*rudder + + // nav state + << "\n\nnav state" + << "\n\taltitude, ft\t:\t" << m_fdm->GetIC()->GetAltitudeASLFtIC() + << "\n\tpsi, deg\t:\t" << m_fdm->GetIC()->GetPsiDegIC() + << "\n\tlat, deg\t:\t" << lat + << "\n\tlon, deg\t:\t" << lon + + // d/dt aircraft state + << "\n\naircraft d/dt state" + << std::scientific + + << "\n\td/dt vt\t\t\t:\t" << dvt + << "\n\td/dt alpha, deg/s\t:\t" << m_fdm->GetAuxiliary()->Getadot()*180/M_PI + << "\n\td/dt theta, deg/s\t:\t" << m_fdm->GetAuxiliary()->GetEulerRates(2)*180/M_PI + << "\n\td/dt q, rad/s^2\t\t:\t" << m_fdm->GetAccelerations()->GetPQRdot(2) + << "\n\td/dt thrust, lbf\t:\t" << dthrust + << "\n\td/dt beta, deg/s\t:\t" << m_fdm->GetAuxiliary()->Getbdot()*180/M_PI + << "\n\td/dt phi, deg/s\t\t:\t" << m_fdm->GetAuxiliary()->GetEulerRates(1)*180/M_PI + << "\n\td/dt p, rad/s^2\t\t:\t" << m_fdm->GetAccelerations()->GetPQRdot(1) + << "\n\td/dt r, rad/s^2\t\t:\t" << m_fdm->GetAccelerations()->GetPQRdot(3) + + // d/dt actuator states + << "\n\nd/dt actuator state" + << "\n\td/dt throttle, %/s\t:\t" << dthrottle + << "\n\td/dt elevator, %/s\t:\t" << delevator + << "\n\td/dt aileron, %/s\t:\t" << daileron + << "\n\td/dt rudder, %/s\t:\t" << drudder + + // nav state + << "\n\nd/dt nav state" + << "\n\td/dt altitude, ft/s\t:\t" << m_fdm->GetPropagate()->Gethdot() + << "\n\td/dt psi, deg/s\t\t:\t" << m_fdm->GetAuxiliary()->GetEulerRates(3) + << "\n\td/dt lat, deg/s\t\t:\t" << dlat + << "\n\td/dt lon, deg/s\t\t:\t" << dlon + << std::fixed + + << "\n\npropulsion system state" + << std::scientific << std::setw(10); + + for (unsigned int i=0;iGetPropulsion()->GetNumTanks();i++) { + stream + << "\n\ttank " << i << ": fuel (lbm)\t\t\t:\t" + << m_fdm->GetPropulsion()->GetTank(i)->GetContents(); + } + + for (unsigned int i=0;iGetPropulsion()->GetNumEngines();i++) { + m_fdm->GetPropulsion()->GetEngine(i)->CalcFuelNeed(); + stream + << "\n\tengine " << i + << "\n\t\tfuel flow rate (lbm/s)\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetFuelFlowRate() + << "\n\t\tfuel flow rate (gph)\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetFuelFlowRateGPH() + << "\n\t\tstarved\t\t\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetStarved() + << "\n\t\trunning\t\t\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetRunning() + << std::endl; + } +} + +void FGTrimmer::printState(std::ostream & stream) +{ + // state + stream << std::setw(10) + + // interval method comparison + //<< "\n\ninterval method comparison" + //<< "\nAngle of Attack: \t:\t" << m_fdm->GetAuxiliary()->Getalpha(ofDeg) << "\twdot: " << m_fdm->GetAccelerations()->GetUVWdot(3) + //<< "\nThrottle: \t:\t" << 100*m_fdm->GetFCS()->GetThrottlePos(0) << "\tudot: " << m_fdm->GetAccelerations()->GetUVWdot(1) + //<< "\nPitch Trim: \t:\t" << 100*m_fdm->GetFCS()->GetDePos(ofNorm) << "\tqdot: " << m_fdm->GetAccelerations()->GetPQRdot(2) + //<< "\nSideslip: \t:\t" << m_fdm->GetAuxiliary()->Getbeta(ofDeg) << "\tvdot: " << m_fdm->GetAccelerations()->GetUVWdot(2) + //<< "\nAilerons: \t:\t" << 100*m_fdm->GetFCS()->GetDaLPos(ofNorm) << "\tpdot: " << m_fdm->GetAccelerations()->GetPQRdot(1) + //<< "\nRudder: \t:\t" << 100*m_fdm->GetFCS()->GetDrPos(ofNorm) << "\trdot: " << m_fdm->GetAccelerations()->GetPQRdot(3) + + << "\n\naircraft state" + << "\nvt\t\t:\t" << m_fdm->GetAuxiliary()->GetVt() + << "\nalpha, deg\t:\t" << m_fdm->GetAuxiliary()->Getalpha(ofDeg) + << "\ntheta, deg\t:\t" << m_fdm->GetPropagate()->GetEuler(2)*180/M_PI + << "\nq, rad/s\t:\t" << m_fdm->GetPropagate()->GetPQR(2) + << "\nthrust, lbf\t:\t" << m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust() + << "\nbeta, deg\t:\t" << m_fdm->GetAuxiliary()->Getbeta(ofDeg) + << "\nphi, deg\t:\t" << m_fdm->GetPropagate()->GetEuler(1)*180/M_PI + << "\np, rad/s\t:\t" << m_fdm->GetPropagate()->GetPQR(1) + << "\nr, rad/s\t:\t" << m_fdm->GetPropagate()->GetPQR(3) + + // actuator states + << "\n\nactuator state" + << "\nthrottle, %\t:\t" << 100*m_fdm->GetFCS()->GetThrottlePos(0) + << "\nelevator, %\t:\t" << 100*m_fdm->GetFCS()->GetDePos(ofNorm) + << "\naileron, %\t:\t" << 100*m_fdm->GetFCS()->GetDaLPos(ofNorm) + << "\nrudder, %\t:\t" << 100*m_fdm->GetFCS()->GetDrPos(ofNorm) + + // nav state + << "\n\nnav state" + << "\naltitude, ft\t:\t" << m_fdm->GetPropagate()->GetAltitudeASL() + << "\npsi, deg\t:\t" << m_fdm->GetPropagate()->GetEuler(3)*180/M_PI + << "\nlat, deg\t:\t" << m_fdm->GetPropagate()->GetLatitudeDeg() + << "\nlon, deg\t:\t" << m_fdm->GetPropagate()->GetLongitudeDeg() + + // input + << "\n\ninput" + << "\nthrottle cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetThrottleCmd(0) + << "\nelevator cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetDeCmd() + << "\naileron cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetDaCmd() + << "\nrudder cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetDrCmd() + + << std::endl; + +} + +double FGTrimmer::eval(const std::vector & v) +{ + double cost = 0; + double cost0 = -1; + + double dvt=0; + double dalpha = 0; + double dbeta = 0; + double dp = 0; + double dq = 0; + double dr = 0; + + double dvt0 = 0; + double dalpha0 = 0; + double dbeta0 = 0; + double dp0 = 0; + double dq0 = 0; + double dr0 = 0; + + uint16_t steadyCount = 0; + + for(int i=0;;i++) { + constrain(v); + + dvt = (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetAccelerations()->GetUVWdot(1) + + m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetAccelerations()->GetUVWdot(2) + + m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetAccelerations()->GetUVWdot(3))/ + m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot + dalpha = m_fdm->GetAuxiliary()->Getadot(); + dbeta = m_fdm->GetAuxiliary()->Getbdot(); + dp = m_fdm->GetAccelerations()->GetPQRdot(1); + dq = m_fdm->GetAccelerations()->GetPQRdot(2); + dr = m_fdm->GetAccelerations()->GetPQRdot(3); + + if(m_fdm->GetDebugLevel() > 1) { + std::cout + << "dvt: " << dvt + << "\tdalpha: " << dalpha + << "\tdbeta: " << dbeta + << "\tdp: " << dp + << "\tdq: " << dq + << "\tdr: " << dr + << std::endl; + } + + cost = dvt*dvt + + 100.0*(dalpha*dalpha + dbeta*dbeta) + + 10.0*(dp*dp + dq*dq + dr*dr); + + double deltaCost = std::abs(cost - cost0); + cost0 = cost; + dvt0 = dvt; + dalpha0 = dalpha; + dbeta0 = dbeta; + dp0 = dp; + dq0 = dq; + dr0 = dr; + + if (deltaCost < 1e-10) { + if (steadyCount++ > 3) break; + } else if (i> 1000) { + std::cout << "deltaCost: " << std::scientific << std::setw(10) + << deltaCost << std::endl; + break; + //throw std::runtime_error("cost failed to converge"); + } else { + steadyCount=0; + } + + } + + return cost; +} + +} // JSBSim + + +// vim:ts=4:sw=4:expandtab diff --git a/src/FDM/JSBSim/initialization/FGTrimmer.h b/src/FDM/JSBSim/initialization/FGTrimmer.h new file mode 100755 index 000000000..3cca7f320 --- /dev/null +++ b/src/FDM/JSBSim/initialization/FGTrimmer.h @@ -0,0 +1,64 @@ +/* + * FGTrimmer.h + * Copyright (C) James Goppert 2010 + * + * FGTrimmer.h 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. + * + * FGTrimmer.h 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, see . + */ + +#ifndef JSBSim_FGTrimmer_H +#define JSBSim_FGTrimmer_H + +#include "math/FGNelderMead.h" +#include "FGFDMExec.h" +#include "models/FGInertial.h" + +namespace JSBSim +{ + +class FGTrimmer : public FGNelderMead::Function +{ +public: + struct Constraints + { + Constraints() : + velocity(100), altitude(1000), gamma(0), + rollRate(0), pitchRate(0), yawRate(0), + coordinatedTurn(true), stabAxisRoll(true) + { + } + double velocity, altitude, gamma; + double rollRate, pitchRate, yawRate; + bool coordinatedTurn, stabAxisRoll; + }; + FGTrimmer(FGFDMExec * fdm, Constraints * constraints); + std::vector constrain(const vector & v); + void printSolution(std::ostream & stream, const vector & v); + void printState(std::ostream & stream); + double eval(const vector & v); + static void limit(double min, double max, double &val) + { + if (valmax) val=max; + } + void setFdm(FGFDMExec * fdm) {m_fdm = fdm; } +private: + FGFDMExec * m_fdm; + Constraints * m_constraints; +}; + +} // JSBSim + +#endif + +// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/input_output/FGOutputFG.cpp b/src/FDM/JSBSim/input_output/FGOutputFG.cpp new file mode 100644 index 000000000..873e3dcbd --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputFG.cpp @@ -0,0 +1,352 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGOutputFG.cpp + Author: Bertrand Coconnier + Date started: 09/10/11 + Purpose: Manage output of sim parameters to FlightGear + Called by: FGOutput + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This is the place where you create output routines to dump data for perusal +later. + +HISTORY +-------------------------------------------------------------------------------- +11/09/07 HDW Added FlightGear Socket Interface +09/10/11 BC Moved the FlightGear socket in a separate class + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include +#include + +#include "FGOutputFG.h" +#include "FGFDMExec.h" +#include "models/FGAerodynamics.h" +#include "models/FGAuxiliary.h" +#include "models/FGPropulsion.h" +#include "models/FGMassBalance.h" +#include "models/FGPropagate.h" +#include "models/FGGroundReactions.h" +#include "models/FGFCS.h" +#include "models/propulsion/FGPiston.h" +#include "models/propulsion/FGTank.h" + +#if defined(WIN32) && !defined(__CYGWIN__) +# include +#else +# include // htonl() ntohl() +#endif + +#if !defined (min) +# define min(X,Y) Xversion = FG_NET_FDM_VERSION; + + // Positions + net->longitude = Propagate->GetLocation().GetLongitude(); // geodetic (radians) + net->latitude = Propagate->GetLocation().GetLatitude(); // geodetic (radians) + net->altitude = Propagate->GetAltitudeASL()*0.3048; // altitude, above sea level (meters) + net->agl = (float)(Propagate->GetDistanceAGL()*0.3048); // altitude, above ground level (meters) + + net->phi = (float)(Propagate->GetEuler(ePhi)); // roll (radians) + net->theta = (float)(Propagate->GetEuler(eTht)); // pitch (radians) + net->psi = (float)(Propagate->GetEuler(ePsi)); // yaw or true heading (radians) + + net->alpha = (float)(Auxiliary->Getalpha()); // angle of attack (radians) + net->beta = (float)(Auxiliary->Getbeta()); // side slip angle (radians) + + // Velocities + net->phidot = (float)(Auxiliary->GetEulerRates(ePhi)); // roll rate (radians/sec) + net->thetadot = (float)(Auxiliary->GetEulerRates(eTht)); // pitch rate (radians/sec) + net->psidot = (float)(Auxiliary->GetEulerRates(ePsi)); // yaw rate (radians/sec) + net->vcas = (float)(Auxiliary->GetVcalibratedKTS()); // VCAS, knots + net->climb_rate = (float)(Propagate->Gethdot()); // altitude rate, ft/sec + net->v_north = (float)(Propagate->GetVel(eNorth)); // north vel in NED frame, fps + net->v_east = (float)(Propagate->GetVel(eEast)); // east vel in NED frame, fps + net->v_down = (float)(Propagate->GetVel(eDown)); // down vel in NED frame, fps +//---ADD METHOD TO CALCULATE THESE TERMS--- + net->v_wind_body_north = (float)(Propagate->GetVel(eNorth)); // north vel in NED relative to airmass, fps + net->v_wind_body_east = (float)(Propagate->GetVel(eEast)); // east vel in NED relative to airmass, fps + net->v_wind_body_down = (float)(Propagate->GetVel(eDown)); // down vel in NED relative to airmass, fps + + // Accelerations + net->A_X_pilot = (float)(Auxiliary->GetPilotAccel(1)); // X body accel, ft/s/s + net->A_Y_pilot = (float)(Auxiliary->GetPilotAccel(2)); // Y body accel, ft/s/s + net->A_Z_pilot = (float)(Auxiliary->GetPilotAccel(3)); // Z body accel, ft/s/s + + // Stall + net->stall_warning = 0.0; // 0.0 - 1.0 indicating the amount of stall + net->slip_deg = (float)(Auxiliary->Getbeta(inDegrees)); // slip ball deflection, deg + + // Engine status + if (Propulsion->GetNumEngines() > FGNetFDM::FG_MAX_ENGINES && FDMExec->GetSimTime() == 0.0) + cerr << "This vehicle has " << Propulsion->GetNumEngines() << " engines, but the current " << endl + << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_ENGINES << " engines." << endl + << "Only the first " << FGNetFDM::FG_MAX_ENGINES << " engines will be used." << endl; + + net->num_engines = min(FGNetFDM::FG_MAX_ENGINES,Propulsion->GetNumEngines()); // Number of valid engines + + for (i=0; inum_engines; i++) { + if (Propulsion->GetEngine(i)->GetRunning()) + net->eng_state[i] = 2; // Engine state running + else if (Propulsion->GetEngine(i)->GetCranking()) + net->eng_state[i] = 1; // Engine state cranking + else + net->eng_state[i] = 0; // Engine state off + + switch (Propulsion->GetEngine(i)->GetType()) { + case (FGEngine::etRocket): + break; + case (FGEngine::etPiston): + net->rpm[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getRPM()); + net->fuel_flow[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getFuelFlow_gph()); + net->fuel_px[i] = 0; // Fuel pressure, psi (N/A in current model) + net->egt[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->GetEGT()); + net->cht[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getCylinderHeadTemp_degF()); + net->mp_osi[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getManifoldPressure_inHg()); + net->oil_temp[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getOilTemp_degF()); + net->oil_px[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getOilPressure_psi()); + net->tit[i] = 0; // Turbine Inlet Temperature (N/A for piston) + break; + case (FGEngine::etTurbine): + break; + case (FGEngine::etTurboprop): + break; + case (FGEngine::etElectric): + break; + case (FGEngine::etUnknown): + break; + } + } + + // Consumables + if (Propulsion->GetNumTanks() > FGNetFDM::FG_MAX_TANKS && FDMExec->GetSimTime() == 0.0) + cerr << "This vehicle has " << Propulsion->GetNumTanks() << " tanks, but the current " << endl + << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_TANKS << " tanks." << endl + << "Only the first " << FGNetFDM::FG_MAX_TANKS << " tanks will be used." << endl; + + net->num_tanks = min(FGNetFDM::FG_MAX_TANKS, Propulsion->GetNumTanks()); // Max number of fuel tanks + + for (i=0; inum_tanks; i++) { + net->fuel_quantity[i] = (float)(((FGTank *)Propulsion->GetTank(i))->GetContents()); + } + + // Gear status + if (GroundReactions->GetNumGearUnits() > FGNetFDM::FG_MAX_WHEELS && FDMExec->GetSimTime() == 0.0) + cerr << "This vehicle has " << GroundReactions->GetNumGearUnits() << " bogeys, but the current " << endl + << "version of FlightGear's FGNetFDM only supports " << FGNetFDM::FG_MAX_WHEELS << " bogeys." << endl + << "Only the first " << FGNetFDM::FG_MAX_WHEELS << " bogeys will be used." << endl; + + net->num_wheels = min(FGNetFDM::FG_MAX_WHEELS, GroundReactions->GetNumGearUnits()); + + for (i=0; inum_wheels; i++) { + net->wow[i] = GroundReactions->GetGearUnit(i)->GetWOW(); + if (GroundReactions->GetGearUnit(i)->GetGearUnitDown()) + net->gear_pos[i] = 1; //gear down, using FCS convention + else + net->gear_pos[i] = 0; //gear up, using FCS convention + net->gear_steer[i] = (float)(GroundReactions->GetGearUnit(i)->GetSteerNorm()); + net->gear_compression[i] = (float)(GroundReactions->GetGearUnit(i)->GetCompLen()); + } + + // Environment + net->cur_time = (long int)1234567890; // Friday, Feb 13, 2009, 23:31:30 UTC (not processed by FGFS anyway) + net->warp = 0; // offset in seconds to unix time + net->visibility = 25000.0; // visibility in meters (for env. effects) + + // Control surface positions (normalized values) + net->elevator = (float)(FCS->GetDePos(ofNorm)); // Norm Elevator Pos, -- + net->elevator_trim_tab = (float)(FCS->GetPitchTrimCmd()); // Norm Elev Trim Tab Pos, -- + net->left_flap = (float)(FCS->GetDfPos(ofNorm)); // Norm Flap Pos, -- + net->right_flap = (float)(FCS->GetDfPos(ofNorm)); // Norm Flap Pos, -- + net->left_aileron = (float)(FCS->GetDaLPos(ofNorm)); // Norm L Aileron Pos, -- + net->right_aileron = (float)(FCS->GetDaRPos(ofNorm)); // Norm R Aileron Pos, -- + net->rudder = (float)(FCS->GetDrPos(ofNorm)); // Norm Rudder Pos, -- + net->nose_wheel = (float)(FCS->GetDrPos(ofNorm)); // *** FIX *** Using Rudder Pos for NWS, -- + net->speedbrake = (float)(FCS->GetDsbPos(ofNorm)); // Norm Speedbrake Pos, -- + net->spoilers = (float)(FCS->GetDspPos(ofNorm)); // Norm Spoiler Pos, -- + + // Convert the net buffer to network format + if ( isLittleEndian ) { + net->version = htonl(net->version); + + htond(net->longitude); + htond(net->latitude); + htond(net->altitude); + htonf(net->agl); + htonf(net->phi); + htonf(net->theta); + htonf(net->psi); + htonf(net->alpha); + htonf(net->beta); + + htonf(net->phidot); + htonf(net->thetadot); + htonf(net->psidot); + htonf(net->vcas); + htonf(net->climb_rate); + htonf(net->v_north); + htonf(net->v_east); + htonf(net->v_down); + htonf(net->v_wind_body_north); + htonf(net->v_wind_body_east); + htonf(net->v_wind_body_down); + + htonf(net->A_X_pilot); + htonf(net->A_Y_pilot); + htonf(net->A_Z_pilot); + + htonf(net->stall_warning); + htonf(net->slip_deg); + + for (i=0; inum_engines; ++i ) { + net->eng_state[i] = htonl(net->eng_state[i]); + htonf(net->rpm[i]); + htonf(net->fuel_flow[i]); + htonf(net->fuel_px[i]); + htonf(net->egt[i]); + htonf(net->cht[i]); + htonf(net->mp_osi[i]); + htonf(net->tit[i]); + htonf(net->oil_temp[i]); + htonf(net->oil_px[i]); + } + net->num_engines = htonl(net->num_engines); + + for (i=0; inum_tanks; ++i ) { + htonf(net->fuel_quantity[i]); + } + net->num_tanks = htonl(net->num_tanks); + + for (i=0; inum_wheels; ++i ) { + net->wow[i] = htonl(net->wow[i]); + htonf(net->gear_pos[i]); + htonf(net->gear_steer[i]); + htonf(net->gear_compression[i]); + } + net->num_wheels = htonl(net->num_wheels); + + net->cur_time = htonl( net->cur_time ); + net->warp = htonl( net->warp ); + htonf(net->visibility); + + htonf(net->elevator); + htonf(net->elevator_trim_tab); + htonf(net->left_flap); + htonf(net->right_flap); + htonf(net->left_aileron); + htonf(net->right_aileron); + htonf(net->rudder); + htonf(net->nose_wheel); + htonf(net->speedbrake); + htonf(net->spoilers); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputFG::Print(void) +{ + int length = sizeof(fgSockBuf); + + if (socket == 0) return; + if (!socket->GetConnectStatus()) return; + + SocketDataFill(&fgSockBuf); + socket->Send((char *)&fgSockBuf, length); +} +} diff --git a/src/FDM/JSBSim/input_output/FGOutputFG.h b/src/FDM/JSBSim/input_output/FGOutputFG.h new file mode 100644 index 000000000..1fb195414 --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputFG.h @@ -0,0 +1,87 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGOutputFG.h + Author: Bertrand Coconnier + Date started: 09/10/11 + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +11/09/07 HDW Added FlightGear Socket Interface +09/10/11 BC Moved the FlightGear socket in a separate class + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGOUTPUTFG_H +#define FGOUTPUTFG_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGOutputSocket.h" +#include "input_output/net_fdm.hxx" +#include "input_output/FGfdmSocket.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_OUTPUTFG "$Id: FGOutputFG.h,v 1.3 2013/01/12 18:08:40 bcoconni Exp $" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** Implements the output to a FlightGear socket. + */ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGOutputFG : public FGOutputSocket +{ +public: + /// Constructor + FGOutputFG(FGFDMExec* fdmex); + + virtual void Print(void); + +protected: + virtual void PrintHeaders(void) {}; + +private: + FGNetFDM fgSockBuf; + void SocketDataFill(FGNetFDM* net); +}; +} +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif + diff --git a/src/FDM/JSBSim/input_output/FGOutputFile.cpp b/src/FDM/JSBSim/input_output/FGOutputFile.cpp new file mode 100644 index 000000000..700bee6c4 --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputFile.cpp @@ -0,0 +1,101 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGOutputFile.cpp + Author: Bertrand Coconnier + Date started: 09/10/11 + Purpose: Manage output of sim parameters to a file + Called by: FGOutput + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This is the place where you create output routines to dump data for perusal +later. + +HISTORY +-------------------------------------------------------------------------------- +09/10/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include + +#include "FGOutputFile.h" + +using namespace std; + +namespace JSBSim { + +static const char *IdSrc = "$Id: FGOutputFile.cpp,v 1.3 2012/12/15 16:13:57 bcoconni Exp $"; +static const char *IdHdr = ID_OUTPUTFILE; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGOutputFile::FGOutputFile(FGFDMExec* fdmex) : + FGOutputType(fdmex), + runID_postfix(0) +{ +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputFile::InitModel(void) +{ + if (FGOutputType::InitModel()) + return OpenFile(); + + return false; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputFile::SetStartNewOutput(void) +{ + if (Filename.size() > 0) { + ostringstream buf; + string::size_type dot = Name.find_last_of('.'); + if (dot != string::npos) { + buf << Name.substr(0, dot) << '_' << runID_postfix++ << Name.substr(dot); + } else { + buf << Name << '_' << runID_postfix++; + } + Filename = buf.str(); + CloseFile(); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputFile::Load(Element* el) +{ + if (!FGOutputType::Load(el)) + return false; + + SetOutputName(el->GetAttributeValue("name")); + + return true; +} + +} diff --git a/src/FDM/JSBSim/input_output/FGOutputFile.h b/src/FDM/JSBSim/input_output/FGOutputFile.h new file mode 100644 index 000000000..ee8fd1b4d --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputFile.h @@ -0,0 +1,131 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGOutputFile.h + Author: Bertrand Coconnier + Date started: 09/10/11 + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +09/10/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGOUTPUTFILE_H +#define FGOUTPUTFILE_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGFDMExec.h" +#include "FGOutputType.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_OUTPUTFILE "$Id: FGOutputFile.h,v 1.3 2012/12/15 16:13:57 bcoconni Exp $" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** Abstract class that provide functions that are generic to all the outputs + that are directed to a file. A new class derived from FGOutputFile should + be created for each file format that JSBSim is able to output. + + This class provides all the machinery necessary to manage the file naming + including the sequence in which the file should be opened then closed. The + logic of SetStartNewOutput() is also managed in this class. Derived class + should normally not need to reimplement this method. In most cases, derived + classes only need to implement the methods OpenFile(), CloseFile() and + Print(). + */ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGOutputFile : public FGOutputType +{ +public: + /// Constructor + FGOutputFile(FGFDMExec* fdmex); + + /// Destructor : closes the file. + virtual ~FGOutputFile() { CloseFile(); } + + /** Init the output directives from an XML file. + @param element XML Element that is pointing to the output directives + */ + bool Load(Element* el); + + /** Initializes the instance. This method basically opens the file to which + outputs will be directed. + @result true if the execution succeeded. + */ + bool InitModel(void); + /** Reset the output prior to a restart of the simulation. This method should + be called when the simulation is restarted with, for example, new initial + conditions. The current file is closed and reopened with a new name. The + new name is contructed from the base file name set by the class + constructor or SetOutputName() and is appended with an underscore _ and + an ID that is incremented at each call to this method. + */ + void SetStartNewOutput(void); + /** Overwrites the name identifier under which the output will be logged. + For this method to take effect, it must be called prior to + FGFDMExec::RunIC(). If it is called after, it will not take effect before + the next call to SetStartNewOutput(). + @param name new name */ + void SetOutputName(const std::string& fname) { + Name = Filename = FDMExec->GetRootDir() + fname; + runID_postfix = 0; + } + /** Generate the output. This is a pure method so it must be implemented by + the classes that inherits from FGOutputFile. + */ + void Print(void) = 0; + +protected: + std::string Filename; + + /// Opens the file + virtual bool OpenFile(void) = 0; + /// Closes the file + virtual void CloseFile(void) {} + +private: + int runID_postfix; +}; +} +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif + diff --git a/src/FDM/JSBSim/input_output/FGOutputSocket.cpp b/src/FDM/JSBSim/input_output/FGOutputSocket.cpp new file mode 100644 index 000000000..f41cc21e1 --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputSocket.cpp @@ -0,0 +1,402 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGOutputSocket.cpp + Author: Bertrand Coconnier + Date started: 09/10/11 + Purpose: Manage output of sim parameters to a socket + Called by: FGOutput + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This is the place where you create output routines to dump data for perusal +later. + +HISTORY +-------------------------------------------------------------------------------- +09/10/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include +#include + +#include "FGOutputSocket.h" +#include "FGFDMExec.h" +#include "models/FGAerodynamics.h" +#include "models/FGAccelerations.h" +#include "models/FGAircraft.h" +#include "models/FGAtmosphere.h" +#include "models/FGAuxiliary.h" +#include "models/FGPropulsion.h" +#include "models/FGMassBalance.h" +#include "models/FGPropagate.h" +#include "models/FGGroundReactions.h" +#include "models/FGFCS.h" +#include "models/atmosphere/FGWinds.h" + +using namespace std; + +namespace JSBSim { + +static const char *IdSrc = "$Id: FGOutputSocket.cpp,v 1.5 2013/01/12 21:14:46 bcoconni Exp $"; +static const char *IdHdr = ID_OUTPUTSOCKET; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGOutputSocket::FGOutputSocket(FGFDMExec* fdmex) : + FGOutputType(fdmex), + socket(0) +{ +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGOutputSocket::~FGOutputSocket() +{ + delete socket; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputSocket::SetOutputName(const string& fname) +{ + // tokenize the output name + size_t dot_pos = fname.find(':', 0); + size_t slash_pos = fname.find('/', 0); + + string name = fname.substr(0, dot_pos); + + string proto = "TCP"; + if(dot_pos + 1 < slash_pos) + proto = fname.substr(dot_pos + 1, slash_pos - dot_pos - 1); + + string port = "1138"; + if(slash_pos < string::npos) + port = fname.substr(slash_pos + 1, string::npos); + + // set the model name + Name = name + ":" + port + "/" + proto; + + // set the socket params + SockName = name; + + SockPort = atoi(port.c_str()); + + if (proto == "UDP") + SockProtocol = FGfdmSocket::ptUDP; + else // Default to TCP + SockProtocol = FGfdmSocket::ptTCP; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputSocket::Load(Element* el) +{ + if (!FGOutputType::Load(el)) + return false; + + SetOutputName(el->GetAttributeValue("name") + ":" + + el->GetAttributeValue("protocol") + "/" + + el->GetAttributeValue("port")); + + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputSocket::InitModel(void) +{ + if (FGOutputType::InitModel()) { + delete socket; + socket = new FGfdmSocket(SockName, SockPort, SockProtocol); + + if (socket == 0) return false; + if (!socket->GetConnectStatus()) return false; + + PrintHeaders(); + + return true; + } + + return false; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputSocket::PrintHeaders(void) +{ + string scratch; + + socket->Clear(); + socket->Clear(""); + socket->Append("Time"); + + if (SubSystems & ssAerosurfaces) { + socket->Append("Aileron Command"); + socket->Append("Elevator Command"); + socket->Append("Rudder Command"); + socket->Append("Flap Command"); + socket->Append("Left Aileron Position"); + socket->Append("Right Aileron Position"); + socket->Append("Elevator Position"); + socket->Append("Rudder Position"); + socket->Append("Flap Position"); + } + + if (SubSystems & ssRates) { + socket->Append("P"); + socket->Append("Q"); + socket->Append("R"); + socket->Append("PDot"); + socket->Append("QDot"); + socket->Append("RDot"); + } + + if (SubSystems & ssVelocities) { + socket->Append("QBar"); + socket->Append("Vtotal"); + socket->Append("UBody"); + socket->Append("VBody"); + socket->Append("WBody"); + socket->Append("UAero"); + socket->Append("VAero"); + socket->Append("WAero"); + socket->Append("Vn"); + socket->Append("Ve"); + socket->Append("Vd"); + } + + if (SubSystems & ssForces) { + socket->Append("F_Drag"); + socket->Append("F_Side"); + socket->Append("F_Lift"); + socket->Append("LoD"); + socket->Append("Fx"); + socket->Append("Fy"); + socket->Append("Fz"); + } + + if (SubSystems & ssMoments) { + socket->Append("L"); + socket->Append("M"); + socket->Append("N"); + } + + if (SubSystems & ssAtmosphere) { + socket->Append("Rho"); + socket->Append("SL pressure"); + socket->Append("Ambient pressure"); + socket->Append("Turbulence Magnitude"); + socket->Append("Turbulence Direction X"); + socket->Append("Turbulence Direction Y"); + socket->Append("Turbulence Direction Z"); + socket->Append("NWind"); + socket->Append("EWind"); + socket->Append("DWind"); + } + + if (SubSystems & ssMassProps) { + socket->Append("Ixx"); + socket->Append("Ixy"); + socket->Append("Ixz"); + socket->Append("Iyx"); + socket->Append("Iyy"); + socket->Append("Iyz"); + socket->Append("Izx"); + socket->Append("Izy"); + socket->Append("Izz"); + socket->Append("Mass"); + socket->Append("Xcg"); + socket->Append("Ycg"); + socket->Append("Zcg"); + } + + if (SubSystems & ssPropagate) { + socket->Append("Altitude"); + socket->Append("Phi (deg)"); + socket->Append("Tht (deg)"); + socket->Append("Psi (deg)"); + socket->Append("Alpha (deg)"); + socket->Append("Beta (deg)"); + socket->Append("Latitude (deg)"); + socket->Append("Longitude (deg)"); + } + + if (SubSystems & ssAeroFunctions) { + scratch = Aerodynamics->GetAeroFunctionStrings(","); + if (scratch.length() != 0) socket->Append(scratch); + } + + if (SubSystems & ssFCS) { + scratch = FCS->GetComponentStrings(","); + if (scratch.length() != 0) socket->Append(scratch); + } + + if (SubSystems & ssGroundReactions) + socket->Append(GroundReactions->GetGroundReactionStrings(",")); + + if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) + socket->Append(Propulsion->GetPropulsionStrings(",")); + + if (OutputProperties.size() > 0) { + for (unsigned int i=0;i 0) { + socket->Append(OutputCaptions[i]); + } else { + socket->Append(OutputProperties[i]->GetPrintableName()); + } + } + + socket->Send(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputSocket::Print(void) +{ + string asciiData, scratch; + + if (socket == 0) return; + if (!socket->GetConnectStatus()) return; + + socket->Clear(); + socket->Append(FDMExec->GetSimTime()); + + if (SubSystems & ssAerosurfaces) { + socket->Append(FCS->GetDaCmd()); + socket->Append(FCS->GetDeCmd()); + socket->Append(FCS->GetDrCmd()); + socket->Append(FCS->GetDfCmd()); + socket->Append(FCS->GetDaLPos()); + socket->Append(FCS->GetDaRPos()); + socket->Append(FCS->GetDePos()); + socket->Append(FCS->GetDrPos()); + socket->Append(FCS->GetDfPos()); + } + if (SubSystems & ssRates) { + socket->Append(radtodeg*Propagate->GetPQR(eP)); + socket->Append(radtodeg*Propagate->GetPQR(eQ)); + socket->Append(radtodeg*Propagate->GetPQR(eR)); + socket->Append(radtodeg*Accelerations->GetPQRdot(eP)); + socket->Append(radtodeg*Accelerations->GetPQRdot(eQ)); + socket->Append(radtodeg*Accelerations->GetPQRdot(eR)); + } + if (SubSystems & ssVelocities) { + socket->Append(Auxiliary->Getqbar()); + socket->Append(Auxiliary->GetVt()); + socket->Append(Propagate->GetUVW(eU)); + socket->Append(Propagate->GetUVW(eV)); + socket->Append(Propagate->GetUVW(eW)); + socket->Append(Auxiliary->GetAeroUVW(eU)); + socket->Append(Auxiliary->GetAeroUVW(eV)); + socket->Append(Auxiliary->GetAeroUVW(eW)); + socket->Append(Propagate->GetVel(eNorth)); + socket->Append(Propagate->GetVel(eEast)); + socket->Append(Propagate->GetVel(eDown)); + } + if (SubSystems & ssForces) { + socket->Append(Aerodynamics->GetvFw()(eDrag)); + socket->Append(Aerodynamics->GetvFw()(eSide)); + socket->Append(Aerodynamics->GetvFw()(eLift)); + socket->Append(Aerodynamics->GetLoD()); + socket->Append(Aircraft->GetForces(eX)); + socket->Append(Aircraft->GetForces(eY)); + socket->Append(Aircraft->GetForces(eZ)); + } + if (SubSystems & ssMoments) { + socket->Append(Aircraft->GetMoments(eL)); + socket->Append(Aircraft->GetMoments(eM)); + socket->Append(Aircraft->GetMoments(eN)); + } + if (SubSystems & ssAtmosphere) { + socket->Append(Atmosphere->GetDensity()); + socket->Append(Atmosphere->GetPressureSL()); + socket->Append(Atmosphere->GetPressure()); + socket->Append(Winds->GetTurbMagnitude()); + socket->Append(Winds->GetTurbDirection().Dump(",")); + socket->Append(Winds->GetTotalWindNED().Dump(",")); + } + if (SubSystems & ssMassProps) { + socket->Append(MassBalance->GetJ()(1,1)); + socket->Append(MassBalance->GetJ()(1,2)); + socket->Append(MassBalance->GetJ()(1,3)); + socket->Append(MassBalance->GetJ()(2,1)); + socket->Append(MassBalance->GetJ()(2,2)); + socket->Append(MassBalance->GetJ()(2,3)); + socket->Append(MassBalance->GetJ()(3,1)); + socket->Append(MassBalance->GetJ()(3,2)); + socket->Append(MassBalance->GetJ()(3,3)); + socket->Append(MassBalance->GetMass()); + socket->Append(MassBalance->GetXYZcg()(eX)); + socket->Append(MassBalance->GetXYZcg()(eY)); + socket->Append(MassBalance->GetXYZcg()(eZ)); + } + if (SubSystems & ssPropagate) { + socket->Append(Propagate->GetAltitudeASL()); + socket->Append(radtodeg*Propagate->GetEuler(ePhi)); + socket->Append(radtodeg*Propagate->GetEuler(eTht)); + socket->Append(radtodeg*Propagate->GetEuler(ePsi)); + socket->Append(Auxiliary->Getalpha(inDegrees)); + socket->Append(Auxiliary->Getbeta(inDegrees)); + socket->Append(Propagate->GetLocation().GetLatitudeDeg()); + socket->Append(Propagate->GetLocation().GetLongitudeDeg()); + } + if (SubSystems & ssAeroFunctions) { + scratch = Aerodynamics->GetAeroFunctionValues(","); + if (scratch.length() != 0) socket->Append(scratch); + } + if (SubSystems & ssFCS) { + scratch = FCS->GetComponentValues(","); + if (scratch.length() != 0) socket->Append(scratch); + } + if (SubSystems & ssGroundReactions) { + socket->Append(GroundReactions->GetGroundReactionValues(",")); + } + if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { + socket->Append(Propulsion->GetPropulsionValues(",")); + } + + for (unsigned int i=0;iAppend(OutputProperties[i]->getDoubleValue()); + } + + socket->Send(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputSocket::SocketStatusOutput(const string& out_str) +{ + string asciiData; + + if (socket == 0) return; + + socket->Clear(); + asciiData = string("") + out_str; + socket->Append(asciiData.c_str()); + socket->Send(); +} +} diff --git a/src/FDM/JSBSim/input_output/FGOutputSocket.h b/src/FDM/JSBSim/input_output/FGOutputSocket.h new file mode 100644 index 000000000..c753329fc --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputSocket.h @@ -0,0 +1,121 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGOutputSocket.h + Author: Bertrand Coconnier + Date started: 09/10/11 + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +09/10/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGOUTPUTSOCKET_H +#define FGOUTPUTSOCKET_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGOutputType.h" +#include "input_output/net_fdm.hxx" +#include "input_output/FGfdmSocket.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_OUTPUTSOCKET "$Id: FGOutputSocket.h,v 1.3 2013/01/12 18:08:40 bcoconni Exp $" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** Implements the output to a socket. This class outputs data to a socket + according to the JSBSim format. It can be inherited as a generic class that + provides services for socket outputs. For instance FGOutputFG inherits + FGOutputSocket for the socket management but outputs data with a format + different than FGOutputSocket. + */ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGOutputSocket : public FGOutputType +{ +public: + /** Constructor. */ + FGOutputSocket(FGFDMExec* fdmex); + + /** Destructor. */ + ~FGOutputSocket(); + + /** Overwrites the name identifier under which the output will be logged. + This method is taken into account if it is called before + FGFDMExec::RunIC() otherwise it is ignored until the next call to + SetStartNewOutput(). + @param name new name in the form "hostname:port/proto" + hostname could be an ip, port a numerical value and + proto should be UDP or TCP (the default if omitted) + */ + virtual void SetOutputName(const std::string& name); + + /** Init the output directives from an XML file. + @param element XML Element that is pointing to the output directives + */ + virtual bool Load(Element* el); + + /** Initializes the instance. This method basically opens the socket to which + outputs will be directed. + @result true if the execution succeeded. + */ + bool InitModel(void); + /// Generates the output. + void Print(void); + + /** Outputs a status thru the socket. This method issues a message prepended + by the string "" to the socket. + @param out_str status message + */ + void SocketStatusOutput(const std::string& out_str); + +protected: + virtual void PrintHeaders(void); + + std::string SockName; + unsigned int SockPort; + FGfdmSocket::ProtocolType SockProtocol; + FGfdmSocket* socket; +}; +} +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif + diff --git a/src/FDM/JSBSim/input_output/FGOutputTextFile.cpp b/src/FDM/JSBSim/input_output/FGOutputTextFile.cpp new file mode 100644 index 000000000..f0eb0561c --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputTextFile.cpp @@ -0,0 +1,390 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGOutputTextFile.cpp + Author: Bertrand Coconnier + Date started: 09/17/11 + Purpose: Manage output of sim parameters to a text file + Called by: FGOutput + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This is the place where you create output routines to dump data for perusal +later. + +HISTORY +-------------------------------------------------------------------------------- +09/17/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include +#include +#include +#include + +#include "FGOutputTextFile.h" +#include "FGFDMExec.h" +#include "models/FGAerodynamics.h" +#include "models/FGAccelerations.h" +#include "models/FGAircraft.h" +#include "models/FGAtmosphere.h" +#include "models/FGAuxiliary.h" +#include "models/FGPropulsion.h" +#include "models/FGMassBalance.h" +#include "models/FGPropagate.h" +#include "models/FGGroundReactions.h" +#include "models/FGExternalReactions.h" +#include "models/FGBuoyantForces.h" +#include "models/FGFCS.h" +#include "models/atmosphere/FGWinds.h" + +using namespace std; + +namespace JSBSim { + +static const char *IdSrc = "$Id: FGOutputTextFile.cpp,v 1.5 2013/01/12 19:26:59 jberndt Exp $"; +static const char *IdHdr = ID_OUTPUTTEXTFILE; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +bool FGOutputTextFile::Load(Element* el) +{ + if(!FGOutputFile::Load(el)) + return false; + + string type = el->GetAttributeValue("type"); + string delim; + if (type == "TABULAR") { + delim = "\t"; + } else { + delim = ","; + } + + SetDelimiter(delim); + + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputTextFile::OpenFile(void) +{ + datafile.clear(); + datafile.open(Filename.c_str()); + if (!datafile) { + cerr << endl << fgred << highint << "ERROR: unable to open the file " + << reset << Filename.c_str() << endl + << fgred << highint << " => Output to this file is disabled." + << reset << endl << endl; + Disable(); + return false; + } + + string scratch = ""; + streambuf* buffer = datafile.rdbuf(); + ostream outstream(buffer); + + outstream.precision(10); + + outstream << "Time"; + if (SubSystems & ssSimulation) { + // Nothing here, yet + } + if (SubSystems & ssAerosurfaces) { + outstream << delimeter; + outstream << "Aileron Command (norm)" + delimeter; + outstream << "Elevator Command (norm)" + delimeter; + outstream << "Rudder Command (norm)" + delimeter; + outstream << "Flap Command (norm)" + delimeter; + outstream << "Left Aileron Position (deg)" + delimeter; + outstream << "Right Aileron Position (deg)" + delimeter; + outstream << "Elevator Position (deg)" + delimeter; + outstream << "Rudder Position (deg)" + delimeter; + outstream << "Flap Position (deg)"; + } + if (SubSystems & ssRates) { + outstream << delimeter; + outstream << "P (deg/s)" + delimeter + "Q (deg/s)" + delimeter + "R (deg/s)" + delimeter; + outstream << "P dot (deg/s^2)" + delimeter + "Q dot (deg/s^2)" + delimeter + "R dot (deg/s^2)" + delimeter; + outstream << "P_{inertial} (deg/s)" + delimeter + "Q_{inertial} (deg/s)" + delimeter + "R_{inertial} (deg/s)"; + } + if (SubSystems & ssVelocities) { + outstream << delimeter; + outstream << "q bar (psf)" + delimeter; + outstream << "Reynolds Number" + delimeter; + outstream << "V_{Total} (ft/s)" + delimeter; + outstream << "V_{Inertial} (ft/s)" + delimeter; + outstream << "UBody" + delimeter + "VBody" + delimeter + "WBody" + delimeter; + outstream << "UdotBody" + delimeter + "VdotBody" + delimeter + "WdotBody" + delimeter; + outstream << "UdotBody_i" + delimeter + "VdotBody_i" + delimeter + "WdotBody_i" + delimeter; + outstream << "BodyAccel_X" + delimeter + "BodyAccel_Y" + delimeter + "BodyAccel_Z" + delimeter; + outstream << "Aero V_{X Body} (ft/s)" + delimeter + "Aero V_{Y Body} (ft/s)" + delimeter + "Aero V_{Z Body} (ft/s)" + delimeter; + outstream << "V_{X_{inertial}} (ft/s)" + delimeter + "V_{Y_{inertial}} (ft/s)" + delimeter + "V_{Z_{inertial}} (ft/s)" + delimeter; + outstream << "V_{X_{ecef}} (ft/s)" + delimeter + "V_{Y_{ecef}} (ft/s)" + delimeter + "V_{Z_{ecef}} (ft/s)" + delimeter; + outstream << "V_{North} (ft/s)" + delimeter + "V_{East} (ft/s)" + delimeter + "V_{Down} (ft/s)"; + } + if (SubSystems & ssForces) { + outstream << delimeter; + outstream << "F_{Drag} (lbs)" + delimeter + "F_{Side} (lbs)" + delimeter + "F_{Lift} (lbs)" + delimeter; + outstream << "L/D" + delimeter; + outstream << "F_{Aero x} (lbs)" + delimeter + "F_{Aero y} (lbs)" + delimeter + "F_{Aero z} (lbs)" + delimeter; + outstream << "F_{Prop x} (lbs)" + delimeter + "F_{Prop y} (lbs)" + delimeter + "F_{Prop z} (lbs)" + delimeter; + outstream << "F_{Gear x} (lbs)" + delimeter + "F_{Gear y} (lbs)" + delimeter + "F_{Gear z} (lbs)" + delimeter; + outstream << "F_{Ext x} (lbs)" + delimeter + "F_{Ext y} (lbs)" + delimeter + "F_{Ext z} (lbs)" + delimeter; + outstream << "F_{Buoyant x} (lbs)" + delimeter + "F_{Buoyant y} (lbs)" + delimeter + "F_{Buoyant z} (lbs)" + delimeter; + outstream << "F_{Total x} (lbs)" + delimeter + "F_{Total y} (lbs)" + delimeter + "F_{Total z} (lbs)"; + } + if (SubSystems & ssMoments) { + outstream << delimeter; + outstream << "L_{Aero} (ft-lbs)" + delimeter + "M_{Aero} (ft-lbs)" + delimeter + "N_{Aero} (ft-lbs)" + delimeter; + outstream << "L_{Aero MRC} (ft-lbs)" + delimeter + "M_{Aero MRC} (ft-lbs)" + delimeter + "N_{Aero MRC} (ft-lbs)" + delimeter; + outstream << "L_{Prop} (ft-lbs)" + delimeter + "M_{Prop} (ft-lbs)" + delimeter + "N_{Prop} (ft-lbs)" + delimeter; + outstream << "L_{Gear} (ft-lbs)" + delimeter + "M_{Gear} (ft-lbs)" + delimeter + "N_{Gear} (ft-lbs)" + delimeter; + outstream << "L_{ext} (ft-lbs)" + delimeter + "M_{ext} (ft-lbs)" + delimeter + "N_{ext} (ft-lbs)" + delimeter; + outstream << "L_{Buoyant} (ft-lbs)" + delimeter + "M_{Buoyant} (ft-lbs)" + delimeter + "N_{Buoyant} (ft-lbs)" + delimeter; + outstream << "L_{Total} (ft-lbs)" + delimeter + "M_{Total} (ft-lbs)" + delimeter + "N_{Total} (ft-lbs)"; + } + if (SubSystems & ssAtmosphere) { + outstream << delimeter; + outstream << "Rho (slugs/ft^3)" + delimeter; + outstream << "Absolute Viscosity" + delimeter; + outstream << "Kinematic Viscosity" + delimeter; + outstream << "Temperature (R)" + delimeter; + outstream << "P_{SL} (psf)" + delimeter; + outstream << "P_{Ambient} (psf)" + delimeter; + outstream << "Turbulence Magnitude (ft/sec)" + delimeter; + outstream << "Turbulence X Direction (rad)" + delimeter + "Turbulence Y Direction (rad)" + delimeter + "Turbulence Z Direction (rad)" + delimeter; + outstream << "Wind V_{North} (ft/s)" + delimeter + "Wind V_{East} (ft/s)" + delimeter + "Wind V_{Down} (ft/s)"; + } + if (SubSystems & ssMassProps) { + outstream << delimeter; + outstream << "I_{xx}" + delimeter; + outstream << "I_{xy}" + delimeter; + outstream << "I_{xz}" + delimeter; + outstream << "I_{yx}" + delimeter; + outstream << "I_{yy}" + delimeter; + outstream << "I_{yz}" + delimeter; + outstream << "I_{zx}" + delimeter; + outstream << "I_{zy}" + delimeter; + outstream << "I_{zz}" + delimeter; + outstream << "Mass" + delimeter; + outstream << "X_{cg}" + delimeter + "Y_{cg}" + delimeter + "Z_{cg}"; + } + if (SubSystems & ssPropagate) { + outstream << delimeter; + outstream << "Altitude ASL (ft)" + delimeter; + outstream << "Altitude AGL (ft)" + delimeter; + outstream << "Phi (deg)" + delimeter + "Theta (deg)" + delimeter + "Psi (deg)" + delimeter; + outstream << "Q(1)_{LOCAL}" + delimeter + "Q(2)_{LOCAL}" + delimeter + "Q(3)_{LOCAL}" + delimeter + "Q(4)_{LOCAL}" + delimeter; + outstream << "Q(1)_{ECEF}" + delimeter + "Q(2)_{ECEF}" + delimeter + "Q(3)_{ECEF}" + delimeter + "Q(4)_{ECEF}" + delimeter; + outstream << "Q(1)_{ECI}" + delimeter + "Q(2)_{ECI}" + delimeter + "Q(3)_{ECI}" + delimeter + "Q(4)_{ECI}" + delimeter; + outstream << "Alpha (deg)" + delimeter; + outstream << "Beta (deg)" + delimeter; + outstream << "Latitude (deg)" + delimeter; + outstream << "Latitude Geodetic (deg)" + delimeter; + outstream << "Longitude (deg)" + delimeter; + outstream << "X_{ECI} (ft)" + delimeter + "Y_{ECI} (ft)" + delimeter + "Z_{ECI} (ft)" + delimeter; + outstream << "X_{ECEF} (ft)" + delimeter + "Y_{ECEF} (ft)" + delimeter + "Z_{ECEF} (ft)" + delimeter; + outstream << "Earth Position Angle (deg)" + delimeter; + outstream << "Distance AGL (ft)" + delimeter; + outstream << "Terrain Elevation (ft)"; + } + if (SubSystems & ssAeroFunctions) { + scratch = Aerodynamics->GetAeroFunctionStrings(delimeter); + if (scratch.length() != 0) outstream << delimeter << scratch; + } + if (SubSystems & ssFCS) { + scratch = FCS->GetComponentStrings(delimeter); + if (scratch.length() != 0) outstream << delimeter << scratch; + } + if (SubSystems & ssGroundReactions) { + outstream << delimeter; + outstream << GroundReactions->GetGroundReactionStrings(delimeter); + } + if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { + outstream << delimeter; + outstream << Propulsion->GetPropulsionStrings(delimeter); + } + if (OutputProperties.size() > 0) { + for (unsigned int i=0;i 0) { + outstream << delimeter << OutputCaptions[i]; + } else { + outstream << delimeter << OutputProperties[i]->GetFullyQualifiedName(); + } + } + } + + outstream << endl; + outstream.flush(); + + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputTextFile::Print(void) +{ + streambuf* buffer; + string scratch = ""; + + if (Filename == "COUT" || Filename == "cout") { + buffer = cout.rdbuf(); + } else { + buffer = datafile.rdbuf(); + } + + ostream outstream(buffer); + + outstream.precision(10); + + outstream << FDMExec->GetSimTime(); + if (SubSystems & ssSimulation) { + } + if (SubSystems & ssAerosurfaces) { + outstream << delimeter; + outstream << FCS->GetDaCmd() << delimeter; + outstream << FCS->GetDeCmd() << delimeter; + outstream << FCS->GetDrCmd() << delimeter; + outstream << FCS->GetDfCmd() << delimeter; + outstream << FCS->GetDaLPos(ofDeg) << delimeter; + outstream << FCS->GetDaRPos(ofDeg) << delimeter; + outstream << FCS->GetDePos(ofDeg) << delimeter; + outstream << FCS->GetDrPos(ofDeg) << delimeter; + outstream << FCS->GetDfPos(ofDeg); + } + if (SubSystems & ssRates) { + outstream << delimeter; + outstream << (radtodeg*Propagate->GetPQR()).Dump(delimeter) << delimeter; + outstream << (radtodeg*Accelerations->GetPQRdot()).Dump(delimeter) << delimeter; + outstream << (radtodeg*Propagate->GetPQRi()).Dump(delimeter); + } + if (SubSystems & ssVelocities) { + outstream << delimeter; + outstream << Auxiliary->Getqbar() << delimeter; + outstream << Auxiliary->GetReynoldsNumber() << delimeter; + outstream << setprecision(12) << Auxiliary->GetVt() << delimeter; + outstream << Propagate->GetInertialVelocityMagnitude() << delimeter; + outstream << setprecision(12) << Propagate->GetUVW().Dump(delimeter) << delimeter; + outstream << setprecision(12) << Accelerations->GetUVWdot().Dump(delimeter) << delimeter; + outstream << setprecision(12) << Accelerations->GetUVWidot().Dump(delimeter) << delimeter; + outstream << setprecision(12) << Accelerations->GetBodyAccel().Dump(delimeter) << delimeter; + outstream << Auxiliary->GetAeroUVW().Dump(delimeter) << delimeter; + outstream << Propagate->GetInertialVelocity().Dump(delimeter) << delimeter; + outstream << Propagate->GetECEFVelocity().Dump(delimeter) << delimeter; + outstream << Propagate->GetVel().Dump(delimeter); + outstream.precision(10); + } + if (SubSystems & ssForces) { + outstream << delimeter; + outstream << Aerodynamics->GetvFw().Dump(delimeter) << delimeter; + outstream << Aerodynamics->GetLoD() << delimeter; + outstream << Aerodynamics->GetForces().Dump(delimeter) << delimeter; + outstream << Propulsion->GetForces().Dump(delimeter) << delimeter; + outstream << GroundReactions->GetForces().Dump(delimeter) << delimeter; + outstream << ExternalReactions->GetForces().Dump(delimeter) << delimeter; + outstream << BuoyantForces->GetForces().Dump(delimeter) << delimeter; + outstream << Aircraft->GetForces().Dump(delimeter); + } + if (SubSystems & ssMoments) { + outstream << delimeter; + outstream << Aerodynamics->GetMoments().Dump(delimeter) << delimeter; + outstream << Aerodynamics->GetMomentsMRC().Dump(delimeter) << delimeter; + outstream << Propulsion->GetMoments().Dump(delimeter) << delimeter; + outstream << GroundReactions->GetMoments().Dump(delimeter) << delimeter; + outstream << ExternalReactions->GetMoments().Dump(delimeter) << delimeter; + outstream << BuoyantForces->GetMoments().Dump(delimeter) << delimeter; + outstream << Aircraft->GetMoments().Dump(delimeter); + } + if (SubSystems & ssAtmosphere) { + outstream << delimeter; + outstream << Atmosphere->GetDensity() << delimeter; + outstream << Atmosphere->GetAbsoluteViscosity() << delimeter; + outstream << Atmosphere->GetKinematicViscosity() << delimeter; + outstream << Atmosphere->GetTemperature() << delimeter; + outstream << Atmosphere->GetPressureSL() << delimeter; + outstream << Atmosphere->GetPressure() << delimeter; + outstream << Winds->GetTurbMagnitude() << delimeter; + outstream << Winds->GetTurbDirection().Dump(delimeter) << delimeter; + outstream << Winds->GetTotalWindNED().Dump(delimeter); + } + if (SubSystems & ssMassProps) { + outstream << delimeter; + outstream << MassBalance->GetJ().Dump(delimeter) << delimeter; + outstream << MassBalance->GetMass() << delimeter; + outstream << MassBalance->GetXYZcg().Dump(delimeter); + } + if (SubSystems & ssPropagate) { + outstream.precision(14); + outstream << delimeter; + outstream << Propagate->GetAltitudeASL() << delimeter; + outstream << Propagate->GetDistanceAGL() << delimeter; + outstream << (radtodeg*Propagate->GetEuler()).Dump(delimeter) << delimeter; + outstream << Propagate->GetQuaternion().Dump(delimeter) << delimeter; + FGQuaternion Qec = Propagate->GetQuaternionECEF(); + outstream << Qec.Dump(delimeter) << delimeter; + outstream << Propagate->GetQuaternionECI().Dump(delimeter) << delimeter; + outstream << Auxiliary->Getalpha(inDegrees) << delimeter; + outstream << Auxiliary->Getbeta(inDegrees) << delimeter; + outstream << Propagate->GetLocation().GetLatitudeDeg() << delimeter; + outstream << Propagate->GetLocation().GetGeodLatitudeDeg() << delimeter; + outstream << Propagate->GetLocation().GetLongitudeDeg() << delimeter; + outstream.precision(18); + outstream << ((FGColumnVector3)Propagate->GetInertialPosition()).Dump(delimeter) << delimeter; + outstream << ((FGColumnVector3)Propagate->GetLocation()).Dump(delimeter) << delimeter; + outstream.precision(14); + outstream << Propagate->GetEarthPositionAngleDeg() << delimeter; + outstream << Propagate->GetDistanceAGL() << delimeter; + outstream << Propagate->GetTerrainElevation(); + outstream.precision(10); + } + if (SubSystems & ssAeroFunctions) { + scratch = Aerodynamics->GetAeroFunctionValues(delimeter); + if (scratch.length() != 0) outstream << delimeter << scratch; + } + if (SubSystems & ssFCS) { + scratch = FCS->GetComponentValues(delimeter); + if (scratch.length() != 0) outstream << delimeter << scratch; + } + if (SubSystems & ssGroundReactions) { + outstream << delimeter; + outstream << GroundReactions->GetGroundReactionValues(delimeter); + } + if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { + outstream << delimeter; + outstream << Propulsion->GetPropulsionValues(delimeter); + } + + outstream.precision(18); + for (unsigned int i=0;igetDoubleValue(); + } + outstream.precision(10); + + outstream << endl; + outstream.flush(); +} +} diff --git a/src/FDM/JSBSim/input_output/FGOutputTextFile.h b/src/FDM/JSBSim/input_output/FGOutputTextFile.h new file mode 100644 index 000000000..b3c9e1ec4 --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputTextFile.h @@ -0,0 +1,100 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGOutputTextFile.h + Author: Bertrand Coconnier + Date started: 09/17/11 + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +09/17/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGOUTPUTTEXTFILE_H +#define FGOUTPUTTEXTFILE_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include + +#include "FGOutputFile.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_OUTPUTTEXTFILE "$Id: FGOutputTextFile.h,v 1.4 2012/12/15 16:13:58 bcoconni Exp $" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** Implements the output to a human readable text file. This class uses the + standard C++ library to open and close a file to which output values are + comma-separated (CSV) or tabulated (TAB). + */ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGOutputTextFile : public FGOutputFile +{ +public: + /// Constructor + FGOutputTextFile(FGFDMExec* fdmex) : FGOutputFile(fdmex), delimeter(",") {} + + /** Set the delimiter. + @param delim delimiter of the output values (most likely a comma or a + tab) + */ + void SetDelimiter(const std::string& delim) { delimeter = delim; } + + /** Init the output directives from an XML file. + @param element XML Element that is pointing to the output directives + */ + virtual bool Load(Element* el); + + /// Generates the output to the text file. + virtual void Print(void); + +protected: + std::string delimeter; + std::ofstream datafile; + + virtual bool OpenFile(void); + virtual void CloseFile(void) { if (datafile.is_open()) datafile.close(); } +}; +} +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif + diff --git a/src/FDM/JSBSim/input_output/FGOutputType.cpp b/src/FDM/JSBSim/input_output/FGOutputType.cpp new file mode 100644 index 000000000..cdf568ee1 --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputType.cpp @@ -0,0 +1,275 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGOutputType.cpp + Author: Bertrand Coconnier + Date started: 09/10/11 + Purpose: Manage output of sim parameters to file or stdout + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- +This is the place where you create output routines to dump data for perusal +later. + +HISTORY +-------------------------------------------------------------------------------- +09/10/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include + +#include "FGFDMExec.h" +#include "FGOutputType.h" +#include "input_output/FGXMLElement.h" +#include "input_output/FGPropertyManager.h" + +namespace JSBSim { + +static const char *IdSrc = "$Id: FGOutputType.cpp,v 1.5 2013/01/14 22:44:51 bcoconni Exp $"; +static const char *IdHdr = ID_OUTPUTTYPE; + +using namespace std; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGOutputType::FGOutputType(FGFDMExec* fdmex) : + FGModel(fdmex), + SubSystems(0), + enabled(true) +{ + Aerodynamics = FDMExec->GetAerodynamics(); + Auxiliary = FDMExec->GetAuxiliary(); + Aircraft = FDMExec->GetAircraft(); + Atmosphere = FDMExec->GetAtmosphere(); + Winds = FDMExec->GetWinds(); + Propulsion = FDMExec->GetPropulsion(); + MassBalance = FDMExec->GetMassBalance(); + Propagate = FDMExec->GetPropagate(); + Accelerations = FDMExec->GetAccelerations(); + FCS = FDMExec->GetFCS(); + GroundReactions = FDMExec->GetGroundReactions(); + ExternalReactions = FDMExec->GetExternalReactions(); + BuoyantForces = FDMExec->GetBuoyantForces(); + + Debug(0); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGOutputType::~FGOutputType() +{ + OutputProperties.clear(); + Debug(1); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputType::SetIdx(int idx) +{ + typedef double (FGOutputType::*iOPMF)(void) const; + string outputProp = CreateIndexedPropertyName("simulation/output", idx) + "/log_rate_hz"; + + PropertyManager->Tie(outputProp, this, (iOPMF)0, &FGOutputType::SetRate, false); + OutputIdx = idx; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputType::Load(Element* element) +{ + // Perform base class Load. + if(!FGModel::Load(element)) + return false; + + if (element->FindElementValue("simulation") == string("ON")) + SubSystems += ssSimulation; + if (element->FindElementValue("aerosurfaces") == string("ON")) + SubSystems += ssAerosurfaces; + if (element->FindElementValue("rates") == string("ON")) + SubSystems += ssRates; + if (element->FindElementValue("velocities") == string("ON")) + SubSystems += ssVelocities; + if (element->FindElementValue("forces") == string("ON")) + SubSystems += ssForces; + if (element->FindElementValue("moments") == string("ON")) + SubSystems += ssMoments; + if (element->FindElementValue("atmosphere") == string("ON")) + SubSystems += ssAtmosphere; + if (element->FindElementValue("massprops") == string("ON")) + SubSystems += ssMassProps; + if (element->FindElementValue("position") == string("ON")) + SubSystems += ssPropagate; + if (element->FindElementValue("coefficients") == string("ON") || element->FindElementValue("aerodynamics") == string("ON")) + SubSystems += ssAeroFunctions; + if (element->FindElementValue("ground_reactions") == string("ON")) + SubSystems += ssGroundReactions; + if (element->FindElementValue("fcs") == string("ON")) + SubSystems += ssFCS; + if (element->FindElementValue("propulsion") == string("ON")) + SubSystems += ssPropulsion; + + Element *property_element = element->FindElement("property"); + + while (property_element) { + string caption=""; + string property_str = property_element->GetDataLine(); + FGPropertyManager* node = PropertyManager->GetNode(property_str); + if (!node) { + cerr << fgred << highint << endl << " No property by the name " + << property_str << " has been defined. This property will " << endl + << " not be logged. You should check your configuration file." + << reset << endl; + } else { + OutputProperties.push_back(node); + if (property_element->HasAttribute("caption")) { + OutputCaptions.push_back(property_element->GetAttributeValue("caption")); + } else { + OutputCaptions.push_back(""); + } + } + property_element = element->FindNextElement("property"); + } + + double outRate = 1.0; + if (!element->GetAttributeValue("rate").empty()) { + outRate = element->GetAttributeValueAsNumber("rate"); + } + SetRate(outRate); + + // FIXME : PostLoad should be called in the most derived class ? + PostLoad(element, PropertyManager); + + return true; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputType::InitModel(void) +{ + bool ret = FGModel::InitModel(); + + Debug(2); + return ret; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool FGOutputType::Run(bool Holding) +{ + if (!enabled) return true; + if (FGModel::Run(Holding)) return true; + if (Holding) return false; + + RunPreFunctions(); + Print(); + RunPostFunctions(); + + Debug(4); + + return false; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGOutputType::SetRate(double rtHz) +{ + rtHz = rtHz>1000?1000:(rtHz<0?0:rtHz); + if (rtHz > 0) { + FGModel::SetRate(0.5 + 1.0/(FDMExec->GetDeltaT()*rtHz)); + Enable(); + } else { + FGModel::SetRate(1); + Disable(); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// 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 FGOutputType::Debug(int from) +{ + string scratch=""; + + if (debug_lvl <= 0) return; + + if (debug_lvl & 1) { // Standard console startup message output + if (from == 0) { // Constructor + + } + if (from == 2) { + if (SubSystems & ssSimulation) cout << " Simulation parameters logged" << endl; + if (SubSystems & ssAerosurfaces) cout << " Aerosurface parameters logged" << endl; + if (SubSystems & ssRates) cout << " Rate parameters logged" << endl; + if (SubSystems & ssVelocities) cout << " Velocity parameters logged" << endl; + if (SubSystems & ssForces) cout << " Force parameters logged" << endl; + if (SubSystems & ssMoments) cout << " Moments parameters logged" << endl; + if (SubSystems & ssAtmosphere) cout << " Atmosphere parameters logged" << endl; + if (SubSystems & ssMassProps) cout << " Mass parameters logged" << endl; + if (SubSystems & ssAeroFunctions) cout << " Coefficient parameters logged" << endl; + if (SubSystems & ssPropagate) cout << " Propagate parameters logged" << endl; + if (SubSystems & ssGroundReactions) cout << " Ground parameters logged" << endl; + if (SubSystems & ssFCS) cout << " FCS parameters logged" << endl; + if (SubSystems & ssPropulsion) cout << " Propulsion parameters logged" << endl; + if (OutputProperties.size() > 0) cout << " Properties logged:" << endl; + for (unsigned int i=0;iGetName() << endl; + } + } + } + if (debug_lvl & 2 ) { // Instantiation/Destruction notification + if (from == 0) cout << "Instantiated: FGOutputType" << endl; + if (from == 1) cout << "Destroyed: FGOutputType" << 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; + } + } +} +} diff --git a/src/FDM/JSBSim/input_output/FGOutputType.h b/src/FDM/JSBSim/input_output/FGOutputType.h new file mode 100644 index 000000000..d06ea2cb8 --- /dev/null +++ b/src/FDM/JSBSim/input_output/FGOutputType.h @@ -0,0 +1,222 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGOutputType.h + Author: Bertrand Coconnier + Date started: 09/10/11 + + ------------- Copyright (C) 2011 Bertrand Coconnier ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +09/10/11 BC Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGOUTPUTTYPE_H +#define FGOUTPUTTYPE_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "models/FGModel.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_OUTPUTTYPE "$Id: FGOutputType.h,v 1.4 2013/01/12 20:56:19 bcoconni Exp $" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +class FGFDMExec; +class FGPropertyManager; +class Element; +class FGAerodynamics; +class FGAuxiliary; +class FGAircraft; +class FGAtmosphere; +class FGWinds; +class FGPropulsion; +class FGMassBalance; +class FGPropagate; +class FGAccelerations; +class FGFCS; +class FGGroundReactions; +class FGExternalReactions; +class FGBuoyantForces; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** Abstract class to provide functions generic to all the output directives. + This class is used by the output manager FGOutput to manage a list of + different output classes without needing to know the details of each one of + them. It also provides the functions that are common to all the output + classes. + + The class inherits from FGModelFunctions so it is possible to define + functions that execute before or after the output is generated. Such + functions need to be tagged with a "pre" or "post" type attribute to denote + the sequence in which they should be executed. + + The class mimics some functionalities of FGModel (methods InitModel(), + Run() and SetRate()). However it does not inherit from FGModel since it is + conceptually different from the model paradigm. + */ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGOutputType : public FGModel +{ +public: + /** Constructor (implement the FGModel interface). + @param fdmex a pointer to the parent executive object + */ + FGOutputType(FGFDMExec* fdmex); + + /// Destructor + virtual ~FGOutputType(); + + /** Set the idx for this output instance + @param idx ID of the output instance that is constructed + */ + void SetIdx(int idx); + + /** Set the output rate for this output instances. + @param rtHz new output rate in Hz */ + void SetRate(double rtHz); + + /** Set the activated subsystems for this output instance. + @param subSystems bitfield that describes the activated subsystems + @param outputProperties list of properties that should be output + */ + void SetSubSystems(int subSystems) { SubSystems = subSystems; } + + /** Set the list of properties that should be output for this output instance. + @param outputProperties list of properties that should be output + */ + void SetOutputProperties(std::vector & outputProperties) + { + OutputProperties = outputProperties; + } + + /** Overwrites the name identifier under which the output will be logged. + This method is taken into account if it is called before + FGFDMExec::RunIC() otherwise it is ignored until the next call to + SetStartNewOutput(). + @param name new name */ + virtual void SetOutputName(const std::string& name) { Name = name; } + + /** Get the name identifier to which the output will be directed. + @result the name identifier.*/ + virtual const std::string& GetOutputName(void) const { return Name; } + + /** Init the output directives from an XML file (implement the FGModel interface). + @param element XML Element that is pointing to the output directives + */ + virtual bool Load(Element* el); + + /// Init the output model according to its configitation. + virtual bool InitModel(void); + + /** Executes the output directives (implement the FGModel interface). + This method checks that the current time step matches the output + rate and calls the registered "pre" functions, the output + generation and finally the "post" functions. + @result false if no error. + */ + bool Run(bool Holding); + + /** Generate the output. This is a pure method so it must be implemented by + the classes that inherits from FGOutputType. The Print name may not be + relevant to all outputs but it has been kept for backward compatibility. + */ + virtual void Print(void) = 0; + + /** Reset the output prior to a restart of the simulation. This method should + be called when the simulation is restarted with, for example, new initial + conditions. When this method is executed the output instance can take + special actions such as closing the current output file and open a new + one with a different name. */ + + virtual void SetStartNewOutput(void) {} + + /// Enables the output generation. + void Enable(void) { enabled = true; } + /// Disables the output generation. + void Disable(void) { enabled = false; } + /** Toggles the output generation. + @result the output generation status i.e. true if the output has been + enabled, false if the output has been disabled. */ + bool Toggle(void) {enabled = !enabled; return enabled;} + + /// Subsystem types for specifying which will be output in the FDM data logging + enum eSubSystems { + /** Subsystem: Simulation (= 1) */ ssSimulation = 1, + /** Subsystem: Aerosurfaces (= 2) */ ssAerosurfaces = 2, + /** Subsystem: Body rates (= 4) */ ssRates = 4, + /** Subsystem: Velocities (= 8) */ ssVelocities = 8, + /** Subsystem: Forces (= 16) */ ssForces = 16, + /** Subsystem: Moments (= 32) */ ssMoments = 32, + /** Subsystem: Atmosphere (= 64) */ ssAtmosphere = 64, + /** Subsystem: Mass Properties (= 128) */ ssMassProps = 128, + /** Subsystem: Coefficients (= 256) */ ssAeroFunctions = 256, + /** Subsystem: Propagate (= 512) */ ssPropagate = 512, + /** Subsystem: Ground Reactions (= 1024) */ ssGroundReactions = 1024, + /** Subsystem: FCS (= 2048) */ ssFCS = 2048, + /** Subsystem: Propulsion (= 4096) */ ssPropulsion = 4096 + } subsystems; + +protected: + int OutputIdx; + int SubSystems; + std::vector OutputProperties; + std::vector OutputCaptions; + bool enabled; + + FGAerodynamics* Aerodynamics; + FGAuxiliary* Auxiliary; + FGAircraft* Aircraft; + FGAtmosphere* Atmosphere; + FGWinds* Winds; + FGPropulsion* Propulsion; + FGMassBalance* MassBalance; + FGPropagate* Propagate; + FGAccelerations* Accelerations; + FGFCS* FCS; + FGGroundReactions* GroundReactions; + FGExternalReactions* ExternalReactions; + FGBuoyantForces* BuoyantForces; + + void Debug(int from); +}; +} +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif diff --git a/src/FDM/JSBSim/input_output/FGPropertyManager.cpp b/src/FDM/JSBSim/input_output/FGPropertyManager.cpp old mode 100644 new mode 100755 index 6d907924e..39e095a8a --- a/src/FDM/JSBSim/input_output/FGPropertyManager.cpp +++ b/src/FDM/JSBSim/input_output/FGPropertyManager.cpp @@ -48,7 +48,6 @@ COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] namespace JSBSim { -bool FGPropertyManager::suppress_warning = true; std::vector FGPropertyManager::tied_properties; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -85,8 +84,8 @@ string FGPropertyManager::mkPropertyName(string name, bool lowercase) { FGPropertyManager* FGPropertyManager::GetNode (const string &path, bool create) { - SGPropertyNode* node=this->getNode(path.c_str(), create); - if (node == 0 && !suppress_warning) { + SGPropertyNode* node = getNode(path.c_str(), create); + if (node == 0) { cerr << "FGPropertyManager::GetNode() No node found for " << path << endl; } return (FGPropertyManager*)node; @@ -97,30 +96,32 @@ FGPropertyManager::GetNode (const string &path, bool create) FGPropertyManager* FGPropertyManager::GetNode (const string &relpath, int index, bool create) { - return (FGPropertyManager*)getNode(relpath.c_str(),index,create); + SGPropertyNode* node = getNode(relpath.c_str(), index, create); + if (node == 0) { + cerr << "FGPropertyManager::GetNode() No node found for " << relpath + << "[" << index << "]" << endl; + } + return (FGPropertyManager*)node; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% bool FGPropertyManager::HasNode (const string &path) { - // Checking if a node exists shouldn't write a warning if it doesn't exist - suppress_warning = true; - bool has_node = (GetNode(path, false) != 0); - suppress_warning = false; - return has_node; + const SGPropertyNode* node = getNode(path.c_str(), false); + return (node != 0); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGPropertyManager::GetName( void ) +string FGPropertyManager::GetName( void ) const { return string( getName() ); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGPropertyManager::GetPrintableName( void ) +string FGPropertyManager::GetPrintableName( void ) const { string temp_string(getName()); size_t initial_location=0; @@ -141,10 +142,11 @@ string FGPropertyManager::GetPrintableName( void ) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGPropertyManager::GetFullyQualifiedName(void) { +string FGPropertyManager::GetFullyQualifiedName(void) const +{ vector stack; stack.push_back( getDisplayName(true) ); - SGPropertyNode* tmpn=getParent(); + const SGPropertyNode* tmpn=getParent(); bool atroot=false; while( !atroot ) { stack.push_back( tmpn->getDisplayName(true) ); @@ -166,7 +168,7 @@ string FGPropertyManager::GetFullyQualifiedName(void) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGPropertyManager::GetRelativeName( const string &path ) +string FGPropertyManager::GetRelativeName( const string &path ) const { string temp_string = GetFullyQualifiedName(); size_t len = path.length(); @@ -180,42 +182,42 @@ string FGPropertyManager::GetRelativeName( const string &path ) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGPropertyManager::GetBool (const string &name, bool defaultValue) +bool FGPropertyManager::GetBool (const string &name, bool defaultValue) const { return getBoolValue(name.c_str(), defaultValue); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -int FGPropertyManager::GetInt (const string &name, int defaultValue ) +int FGPropertyManager::GetInt (const string &name, int defaultValue ) const { return getIntValue(name.c_str(), defaultValue); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -int FGPropertyManager::GetLong (const string &name, long defaultValue ) +int FGPropertyManager::GetLong (const string &name, long defaultValue ) const { return getLongValue(name.c_str(), defaultValue); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -float FGPropertyManager::GetFloat (const string &name, float defaultValue ) +float FGPropertyManager::GetFloat (const string &name, float defaultValue ) const { return getFloatValue(name.c_str(), defaultValue); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGPropertyManager::GetDouble (const string &name, double defaultValue ) +double FGPropertyManager::GetDouble (const string &name, double defaultValue ) const { return getDoubleValue(name.c_str(), defaultValue); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGPropertyManager::GetString (const string &name, string defaultValue ) +string FGPropertyManager::GetString (const string &name, string defaultValue ) const { return string(getStringValue(name.c_str(), defaultValue.c_str())); } @@ -305,8 +307,24 @@ void FGPropertyManager::SetWritable (const string &name, bool state ) void FGPropertyManager::Untie (const string &name) { - if (!untie(name.c_str())) - cerr << "Failed to untie property " << name << endl; + SGPropertyNode* property = getNode(name.c_str()); + if (!property) { + cerr << "Attempt to untie a non-existant property." << name << endl; + return; + } + + vector ::iterator it; + for (it = tied_properties.begin(); it != tied_properties.end(); ++it) { + if (*it == property) { + property->untie(); + tied_properties.erase(it); + if (FGJSBBase::debug_lvl & 0x20) cout << "Untied " << name << endl; + return; + } + } + + cerr << "Failed to untie property " << name << endl + << "JSBSim is not the owner of this property." << endl; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -323,7 +341,7 @@ void FGPropertyManager::Tie (const string &name, bool *pointer, bool useDefault) cerr << "Failed to tie property " << name << " to a pointer" << endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) cout << name << endl; + if (FGJSBBase::debug_lvl & 0x20) cout << name << endl; } } @@ -342,7 +360,7 @@ void FGPropertyManager::Tie (const string &name, int *pointer, cerr << "Failed to tie property " << name << " to a pointer" << endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) cout << name << endl; + if (FGJSBBase::debug_lvl & 0x20) cout << name << endl; } } @@ -361,7 +379,7 @@ void FGPropertyManager::Tie (const string &name, long *pointer, cerr << "Failed to tie property " << name << " to a pointer" << endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) cout << name << endl; + if (FGJSBBase::debug_lvl & 0x20) cout << name << endl; } } @@ -380,7 +398,7 @@ void FGPropertyManager::Tie (const string &name, float *pointer, cerr << "Failed to tie property " << name << " to a pointer" << endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) cout << name << endl; + if (FGJSBBase::debug_lvl & 0x20) cout << name << endl; } } @@ -398,7 +416,7 @@ void FGPropertyManager::Tie (const string &name, double *pointer, bool useDefaul cerr << "Failed to tie property " << name << " to a pointer" << endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) cout << name << endl; + if (FGJSBBase::debug_lvl & 0x20) cout << name << endl; } } diff --git a/src/FDM/JSBSim/input_output/FGPropertyManager.h b/src/FDM/JSBSim/input_output/FGPropertyManager.h index f877bfcdd..e8156a0bb 100644 --- a/src/FDM/JSBSim/input_output/FGPropertyManager.h +++ b/src/FDM/JSBSim/input_output/FGPropertyManager.h @@ -53,7 +53,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_PROPERTYMANAGER "$Id: FGPropertyManager.h,v 1.20 2011/02/13 00:42:45 jberndt Exp $" +#define ID_PROPERTYMANAGER "$Id: FGPropertyManager.h,v 1.24 2013/01/06 14:50:31 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -73,14 +73,13 @@ CLASS DOCUMENTATION CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGPropertyManager : public SGPropertyNode, public FGJSBBase +class FGPropertyManager : public SGPropertyNode { private: - static bool suppress_warning; static std::vector tied_properties; public: /// Constructor - FGPropertyManager(void) {suppress_warning = false;} + FGPropertyManager(void) {} /// Destructor virtual ~FGPropertyManager(void) {} @@ -117,18 +116,18 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase /** * Get the name of a node */ - std::string GetName( void ); + std::string GetName( void ) const; /** * Get the name of a node without underscores, etc. */ - std::string GetPrintableName( void ); + std::string GetPrintableName( void ) const; /** * Get the fully qualified name of a node * This function is very slow, so is probably useful for debugging only. */ - std::string GetFullyQualifiedName(void); + std::string GetFullyQualifiedName(void) const; /** * Get the qualified name of a node relative to given base path, @@ -137,7 +136,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * * @param path The path to strip off, if found. */ - std::string GetRelativeName( const std::string &path = "/fdm/jsbsim/" ); + std::string GetRelativeName( const std::string &path = "/fdm/jsbsim/" ) const; /** * Get a bool value for a property. @@ -153,7 +152,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * does not exist. * @return The property's value as a bool, or the default value provided. */ - bool GetBool (const std::string &name, bool defaultValue = false); + bool GetBool (const std::string &name, bool defaultValue = false) const; /** @@ -170,7 +169,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * does not exist. * @return The property's value as an int, or the default value provided. */ - int GetInt (const std::string &name, int defaultValue = 0); + int GetInt (const std::string &name, int defaultValue = 0) const; /** @@ -187,7 +186,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * does not exist. * @return The property's value as a long, or the default value provided. */ - int GetLong (const std::string &name, long defaultValue = 0L); + int GetLong (const std::string &name, long defaultValue = 0L) const; /** @@ -204,7 +203,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * does not exist. * @return The property's value as a float, or the default value provided. */ - float GetFloat (const std::string &name, float defaultValue = 0.0); + float GetFloat (const std::string &name, float defaultValue = 0.0) const; /** @@ -221,7 +220,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * does not exist. * @return The property's value as a double, or the default value provided. */ - double GetDouble (const std::string &name, double defaultValue = 0.0); + double GetDouble (const std::string &name, double defaultValue = 0.0) const; /** @@ -238,7 +237,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase * does not exist. * @return The property's value as a string, or the default value provided. */ - std::string GetString (const std::string &name, std::string defaultValue = ""); + std::string GetString (const std::string &name, std::string defaultValue = "") const; /** @@ -542,7 +541,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase std::cerr << "Failed to tie property " << name << " to functions" << std::endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) std::cout << name << std::endl; + if (FGJSBBase::debug_lvl & 0x20) std::cout << name << std::endl; } } @@ -578,7 +577,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase std::cerr << "Failed to tie property " << name << " to indexed functions" << std::endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) std::cout << name << std::endl; + if (FGJSBBase::debug_lvl & 0x20) std::cout << name << std::endl; } } @@ -616,7 +615,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase std::cerr << "Failed to tie property " << name << " to object methods" << std::endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) std::cout << name << std::endl; + if (FGJSBBase::debug_lvl & 0x20) std::cout << name << std::endl; } } @@ -653,7 +652,7 @@ class FGPropertyManager : public SGPropertyNode, public FGJSBBase std::cerr << "Failed to tie property " << name << " to indexed object methods" << std::endl; else { tied_properties.push_back(property); - if (debug_lvl & 0x20) std::cout << name << std::endl; + if (FGJSBBase::debug_lvl & 0x20) std::cout << name << std::endl; } } }; diff --git a/src/FDM/JSBSim/input_output/FGScript.cpp b/src/FDM/JSBSim/input_output/FGScript.cpp old mode 100644 new mode 100755 index acf17368e..bc512ad90 --- a/src/FDM/JSBSim/input_output/FGScript.cpp +++ b/src/FDM/JSBSim/input_output/FGScript.cpp @@ -55,7 +55,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGScript.cpp,v 1.49 2011/11/10 12:06:14 jberndt Exp $"; +static const char *IdSrc = "$Id: FGScript.cpp,v 1.50 2012/09/05 04:49:13 jberndt Exp $"; static const char *IdHdr = ID_FGSCRIPT; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -99,7 +99,7 @@ FGScript::~FGScript() //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGScript::LoadScript(string script, double deltaT) +bool FGScript::LoadScript(string script, double deltaT, const string initfile) { string aircraft="", initialize="", comparison = "", prop_name=""; string notifyPropertyName=""; @@ -174,9 +174,15 @@ bool FGScript::LoadScript(string script, double deltaT) } initialize = element->GetAttributeValue("initialize"); + if (initfile.empty()) { if (initialize.empty()) { cerr << "Initialization file must be specified in use element." << endl; return false; + } + } else { + cout << endl << "The initialization file specified in the script file (" << initialize + << ") has been overridden with a specified file (" << initfile << ")." << endl; + initialize = initfile; } } else { diff --git a/src/FDM/JSBSim/input_output/FGScript.h b/src/FDM/JSBSim/input_output/FGScript.h index c0f8acb74..187b1a8c7 100644 --- a/src/FDM/JSBSim/input_output/FGScript.h +++ b/src/FDM/JSBSim/input_output/FGScript.h @@ -49,7 +49,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FGSCRIPT "$Id: FGScript.h,v 1.21 2011/08/04 12:46:32 jberndt Exp $" +#define ID_FGSCRIPT "$Id: FGScript.h,v 1.22 2012/09/05 04:49:13 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -158,7 +158,7 @@ CLASS DOCUMENTATION comes the "run" section, where the conditions are described in "event" clauses.

@author Jon S. Berndt - @version "$Id: FGScript.h,v 1.21 2011/08/04 12:46:32 jberndt Exp $" + @version "$Id: FGScript.h,v 1.22 2012/09/05 04:49:13 jberndt Exp $" */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -180,8 +180,11 @@ public: specified simulation step size. @param script the filename (including path name, if any) for the script. @param deltaT a simulation step size. + @param initfile An optional initialization file name passed in, empty by + default. If a file name is passed in, it will override the + one present in the script. @return true if successful */ - bool LoadScript(string script, double deltaT); + bool LoadScript(string script, double deltaT, const string initfile); /** This function is called each pass through the executive Run() method IF scripting is enabled. diff --git a/src/FDM/JSBSim/input_output/FGXMLElement.cpp b/src/FDM/JSBSim/input_output/FGXMLElement.cpp old mode 100644 new mode 100755 index 96a03d74b..45132a2e0 --- a/src/FDM/JSBSim/input_output/FGXMLElement.cpp +++ b/src/FDM/JSBSim/input_output/FGXMLElement.cpp @@ -28,12 +28,13 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#include "FGXMLElement.h" - #include #include #include +#include "FGXMLElement.h" +#include "string_utilities.h" + using namespace std; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -42,7 +43,7 @@ FORWARD DECLARATIONS namespace JSBSim { -static const char *IdSrc = "$Id: FGXMLElement.cpp,v 1.33 2011/08/05 12:28:20 jberndt Exp $"; +static const char *IdSrc = "$Id: FGXMLElement.cpp,v 1.38 2012/12/13 04:41:06 jberndt Exp $"; static const char *IdHdr = ID_XMLELEMENT; bool Element::converterIsInitialized = false; @@ -230,6 +231,7 @@ Element::Element(const string& nm) convert["KG/L"]["KG/L"] = 1.0; convert["LBS/GAL"]["LBS/GAL"] = 1.0; } + attribute_key.resize(0); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -246,12 +248,24 @@ Element::~Element(void) string Element::GetAttributeValue(const string& attr) { + if (HasAttribute(attr)) return attributes[attr]; + else return (""); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +bool Element::HasAttribute(const string& attr) +{ + bool status=true; int select=-1; - for (unsigned int i=0; iGetDataAsNumber(); + double value = element->GetDataAsNumber(); + value = DisperseValue(element, value); + return value; } else { cerr << "Attempting to get single data value from multiple lines" << endl; return 0; @@ -422,6 +452,8 @@ double Element::FindElementValueAsNumberConvertTo(const string& el, const string value *= convert[supplied_units][target_units]; } + value = DisperseValue(element, value, supplied_units, target_units); + return value; } @@ -456,6 +488,8 @@ double Element::FindElementValueAsNumberConvertFromTo( const string& el, value *= convert[supplied_units][target_units]; } + value = DisperseValue(element, value, supplied_units, target_units); + return value; } @@ -486,36 +520,89 @@ FGColumnVector3 Element::FindElementTripletConvertTo( const string& target_units if (item) { value = item->GetDataAsNumber(); if (!supplied_units.empty()) value *= convert[supplied_units][target_units]; + triplet(1) = DisperseValue(item, value, supplied_units, target_units); } else { - value = 0.0; + triplet(1) = 0.0; } - triplet(1) = value; + item = FindElement("y"); if (!item) item = FindElement("pitch"); if (item) { value = item->GetDataAsNumber(); if (!supplied_units.empty()) value *= convert[supplied_units][target_units]; + triplet(2) = DisperseValue(item, value, supplied_units, target_units); } else { - value = 0.0; + triplet(2) = 0.0; } - triplet(2) = value; item = FindElement("z"); if (!item) item = FindElement("yaw"); if (item) { value = item->GetDataAsNumber(); if (!supplied_units.empty()) value *= convert[supplied_units][target_units]; + triplet(3) = DisperseValue(item, value, supplied_units, target_units); } else { - value = 0.0; + triplet(3) = 0.0; } - triplet(3) = value; return triplet; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +double Element::DisperseValue(Element *e, double val, const std::string supplied_units, const std::string target_units) +{ + double value=val; + double disp=0.0; + if (e->HasAttribute("dispersion")) { + disp = e->GetAttributeValueAsNumber("dispersion"); + if (!supplied_units.empty()) disp *= convert[supplied_units][target_units]; + string attType = e->GetAttributeValue("type"); + if (attType == "gaussian") { + value = val + disp*GaussianRandomNumber(); + } else if (attType == "uniform") { + value = val + disp * ((((double)rand()/RAND_MAX)-0.5)*2.0); + } else { + std::cerr << "Unknown dispersion type" << endl; + throw("Unknown dispersion type"); + } + + } + return value; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +double Element::GaussianRandomNumber(void) +{ + static double V1, V2, S; + static int phase = 0; + double X; + + if (phase == 0) { + V1 = V2 = S = X = 0.0; + + do { + double U1 = (double)rand() / RAND_MAX; + double U2 = (double)rand() / RAND_MAX; + + V1 = 2 * U1 - 1; + V2 = 2 * U2 - 1; + S = V1 * V1 + V2 * V2; + } while(S >= 1 || S == 0); + + X = V1 * sqrt(-2 * log(S) / S); + } else + X = V2 * sqrt(-2 * log(S) / S); + + phase = 1 - phase; + + return X; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + void Element::Print(unsigned int level) { unsigned int i, spaces; diff --git a/src/FDM/JSBSim/input_output/FGXMLElement.h b/src/FDM/JSBSim/input_output/FGXMLElement.h old mode 100644 new mode 100755 index 21af97275..f8b89c8fe --- a/src/FDM/JSBSim/input_output/FGXMLElement.h +++ b/src/FDM/JSBSim/input_output/FGXMLElement.h @@ -44,7 +44,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_XMLELEMENT "$Id: FGXMLElement.h,v 1.16 2009/10/24 22:59:30 jberndt Exp $" +#define ID_XMLELEMENT "$Id: FGXMLElement.h,v 1.17 2012/07/26 04:33:46 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -136,7 +136,7 @@ CLASS DOCUMENTATION - GAL = gallon (U.S. liquid) @author Jon S. Berndt - @version $Id: FGXMLElement.h,v 1.16 2009/10/24 22:59:30 jberndt Exp $ + @version $Id: FGXMLElement.h,v 1.17 2012/07/26 04:33:46 jberndt Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -152,6 +152,11 @@ public: /// Destructor ~Element(void); + /** Determines if an element has the supplied attribute. + @param key specifies the attribute key to retrieve the value of. + @return true or false. */ + bool HasAttribute(const std::string& key); + /** Retrieves an attribute. @param key specifies the attribute key to retrieve the value of. @return the key value (as a string), or the empty string if no such @@ -301,6 +306,8 @@ public: @return a column vector object built from the LOCATION or ORIENT components. */ FGColumnVector3 FindElementTripletConvertTo( const std::string& target_units); + double DisperseValue(Element *e, double val, const std::string supplied_units="", const std::string target_units=""); + /** This function sets the value of the parent class attribute to the supplied Element pointer. @param p pointer to the parent Element. */ @@ -335,6 +342,7 @@ private: typedef std::map > tMapConvert; static tMapConvert convert; static bool converterIsInitialized; + double GaussianRandomNumber(void); }; } // namespace JSBSim diff --git a/src/FDM/JSBSim/input_output/FGXMLFileRead.h b/src/FDM/JSBSim/input_output/FGXMLFileRead.h old mode 100644 new mode 100755 index d7366db7c..09c889d1b --- a/src/FDM/JSBSim/input_output/FGXMLFileRead.h +++ b/src/FDM/JSBSim/input_output/FGXMLFileRead.h @@ -43,7 +43,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_XMLFILEREAD "$Id: FGXMLFileRead.h,v 1.5 2009/11/28 20:12:47 andgi Exp $" +#define ID_XMLFILEREAD "$Id: FGXMLFileRead.h,v 1.7 2012/12/12 06:19:57 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -58,7 +58,12 @@ public: protected: Element* document; - Element* LoadXMLDocument(std::string XML_filename) + Element* LoadXMLDocument(std::string XML_filename, bool verbose=true) + { + return LoadXMLDocument(XML_filename, file_parser, true); + } + + Element* LoadXMLDocument(std::string XML_filename, FGXMLParse& fparse, bool verbose=true) { std::ifstream infile; @@ -66,7 +71,7 @@ protected: if (XML_filename.find(".xml") == std::string::npos) XML_filename += ".xml"; infile.open(XML_filename.c_str()); if ( !infile.is_open()) { - std::cerr << "Could not open file: " << XML_filename << std::endl; + if (verbose) std::cerr << "Could not open file: " << XML_filename << std::endl; return 0L; } } else { @@ -74,8 +79,8 @@ protected: return 0L; } - readXML(infile, file_parser, XML_filename); - document = file_parser.GetDocument(); + readXML(infile, fparse, XML_filename); + document = fparse.GetDocument(); infile.close(); return document; diff --git a/src/FDM/JSBSim/input_output/FGXMLParse.cpp b/src/FDM/JSBSim/input_output/FGXMLParse.cpp old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/input_output/FGXMLParse.h b/src/FDM/JSBSim/input_output/FGXMLParse.h old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/input_output/net_fdm.hxx b/src/FDM/JSBSim/input_output/net_fdm.hxx index f649a5f2e..db9022a0b 100644 --- a/src/FDM/JSBSim/input_output/net_fdm.hxx +++ b/src/FDM/JSBSim/input_output/net_fdm.hxx @@ -6,7 +6,7 @@ // // This file is in the Public Domain, and comes with no warranty. // -// $Id$ +// $Id: net_fdm.hxx,v 1.5 2011/11/10 12:06:14 jberndt Exp $ #ifndef _NET_FDM_HXX @@ -14,9 +14,7 @@ #include // time_t - -#include //not required for JSBSim - +#include // NOTE: this file defines an external interface structure. Due to // variability between platforms and architectures, we only used fixed @@ -40,26 +38,26 @@ public: FG_MAX_TANKS = 4 }; - uint32_t version; // increment when data values change - uint32_t padding; // padding + uint32_t version; // increment when data values change + uint32_t padding; // padding // Positions - double longitude; // geodetic (radians) - double latitude; // geodetic (radians) - double altitude; // above sea level (meters) - float agl; // above ground level (meters) - float phi; // roll (radians) - float theta; // pitch (radians) - float psi; // yaw or true heading (radians) + double longitude; // geodetic (radians) + double latitude; // geodetic (radians) + double altitude; // above sea level (meters) + float agl; // above ground level (meters) + float phi; // roll (radians) + float theta; // pitch (radians) + float psi; // yaw or true heading (radians) float alpha; // angle of attack (radians) float beta; // side slip angle (radians) // Velocities - float phidot; // roll rate (radians/sec) - float thetadot; // pitch rate (radians/sec) - float psidot; // yaw rate (radians/sec) - float vcas; // calibrated airspeed - float climb_rate; // feet per second + float phidot; // roll rate (radians/sec) + float thetadot; // pitch rate (radians/sec) + float psidot; // yaw rate (radians/sec) + float vcas; // calibrated airspeed + float climb_rate; // feet per second float v_north; // north velocity in local/body frame, fps float v_east; // east velocity in local/body frame, fps float v_down; // down/vertical velocity in local/body frame, fps @@ -71,31 +69,31 @@ public: // frame relative to local airmass, fps // Accelerations - float A_X_pilot; // X accel in body frame ft/sec^2 - float A_Y_pilot; // Y accel in body frame ft/sec^2 - float A_Z_pilot; // Z accel in body frame ft/sec^2 + float A_X_pilot; // X accel in body frame ft/sec^2 + float A_Y_pilot; // Y accel in body frame ft/sec^2 + float A_Z_pilot; // Z accel in body frame ft/sec^2 // Stall float stall_warning; // 0.0 - 1.0 indicating the amount of stall - float slip_deg; // slip ball deflection + float slip_deg; // slip ball deflection // Pressure // Engine status - uint32_t num_engines; // Number of valid engines + uint32_t num_engines; // Number of valid engines uint32_t eng_state[FG_MAX_ENGINES];// Engine state (off, cranking, running) - float rpm[FG_MAX_ENGINES]; // Engine RPM rev/min + float rpm[FG_MAX_ENGINES]; // Engine RPM rev/min float fuel_flow[FG_MAX_ENGINES]; // Fuel flow gallons/hr float fuel_px[FG_MAX_ENGINES]; // Fuel pressure psi - float egt[FG_MAX_ENGINES]; // Exhuast gas temp deg F - float cht[FG_MAX_ENGINES]; // Cylinder head temp deg F + float egt[FG_MAX_ENGINES]; // Exhuast gas temp deg F + float cht[FG_MAX_ENGINES]; // Cylinder head temp deg F float mp_osi[FG_MAX_ENGINES]; // Manifold pressure - float tit[FG_MAX_ENGINES]; // Turbine Inlet Temperature + float tit[FG_MAX_ENGINES]; // Turbine Inlet Temperature float oil_temp[FG_MAX_ENGINES]; // Oil temp deg F float oil_px[FG_MAX_ENGINES]; // Oil pressure psi // Consumables - uint32_t num_tanks; // Max number of fuel tanks + uint32_t num_tanks; // Max number of fuel tanks float fuel_quantity[FG_MAX_TANKS]; // Gear status diff --git a/src/FDM/JSBSim/input_output/string_utilities.h b/src/FDM/JSBSim/input_output/string_utilities.h index b55073f8b..1476e3eb1 100644 --- a/src/FDM/JSBSim/input_output/string_utilities.h +++ b/src/FDM/JSBSim/input_output/string_utilities.h @@ -46,7 +46,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_STRINGUTILS "$Id: string_utilities.h,v 1.14 2010/08/21 17:13:47 jberndt Exp $" +#define ID_STRINGUTILS "$Id: string_utilities.h,v 1.15 2012/11/17 19:31:26 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -125,7 +125,10 @@ CLASS DECLARATION bool is_number(const string& str) { - return (str.find_first_not_of("+-.0123456789Ee") == string::npos); + if (str.size()) + return (str.find_first_not_of("+-.0123456789Ee") == string::npos); + else + return false; } vector split(string str, char d) diff --git a/src/FDM/JSBSim/math/FGCondition.cpp b/src/FDM/JSBSim/math/FGCondition.cpp index 16106874a..ddaf41829 100644 --- a/src/FDM/JSBSim/math/FGCondition.cpp +++ b/src/FDM/JSBSim/math/FGCondition.cpp @@ -45,7 +45,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGCondition.cpp,v 1.16 2011/11/10 12:06:14 jberndt Exp $"; +static const char *IdSrc = "$Id: FGCondition.cpp,v 1.17 2012/10/27 20:29:01 jberndt Exp $"; static const char *IdHdr = ID_CONDITION; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -81,13 +81,16 @@ FGCondition::FGCondition(Element* element, FGPropertyManager* PropertyManager) : } condition_element = element->GetElement(); - while (condition_element) { - conditions.push_back(new FGCondition(condition_element, PropertyManager)); - condition_element = element->GetNextElement(); - } - for (unsigned int i=0; iGetNumDataLines(); i++) { - string data = element->GetDataLine(i); - conditions.push_back(new FGCondition(data, PropertyManager)); + if (condition_element != 0) { + while (condition_element) { + conditions.push_back(new FGCondition(condition_element, PropertyManager)); + condition_element = element->GetNextElement(); + } + } else { + for (unsigned int i=0; iGetNumDataLines(); i++) { + string data = element->GetDataLine(i); + conditions.push_back(new FGCondition(data, PropertyManager)); + } } Debug(0); @@ -242,37 +245,43 @@ bool FGCondition::Evaluate(void ) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGCondition::PrintCondition(void ) +void FGCondition::PrintCondition(string indent) { string scratch; if (isGroup) { + switch(Logic) { case (elUndef): scratch = " UNSET"; cerr << "unset logic for test condition" << endl; break; case (eAND): - scratch = " if all of the following are true:"; + scratch = indent + "if all of the following are true: {"; break; case (eOR): - scratch = " if any of the following are true:"; + scratch = indent + "if any of the following are true: {"; break; default: scratch = " UNKNOWN"; cerr << "Unknown logic for test condition" << endl; } - cout << scratch << endl; - for (unsigned int i=0; iPrintCondition(); + + for (unsigned int i=0; iPrintCondition(indent + " "); + cout << endl; + } + + cout << indent << "}"; } else { if (TestParam2 != 0L) - cout << " " << TestParam1->GetName() << " " + cout << indent << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName(); else - cout << " " << TestParam1->GetName() << " " + cout << indent << TestParam1->GetName() << " " << conditional << " " << TestValue; } } diff --git a/src/FDM/JSBSim/math/FGCondition.h b/src/FDM/JSBSim/math/FGCondition.h index 06632a76d..0d776357b 100644 --- a/src/FDM/JSBSim/math/FGCondition.h +++ b/src/FDM/JSBSim/math/FGCondition.h @@ -44,7 +44,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_CONDITION "$Id: FGCondition.h,v 1.6 2011/04/05 20:20:21 andgi Exp $" +#define ID_CONDITION "$Id: FGCondition.h,v 1.7 2012/10/27 20:29:01 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -75,7 +75,7 @@ public: ~FGCondition(void); bool Evaluate(void); - void PrintCondition(void); + void PrintCondition(std::string indent=" "); private: enum eComparison {ecUndef=0, eEQ, eNE, eGT, eGE, eLT, eLE}; diff --git a/src/FDM/JSBSim/math/FGFunction.cpp b/src/FDM/JSBSim/math/FGFunction.cpp old mode 100644 new mode 100755 index 38322bd96..dc1b830da --- a/src/FDM/JSBSim/math/FGFunction.cpp +++ b/src/FDM/JSBSim/math/FGFunction.cpp @@ -43,7 +43,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGFunction.cpp,v 1.43 2012/02/05 11:15:54 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGFunction.cpp,v 1.46 2012/09/25 12:43:13 jberndt Exp $"; static const char *IdHdr = ID_FUNCTION; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -83,6 +83,8 @@ const std::string FGFunction::avg_string = "avg"; const std::string FGFunction::fraction_string = "fraction"; const std::string FGFunction::mod_string = "mod"; const std::string FGFunction::random_string = "random"; +const std::string FGFunction::urandom_string = "urandom"; +const std::string FGFunction::pi_string = "pi"; const std::string FGFunction::integer_string = "integer"; const std::string FGFunction::rotation_alpha_local_string = "rotation_alpha_local"; const std::string FGFunction::rotation_beta_local_string = "rotation_beta_local"; @@ -101,6 +103,7 @@ const std::string FGFunction::or_string = "or"; const std::string FGFunction::not_string = "not"; const std::string FGFunction::ifthen_string = "ifthen"; const std::string FGFunction::switch_string = "switch"; +const std::string FGFunction::interpolate1d_string = "interpolate1d"; FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& prefix) : PropertyManager(propMan), Prefix(prefix) @@ -110,11 +113,18 @@ FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& pr cached = false; cachedValue = -HUGE_VAL; invlog2val = 1.0/log10(2.0); + pCopyTo = 0L; Name = el->GetAttributeValue("name"); operation = el->GetName(); if (operation == function_string) { + sCopyTo = el->GetAttributeValue("copyto"); + if (!sCopyTo.empty()) { + pCopyTo = PropertyManager->GetNode(sCopyTo); + if (pCopyTo == 0L) cerr << "Property \"" << sCopyTo << "\" must be previously defined in function " + << Name << endl; + } Type = eTopLevel; } else if (operation == product_string) { Type = eProduct; @@ -166,6 +176,10 @@ FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& pr Type = eMod; } else if (operation == random_string) { Type = eRandom; + } else if (operation == urandom_string) { + Type = eUrandom; + } else if (operation == pi_string) { + Type = ePi; } else if (operation == rotation_alpha_local_string) { Type = eRotation_alpha_local; } else if (operation == rotation_beta_local_string) { @@ -198,12 +212,14 @@ FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& pr Type = eIfThen; } else if (operation == switch_string) { Type = eSwitch; + } else if (operation == interpolate1d_string) { + Type = eInterpolate1D; } else if (operation != description_string) { cerr << "Bad operation " << operation << " detected in configuration file" << endl; } element = el->GetElement(); - if (!element) { + if (!element && Type != eRandom && Type != eUrandom && Type != ePi) { cerr << fgred << highint << endl; cerr << " No element was specified as an argument to the \"" << operation << "\" operation" << endl; cerr << " This can happen when, for instance, a cos operation is specified and a " << endl; @@ -263,6 +279,8 @@ FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& pr operation == integer_string || operation == mod_string || operation == random_string || + operation == urandom_string || + operation == pi_string || operation == avg_string || operation == rotation_alpha_local_string|| operation == rotation_beta_local_string|| @@ -279,7 +297,8 @@ FGFunction::FGFunction(FGPropertyManager* propMan, Element* el, const string& pr operation == or_string || operation == not_string || operation == ifthen_string || - operation == switch_string) + operation == switch_string || + operation == interpolate1d_string) { Parameters.push_back(new FGFunction(PropertyManager, element, Prefix)); } else if (operation != description_string) { @@ -334,10 +353,13 @@ double FGFunction::GetValue(void) const if (cached) return cachedValue; - temp = Parameters[0]->GetValue(); + if ( Type != eRandom + && Type != eUrandom + && Type != ePi ) temp = Parameters[0]->GetValue(); switch (Type) { case eTopLevel: + if (pCopyTo) pCopyTo->setDoubleValue(temp); break; case eProduct: for (i=1;iGetValue())?1:0; break; @@ -499,6 +527,27 @@ double FGFunction::GetValue(void) const } } break; + case eInterpolate1D: + { + unsigned int sz = Parameters.size(); + if (temp <= Parameters[1]->GetValue()) { + temp = Parameters[2]->GetValue(); + } else if (temp >= Parameters[sz-2]->GetValue()) { + temp = Parameters[sz-1]->GetValue(); + } else { + for (unsigned int i=1; i<=sz-4; i+=2) { + if (temp < Parameters[i+2]->GetValue()) { + double factor = (temp - Parameters[i]->GetValue()) / + (Parameters[i+2]->GetValue() - Parameters[i]->GetValue()); + double span = Parameters[i+3]->GetValue() - Parameters[i+1]->GetValue(); + double val = factor*span; + temp = Parameters[i+1]->GetValue() + val; + break; + } + } + } + } + break; case eRotation_alpha_local: if (Parameters.size()==6) // calculates local angle of attack for skydiver body component //Euler angles from the intermediate body frame to the local body frame must be from a z-y-x axis rotation order @@ -731,7 +780,11 @@ void FGFunction::bind(void) } } + if (PropertyManager->HasNode(tmp)) { + cout << "Property " << tmp << " has already been successfully bound (late)." << endl; + } else { PropertyManager->Tie( tmp, this, &FGFunction::GetValue); + } } } diff --git a/src/FDM/JSBSim/math/FGFunction.h b/src/FDM/JSBSim/math/FGFunction.h old mode 100644 new mode 100755 index f3277eb6c..785648a82 --- a/src/FDM/JSBSim/math/FGFunction.h +++ b/src/FDM/JSBSim/math/FGFunction.h @@ -42,7 +42,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FUNCTION "$Id: FGFunction.h,v 1.24 2011/08/06 13:10:00 jberndt Exp $" +#define ID_FUNCTION "$Id: FGFunction.h,v 1.26 2012/09/25 12:43:13 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -97,8 +97,11 @@ A function definition consists of an operation, a value, a table, or a property - not (takes 1 args) - if-then (takes 2-3 args) - switch (takes 2 or more args) -- random (Gaussian random number) +- random (Gaussian distribution random number) +- urandom (Uniform random number between -1 and +1) +- pi - integer +- interpolate 1-dimensional (takes a minimum of five arguments, odd number) An operation is defined in the configuration file as in the following example: @@ -235,12 +238,14 @@ private: static const std::string acos_string; static const std::string atan_string; static const std::string atan2_string; + static const std::string pi_string; static const std::string min_string; static const std::string max_string; static const std::string avg_string; static const std::string fraction_string; static const std::string mod_string; static const std::string random_string; + static const std::string urandom_string; static const std::string integer_string; static const std::string rotation_alpha_local_string; static const std::string rotation_beta_local_string; @@ -258,14 +263,18 @@ private: static const std::string not_string; static const std::string ifthen_string; static const std::string switch_string; + static const std::string interpolate1d_string; double cachedValue; enum functionType {eTopLevel=0, eProduct, eDifference, eSum, eQuotient, ePow, eExp, eAbs, eSign, eSin, eCos, eTan, eASin, eACos, eATan, eATan2, - eMin, eMax, eAvg, eFrac, eInteger, eMod, eRandom, eLog2, eLn, - eLog10, eLT, eLE, eGE, eGT, eEQ, eNE, eAND, eOR, eNOT, - eIfThen, eSwitch, eRotation_alpha_local, eRotation_beta_local, - eRotation_gamma_local, eRotation_bf_to_wf, eRotation_wf_to_bf} Type; + eMin, eMax, eAvg, eFrac, eInteger, eMod, eRandom, eUrandom, ePi, + eLog2, eLn, eLog10, eLT, eLE, eGE, eGT, eEQ, eNE, eAND, eOR, eNOT, + eIfThen, eSwitch, eInterpolate1D, eRotation_alpha_local, + eRotation_beta_local, eRotation_gamma_local, eRotation_bf_to_wf, + eRotation_wf_to_bf} Type; std::string Name; + std::string sCopyTo; // Property name to copy function value to + FGPropertyManager* pCopyTo; // Property node for CopyTo property string unsigned int GetBinary(double) const; void bind(void); diff --git a/src/FDM/JSBSim/math/FGMatrix33.cpp b/src/FDM/JSBSim/math/FGMatrix33.cpp index 7f5707040..ef41cfb78 100644 --- a/src/FDM/JSBSim/math/FGMatrix33.cpp +++ b/src/FDM/JSBSim/math/FGMatrix33.cpp @@ -49,7 +49,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGMatrix33.cpp,v 1.12 2011/12/10 15:49:21 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGMatrix33.cpp,v 1.14 2012/11/22 22:04:06 bcoconni Exp $"; static const char *IdHdr = ID_MATRIX33; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -106,7 +106,7 @@ string FGMatrix33::Dump(const string& delimiter, const string& prefix) const //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGQuaternion FGMatrix33::GetQuaternion(void) +FGQuaternion FGMatrix33::GetQuaternion(void) const { FGQuaternion Q; @@ -155,6 +155,38 @@ FGQuaternion FGMatrix33::GetQuaternion(void) return (Q); } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Compute the Euler-angles +// Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8.. + +FGColumnVector3 FGMatrix33::GetEuler(void) const +{ + FGColumnVector3 mEulerAngles; + + if (data[8] == 0.0) + mEulerAngles(1) = 0.5*M_PI; + else + mEulerAngles(1) = atan2(data[7], data[8]); + + if (data[6] < -1.0) + mEulerAngles(2) = 0.5*M_PI; + else if (1.0 < data[6]) + mEulerAngles(2) = -0.5*M_PI; + else + mEulerAngles(2) = asin(-data[6]); + + if (data[0] == 0.0) + mEulerAngles(3) = 0.5*M_PI; + else { + double psi = atan2(data[3], data[0]); + if (psi < 0.0) + psi += 2*M_PI; + mEulerAngles(3) = psi; + } + + return mEulerAngles; +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ostream& operator<<(ostream& os, const FGMatrix33& M) @@ -475,4 +507,4 @@ FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const return FGColumnVector3( tmp1, tmp2, tmp3 ); } -} \ No newline at end of file +} diff --git a/src/FDM/JSBSim/math/FGMatrix33.h b/src/FDM/JSBSim/math/FGMatrix33.h index 3f9fe9368..2f266bbc3 100644 --- a/src/FDM/JSBSim/math/FGMatrix33.h +++ b/src/FDM/JSBSim/math/FGMatrix33.h @@ -49,7 +49,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.15 2011/12/10 15:49:21 bcoconni Exp $" +#define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.17 2012/11/22 22:04:06 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -276,7 +276,11 @@ public: /** Returns the quaternion associated with this direction cosine (rotation) matrix. */ - FGQuaternion GetQuaternion(void); + FGQuaternion GetQuaternion(void) const; + + /** Returns the Euler angle column vector associated with this matrix. + */ + FGColumnVector3 GetEuler() const; /** Determinant of the matrix. @return the determinant of the matrix. diff --git a/src/FDM/JSBSim/math/FGModelFunctions.cpp b/src/FDM/JSBSim/math/FGModelFunctions.cpp old mode 100644 new mode 100755 index 85b039f98..856f43a1a --- a/src/FDM/JSBSim/math/FGModelFunctions.cpp +++ b/src/FDM/JSBSim/math/FGModelFunctions.cpp @@ -46,7 +46,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGModelFunctions.cpp,v 1.5 2012/04/13 13:25:52 jberndt Exp $"; +static const char *IdSrc = "$Id: FGModelFunctions.cpp,v 1.6 2012/09/05 05:00:57 jberndt Exp $"; static const char *IdHdr = ID_MODELFUNCTIONS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -147,9 +147,10 @@ void FGModelFunctions::PostLoad(Element* el, FGPropertyManager* PM, string prefi void FGModelFunctions::RunPreFunctions(void) { - vector ::iterator it; - for (it = PreFunctions.begin(); it != PreFunctions.end(); it++) - (*it)->cacheValue(true); + unsigned int sz = PreFunctions.size(); + for (unsigned int i=0; icacheValue(true); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -160,9 +161,10 @@ void FGModelFunctions::RunPreFunctions(void) void FGModelFunctions::RunPostFunctions(void) { - vector ::iterator it; - for (it = PostFunctions.begin(); it != PostFunctions.end(); it++) - (*it)->GetValue(); + unsigned int sz = PostFunctions.size(); + for (unsigned int i=0; icacheValue(true); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/math/FGModelFunctions.h b/src/FDM/JSBSim/math/FGModelFunctions.h old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/math/FGNelderMead.cpp b/src/FDM/JSBSim/math/FGNelderMead.cpp new file mode 100755 index 000000000..55fe50018 --- /dev/null +++ b/src/FDM/JSBSim/math/FGNelderMead.cpp @@ -0,0 +1,376 @@ +/* + * FGNelderMead.cpp + * Copyright (C) James Goppert 2010 + * + * FGNelderMead.cpp 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. + * + * FGNelderMead.cpp 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, see . + */ + +#include "FGNelderMead.h" +#include +#include +#include +#include +#include +#include +#include + +namespace JSBSim +{ + +FGNelderMead::FGNelderMead(Function * f, const std::vector & initialGuess, + const std::vector & lowerBound, + const std::vector & upperBound, + const std::vector & initialStepSize, int iterMax, + double rtol, double abstol, double speed, double randomization, + bool showConvergeStatus, + bool showSimplex, bool pause, Callback * callback) : + m_f(f), m_callback(callback), m_randomization(randomization), + m_lowerBound(lowerBound), m_upperBound(upperBound), + m_nDim(initialGuess.size()), m_nVert(m_nDim+1), + m_iMax(1), m_iNextMax(1), m_iMin(1), + m_simplex(m_nVert), m_cost(m_nVert), m_elemSum(m_nDim), + m_status(1), + initialGuess(initialGuess), initialStepSize(initialStepSize), + iterMax(iterMax), iter(), rtol(rtol), abstol(abstol), + speed(speed), showConvergeStatus(showConvergeStatus), showSimplex(showSimplex), + pause(pause), rtolI(), minCostPrevResize(1), minCost(), minCostPrev(), maxCost(), + nextMaxCost() +{ + srand ( time(NULL) ); // seed random number generator +} + +void FGNelderMead::update() +{ + std::cout.precision(3); + + // reinitialize simplex whenever rtol condition is met + if ( rtolI < rtol || iter == 0) + { + std::vector guess(m_nDim); + if (iter == 0) + { + //std::cout << "constructing simplex" << std::endl; + guess = initialGuess; + } + else + { + if (std::abs(minCost-minCostPrevResize) < abstol) + { + std::cout << "\nunable to escape local minimum" << std::endl; + m_status = -1; + return; + } + //std::cout << "reinitializing step size" << std::endl; + guess = m_simplex[m_iMin]; + minCostPrevResize = minCost; + } + constructSimplex(guess,initialStepSize); + } + + // find vertex costs + for (int vertex=0;vertexeval(m_simplex[vertex]); + } + catch (const std::exception & e) + { + m_status = -1; + throw; + return; + } + } + + // find max cost, next max cost, and min cost + m_iMax = m_iNextMax = m_iMin = 0; + for (int vertex=0;vertex m_cost[m_iMax] ) + { + m_iMax = vertex; + } + else if ( m_cost[vertex] > m_cost[m_iNextMax] || m_iMax == m_iNextMax ) m_iNextMax = vertex; + else if ( m_cost[vertex] < m_cost[m_iMin] ) m_iMin = vertex; + + } + + // callback + if (m_callback) m_callback->eval(m_simplex[m_iMin]); + + // compute relative tolerance + rtolI = 2*std::abs(m_cost[m_iMax] - + m_cost[m_iMin])/(std::abs(m_cost[m_iMax]+std::abs(m_cost[m_iMin])+ + std::numeric_limits::epsilon())); + + // check for max iteration break condition + if (iter > iterMax) + { + std::cout << "\nmax iterations exceeded" << std::endl; + m_status = -1; + return; + } + // check for convergence break condition + else if ( m_cost[m_iMin] < abstol ) + { + std::cout << "\nsimplex converged" << std::endl; + m_status = 0; + return; + } + + // compute element sum of simplex vertices + for (int dim=0;dim::epsilon() ) + < minCost && minCostPrev != 0) + { + std::cout << "\twarning: simplex cost increased" + << std::scientific + << "\n\tcost: " << minCost + << "\n\tcost previous: " << minCostPrev + << std::fixed << std::endl; + } + + std::cout << "i: " << iter + << std::scientific + << "\tcost: " << m_cost[m_iMin] + << "\trtol: " << rtolI + << std::fixed + << "\talpha: " << m_simplex[m_iMin][2]*180/M_PI + << "\tbeta: " << m_simplex[m_iMin][5]*180/M_PI + << "\tthrottle: " << m_simplex[m_iMin][0] + << "\televator: " << m_simplex[m_iMin][1] + << "\taileron: " << m_simplex[m_iMin][3] + << "\trudder: " << m_simplex[m_iMin][4] + << std::endl; + } + if (showSimplex) + { + std::cout << "simplex: " << std::endl;; + for (int j=0;j nextMaxCost) + { + // 1d contraction + costTry = tryStretch(1./speed); + //std::cout << "cost Try 2: " << costTry << std::endl; + + // if greater than max cost, contract about min + if (costTry > maxCost) + { + if (showSimplex) + std::cout << "multiD contraction about: " << m_iMin << std::endl; + contract(); + } + else + { + if (showSimplex) + std::cout << "contraction about: " << m_iMin << std::endl; + } + } + } + + catch (const std::exception & e) + { + throw; + m_status = -1; + return; + } + + // iteration + iter++; + +} + +int FGNelderMead::status() +{ + return m_status; +} + +double FGNelderMead::getRandomFactor() +{ + double randFact = 1+(float(rand() % 1000)/500-1)*m_randomization; + //std::cout << "random factor: " << randFact << std::endl;; + return randFact; +} + +std::vector FGNelderMead::getSolution() +{ + return m_simplex[m_iMin]; +} + +double FGNelderMead::tryStretch(double factor) +{ + // randomize factor so we can avoid locking situations + factor = factor*getRandomFactor(); + + // create trial vertex + double a= (1.0-factor)/m_nDim; + double b = a - factor; + std::vector tryVertex(m_nDim); + for (int dim=0;dimeval(tryVertex); + costTry = m_f->eval(tryVertex); + } + catch(const std::exception & e) + { + throw; + return 0; + } + + if (std::abs(costTry0-costTry) > std::numeric_limits::epsilon()) + { + //std::cout << "\twarning: dynamics not stable!" << std::endl; + //return 1000000*m_cost[m_iMax]; + } + + // if trial cost lower than max + if (costTry < m_cost[m_iMax]) + { + // update the element sum of the simplex + for (int dim=0;dim & guess, + const std::vector & stepSize) +{ + for (int vertex=0;vertex & vertex, + const std::vector & lowerBound, + const std::vector & upperBound) +{ + for (int dim=0;dim upperBound[dim]) vertex[dim] = upperBound[dim]; + else if (vertex[dim] < lowerBound[dim]) vertex[dim] = lowerBound[dim]; + } +} + +} // JSBSim + + +// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/math/FGNelderMead.h b/src/FDM/JSBSim/math/FGNelderMead.h new file mode 100755 index 000000000..64eb43fc4 --- /dev/null +++ b/src/FDM/JSBSim/math/FGNelderMead.h @@ -0,0 +1,95 @@ +/* + * FGNelderMead.h + * Copyright (C) James Goppert 2010 + * + * FGNelderMead.h 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. + * + * FGNelderMead.h 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, see . + */ + +#ifndef JSBSim_FGNelderMead_H +#define JSBSim_FGNelderMead_H + +#include +#include +#include + +namespace JSBSim +{ + +class FGNelderMead +{ +public: + class Function + { + public: + virtual double eval(const std::vector & v) = 0; + virtual ~Function() {}; + }; + class Callback + { + public: + virtual void eval(const std::vector & v) = 0; + virtual ~Callback() {}; + }; + + FGNelderMead(Function * f, const std::vector & initialGuess, + const std::vector & lowerBound, + const std::vector & upperBound, + const std::vector & initialStepSize, int iterMax=2000, + double rtol=std::numeric_limits::epsilon(), + double abstol=std::numeric_limits::epsilon(), + double speed = 2.0, + double randomization=0.1, + bool showConvergeStatus=true,bool showSimplex=false, + bool pause=false, + Callback * callback=NULL); + std::vector getSolution(); + + void update(); + int status(); + +private: + // attributes + Function * m_f; + Callback * m_callback; + double m_randomization; + const std::vector & m_lowerBound; + const std::vector & m_upperBound; + int m_nDim, m_nVert, m_iMax, m_iNextMax, m_iMin; + std::vector< std::vector > m_simplex; + std::vector m_cost; + std::vector m_elemSum; + int m_status; + const std::vector & initialGuess; + const std::vector & initialStepSize; + int iterMax, iter; + double rtol,abstol,speed; + bool showConvergeStatus, showSimplex, pause; + double rtolI, minCostPrevResize, minCost, minCostPrev, + maxCost, nextMaxCost; + + // methods + double getRandomFactor(); + double tryStretch(double factor); + void contract(); + void constructSimplex(const std::vector & guess, const std::vector & stepSize); + void boundVertex(std::vector & vertex, + const std::vector & upperBound, + const std::vector & lowerBound); +}; + +} // JSBSim + +#endif + +// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/math/FGParameter.h b/src/FDM/JSBSim/math/FGParameter.h old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/math/FGPropertyValue.cpp b/src/FDM/JSBSim/math/FGPropertyValue.cpp old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/math/FGPropertyValue.h b/src/FDM/JSBSim/math/FGPropertyValue.h old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/math/FGQuaternion.cpp b/src/FDM/JSBSim/math/FGQuaternion.cpp index f147a3a2b..6f0295555 100644 --- a/src/FDM/JSBSim/math/FGQuaternion.cpp +++ b/src/FDM/JSBSim/math/FGQuaternion.cpp @@ -40,6 +40,7 @@ SENTRY #include #include +#include #include #include using std::cerr; @@ -57,7 +58,7 @@ using std::endl; namespace JSBSim { -static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.20 2011/10/31 14:54:40 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.22 2012/09/17 12:27:44 jberndt Exp $"; static const char *IdHdr = ID_QUATERNION; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -223,28 +224,8 @@ void FGQuaternion::ComputeDerivedUnconditional(void) const mTInv.T(); // Compute the Euler-angles - // Also see Jack Kuipers, "Quaternions and Rotation Sequences", section 7.8.. - if (mT(3,3) == 0.0) - mEulerAngles(ePhi) = 0.5*M_PI; - else - mEulerAngles(ePhi) = atan2(mT(2,3), mT(3,3)); - - if (mT(1,3) < -1.0) - mEulerAngles(eTht) = 0.5*M_PI; - else if (1.0 < mT(1,3)) - mEulerAngles(eTht) = -0.5*M_PI; - else - mEulerAngles(eTht) = asin(-mT(1,3)); - - if (mT(1,1) == 0.0) - mEulerAngles(ePsi) = 0.5*M_PI; - else { - double psi = atan2(mT(1,2), mT(1,1)); - if (psi < 0.0) - psi += 2*M_PI; - mEulerAngles(ePsi) = psi; - } + mEulerAngles = mT.GetEuler(); // FIXME: may be one can compute those values easier ??? mEulerSines(ePhi) = sin(mEulerAngles(ePhi)); @@ -258,6 +239,18 @@ void FGQuaternion::ComputeDerivedUnconditional(void) const //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +std::string FGQuaternion::Dump(const std::string& delimiter) const +{ + std::ostringstream buffer; + buffer << std::setprecision(16) << data[0] << delimiter; + buffer << std::setprecision(16) << data[1] << delimiter; + buffer << std::setprecision(16) << data[2] << delimiter; + buffer << std::setprecision(16) << data[3]; + return buffer.str(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + std::ostream& operator<<(std::ostream& os, const FGQuaternion& q) { os << q(1) << " , " << q(2) << " , " << q(3) << " , " << q(4); diff --git a/src/FDM/JSBSim/math/FGQuaternion.h b/src/FDM/JSBSim/math/FGQuaternion.h index 97882e29c..e52ccc549 100644 --- a/src/FDM/JSBSim/math/FGQuaternion.h +++ b/src/FDM/JSBSim/math/FGQuaternion.h @@ -40,6 +40,7 @@ SENTRY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +#include #include "FGJSBBase.h" #include "FGColumnVector3.h" @@ -47,7 +48,7 @@ SENTRY DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.24 2011/12/10 15:49:21 bcoconni Exp $" +#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.25 2012/09/05 05:00:57 jberndt Exp $" namespace JSBSim { @@ -463,6 +464,8 @@ public: Useful for initialization of increments */ static FGQuaternion zero(void) { return FGQuaternion( 0.0, 0.0, 0.0, 0.0 ); } + std::string Dump(const std::string& delimiter) const; + friend FGQuaternion QExp(const FGColumnVector3& omega); private: diff --git a/src/FDM/JSBSim/math/FGRealValue.cpp b/src/FDM/JSBSim/math/FGRealValue.cpp old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/math/FGRealValue.h b/src/FDM/JSBSim/math/FGRealValue.h old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/math/FGStateSpace.cpp b/src/FDM/JSBSim/math/FGStateSpace.cpp new file mode 100755 index 000000000..b65995098 --- /dev/null +++ b/src/FDM/JSBSim/math/FGStateSpace.cpp @@ -0,0 +1,172 @@ +/* + * FGStateSpace.cpp + * Copyright (C) James Goppert 2010 + * + * FGStateSpace.cpp 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. + * + * FGStateSpace.cpp 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, see . + */ + +#include "FGStateSpace.h" +#include +#include + +namespace JSBSim +{ + +void FGStateSpace::linearize( + std::vector x0, + std::vector u0, + std::vector y0, + std::vector< std::vector > & A, + std::vector< std::vector > & B, + std::vector< std::vector > & C, + std::vector< std::vector > & D) +{ + double h = 1e-4; + + // A, d(x)/dx + numericalJacobian(A,x,x,x0,x0,h,true); + // B, d(x)/du + numericalJacobian(B,x,u,x0,u0,h,true); + // C, d(y)/dx + numericalJacobian(C,y,x,y0,x0,h); + // D, d(y)/du + numericalJacobian(D,y,u,y0,u0,h); + +} + +void FGStateSpace::numericalJacobian(std::vector< std::vector > & J, ComponentVector & y, + ComponentVector & x, const std::vector & y0, const std::vector & x0, double h, bool computeYDerivative) +{ + int nX = x.getSize(); + int nY = y.getSize(); + double f1 = 0, f2 = 0, fn1 = 0, fn2 = 0; + J.resize(nY); + for (int iY=0;iYGetDebugLevel() > 1) + { + std::cout << std::scientific << "\ty:\t" << y.getName(iY) << "\tx:\t" + << x.getName(iX) + << "\tfn2:\t" << fn2 << "\tfn1:\t" << fn1 + << "\tf1:\t" << f1 << "\tf2:\t" << f2 + << "\tf1-fn1:\t" << f1-fn1 + << "\tf2-fn2:\t" << f2-fn2 + << "\tdf/dx:\t" << J[iY][iX] + << std::fixed << std::endl; + } + } + } +} + +std::ostream &operator<<( std::ostream &out, const FGStateSpace::Component &c ) +{ + out << "\t" << c.getName() + << "\t" << c.getUnit() + << "\t:\t" << c.get(); + return out; +} + +std::ostream &operator<<( std::ostream &out, const FGStateSpace::ComponentVector &v ) +{ + for (int i=0; i< v.getSize(); i++) + { + out << *(v.getComp(i)) << "\n"; + } + out << ""; + return out; +} + +std::ostream &operator<<( std::ostream &out, const FGStateSpace &ss ) +{ + out << "\nX:\n" << ss.x + << "\nU:\n" << ss.u + << "\nY:\n" << ss.y; + return out; +} + +std::ostream &operator<<( std::ostream &out, const std::vector< std::vector > &vec2d ) +{ + int width = out.width(); + int nI = vec2d.size(); + out << std::left << std::setw(1) << "[" << std::right; + for (int i=0;i &vec ) +{ + int width = out.width(); + int nI = vec.size(); + out << std::left << std::setw(1) << "[" << std::right; + for (int i=0;i + * + * FGStateSpace.h 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. + * + * FGStateSpace.h 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, see . + */ + +#ifndef JSBSim_FGStateSpace_H +#define JSBSim_FGStateSpace_H + +#include "FGFDMExec.h" +#include "models/FGPropulsion.h" +#include "models/FGAccelerations.h" +#include "models/propulsion/FGEngine.h" +#include "models/propulsion/FGThruster.h" +#include "models/propulsion/FGTurbine.h" +#include "models/propulsion/FGTurboProp.h" +#include "models/FGAuxiliary.h" +#include "models/FGFCS.h" +#include +#include + +namespace JSBSim +{ + +class FGStateSpace +{ +public: + + // component class + class Component + { + protected: + FGStateSpace * m_stateSpace; + FGFDMExec * m_fdm; + std::string m_name, m_unit; + public: + Component(const std::string & name, const std::string & unit) : + m_stateSpace(), m_fdm(), m_name(name), m_unit(unit) {}; + virtual ~Component() {}; + virtual double get() const = 0; + virtual void set(double val) = 0; + virtual double getDeriv() const + { + // by default should calculate using finite difference approx + std::vector x0 = m_stateSpace->x.get(); + double f0 = get(); + double dt0 = m_fdm->GetDeltaT(); + double time0 = m_fdm->GetSimTime(); + m_fdm->Setdt(1./120.); + m_fdm->DisableOutput(); + m_fdm->Run(); + double f1 = get(); + m_stateSpace->x.set(x0); + if (m_fdm->GetDebugLevel() > 1) + { + std::cout << std::scientific + << "name: " << m_name + << "\nf1: " << f0 + << "\nf2: " << f1 + << "\ndt: " << m_fdm->GetDeltaT() + << "\tdf/dt: " << (f1-f0)/m_fdm->GetDeltaT() + << std::fixed << std::endl; + } + double deriv = (f1-f0)/m_fdm->GetDeltaT(); + m_fdm->Setdt(dt0); // restore original value + m_fdm->Setsim_time(time0); + m_fdm->EnableOutput(); + return deriv; + } + void setStateSpace(FGStateSpace * stateSpace) + { + m_stateSpace = stateSpace; + } + void setFdm(FGFDMExec * fdm) + { + m_fdm = fdm; + } + const std::string & getName() const + { + return m_name; + } + const std::string & getUnit() const + { + return m_unit; + } + }; + + // component vector class + class ComponentVector + { + public: + ComponentVector(FGFDMExec * fdm, FGStateSpace * stateSpace) : + m_stateSpace(stateSpace), m_fdm(fdm), m_components() {} + ComponentVector & operator=(ComponentVector & componentVector) + { + m_stateSpace = componentVector.m_stateSpace; + m_fdm = componentVector.m_fdm; + m_components = componentVector.m_components; + return *this; + } + ComponentVector(const ComponentVector & componentVector) : + m_stateSpace(componentVector.m_stateSpace), + m_fdm(componentVector.m_fdm), + m_components(componentVector.m_components) + { + } + void add(Component * comp) + { + comp->setStateSpace(m_stateSpace); + comp->setFdm(m_fdm); + m_components.push_back(comp); + } + int getSize() const + { + return m_components.size(); + } + Component * getComp(int i) const + { + return m_components[i]; + }; + Component * getComp(int i) + { + return m_components[i]; + }; + double get(int i) const + { + return m_components[i]->get(); + }; + void set(int i, double val) + { + m_components[i]->set(val); + m_fdm->RunIC(); + }; + double get(int i) + { + return m_components[i]->get(); + }; + std::vector get() const + { + std::vector val; + for (int i=0;iget()); + return val; + } + void get(double * array) const + { + for (int i=0;iget(); + } + double getDeriv(int i) + { + return m_components[i]->getDeriv(); + }; + std::vector getDeriv() const + { + std::vector val; + for (int i=0;igetDeriv()); + return val; + } + void getDeriv(double * array) const + { + for (int i=0;igetDeriv(); + } + void set(vector vals) + { + for (int i=0;iset(vals[i]); + m_fdm->RunIC(); + } + void set(double * array) + { + for (int i=0;iset(array[i]); + m_fdm->RunIC(); + } + std::string getName(int i) const + { + return m_components[i]->getName(); + }; + std::vector getName() const + { + std::vector name; + for (int i=0;igetName()); + return name; + } + std::string getUnit(int i) const + { + return m_components[i]->getUnit(); + }; + std::vector getUnit() const + { + std::vector unit; + for (int i=0;igetUnit()); + return unit; + } + void clear() { + m_components.clear(); + } + private: + FGStateSpace * m_stateSpace; + FGFDMExec * m_fdm; + std::vector m_components; + }; + + // component vectors + ComponentVector x, u, y; + + // constructor + FGStateSpace(FGFDMExec * fdm) : x(fdm,this), u(fdm,this), y(fdm,this), m_fdm(fdm) {}; + + void setFdm(FGFDMExec * fdm) { m_fdm = fdm; } + + void clear() { + x.clear(); + u.clear(); + y.clear(); + } + + // deconstructor + virtual ~FGStateSpace() {}; + + // linearization function + void linearize(std::vector x0, std::vector u0, std::vector y0, + std::vector< std::vector > & A, + std::vector< std::vector > & B, + std::vector< std::vector > & C, + std::vector< std::vector > & D); + + +private: + + // compute numerical jacobian of a matrix + void numericalJacobian(std::vector< std::vector > & J, ComponentVector & y, + ComponentVector & x, const std::vector & y0, + const std::vector & x0, double h=1e-5, bool computeYDerivative = false); + + // flight dynamcis model + FGFDMExec * m_fdm; + +public: + + // components + + class Vt : public Component + { + public: + Vt() : Component("Vt","ft/s") {}; + double get() const + { + return m_fdm->GetAuxiliary()->GetVt(); + } + void set(double val) + { + m_fdm->GetIC()->SetVtrueFpsIC(val); + } + double getDeriv() const + { + + return (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetAccelerations()->GetUVWdot(1) + + m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetAccelerations()->GetUVWdot(2) + + m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetAccelerations()->GetUVWdot(3))/ + m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot + } + + }; + + class VGround : public Component + { + public: + VGround() : Component("VGround","ft/s") {}; + double get() const + { + return m_fdm->GetAuxiliary()->GetVground(); + } + void set(double val) + { + m_fdm->GetIC()->SetVgroundFpsIC(val); + } + }; + + class AccelX : public Component + { + public: + AccelX() : Component("AccelX","ft/s^2") {}; + double get() const + { + return m_fdm->GetAuxiliary()->GetPilotAccel(1); + } + void set(double val) + { + // XXX: not possible to implement currently + } + }; + + class AccelY : public Component + { + public: + AccelY() : Component("AccelY","ft/s^2") {}; + double get() const + { + return m_fdm->GetAuxiliary()->GetPilotAccel(2); + } + void set(double val) + { + // XXX: not possible to implement currently + } + }; + + class AccelZ : public Component + { + public: + AccelZ() : Component("AccelZ","ft/s^2") {}; + double get() const + { + return m_fdm->GetAuxiliary()->GetPilotAccel(3); + } + void set(double val) + { + // XXX: not possible to implement currently + } + }; + + class Alpha : public Component + { + public: + Alpha() : Component("Alpha","rad") {}; + double get() const + { + return m_fdm->GetAuxiliary()->Getalpha(); + } + void set(double val) + { + m_fdm->GetIC()->SetFlightPathAngleRadIC(m_fdm->GetIC()->GetThetaRadIC()-val); + m_fdm->GetIC()->SetAlphaRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetAuxiliary()->Getadot(); + } + }; + + class Theta : public Component + { + public: + Theta() : Component("Theta","rad") {}; + double get() const + { + return m_fdm->GetPropagate()->GetEuler(2); + } + void set(double val) + { + m_fdm->GetIC()->SetFlightPathAngleRadIC(val-m_fdm->GetIC()->GetAlphaRadIC()); + m_fdm->GetIC()->SetThetaRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetAuxiliary()->GetEulerRates(2); + } + }; + + class Q : public Component + { + public: + Q() : Component("Q","rad/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetPQR(2); + } + void set(double val) + { + m_fdm->GetIC()->SetQRadpsIC(val); + } + double getDeriv() const + { + return m_fdm->GetAccelerations()->GetPQRdot(2); + } + }; + + class Alt : public Component + { + public: + Alt() : Component("Alt","ft") {}; + double get() const + { + return m_fdm->GetPropagate()->GetAltitudeASL(); + } + void set(double val) + { + m_fdm->GetIC()->SetAltitudeASLFtIC(val); + } + double getDeriv() const + { + return m_fdm->GetPropagate()->Gethdot(); + } + }; + + class Beta : public Component + { + public: + Beta() : Component("Beta","rad") {}; + double get() const + { + return m_fdm->GetAuxiliary()->Getbeta(); + } + void set(double val) + { + m_fdm->GetIC()->SetBetaRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetAuxiliary()->Getbdot(); + } + }; + + class Phi : public Component + { + public: + Phi() : Component("Phi","rad") {}; + double get() const + { + return m_fdm->GetPropagate()->GetEuler(1); + } + void set(double val) + { + m_fdm->GetIC()->SetPhiRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetAuxiliary()->GetEulerRates(1); + } + }; + + class P : public Component + { + public: + P() : Component("P","rad/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetPQR(1); + } + void set(double val) + { + m_fdm->GetIC()->SetPRadpsIC(val); + } + double getDeriv() const + { + return m_fdm->GetAccelerations()->GetPQRdot(1); + } + }; + + class R : public Component + { + public: + R() : Component("R","rad/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetPQR(3); + } + void set(double val) + { + m_fdm->GetIC()->SetRRadpsIC(val); + } + double getDeriv() const + { + return m_fdm->GetAccelerations()->GetPQRdot(3); + } + }; + + class Psi : public Component + { + public: + Psi() : Component("Psi","rad") {}; + double get() const + { + return m_fdm->GetPropagate()->GetEuler(3); + } + void set(double val) + { + m_fdm->GetIC()->SetPsiRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetAuxiliary()->GetEulerRates(3); + } + }; + + class ThrottleCmd : public Component + { + public: + ThrottleCmd() : Component("ThtlCmd","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetThrottleCmd(0); + } + void set(double val) + { + for (unsigned int i=0;iGetPropulsion()->GetNumEngines();i++) + m_fdm->GetFCS()->SetThrottleCmd(i,val); + m_fdm->GetFCS()->Run(true); + } + }; + + class ThrottlePos : public Component + { + public: + ThrottlePos() : Component("ThtlPos","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetThrottlePos(0); + } + void set(double val) + { + for (unsigned int i=0;iGetPropulsion()->GetNumEngines();i++) + m_fdm->GetFCS()->SetThrottlePos(i,val); + } + }; + + class DaCmd : public Component + { + public: + DaCmd() : Component("DaCmd","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetDaCmd(); + } + void set(double val) + { + m_fdm->GetFCS()->SetDaCmd(val); + m_fdm->GetFCS()->Run(true); + } + }; + + class DaPos : public Component + { + public: + DaPos() : Component("DaPos","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetDaLPos(); + } + void set(double val) + { + m_fdm->GetFCS()->SetDaLPos(ofRad,val); + m_fdm->GetFCS()->SetDaRPos(ofRad,val); // TODO: check if this is neg. + } + }; + + class DeCmd : public Component + { + public: + DeCmd() : Component("DeCmd","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetDeCmd(); + } + void set(double val) + { + m_fdm->GetFCS()->SetDeCmd(val); + m_fdm->GetFCS()->Run(true); + } + }; + + class DePos : public Component + { + public: + DePos() : Component("DePos","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetDePos(); + } + void set(double val) + { + m_fdm->GetFCS()->SetDePos(ofRad,val); + } + }; + + class DrCmd : public Component + { + public: + DrCmd() : Component("DrCmd","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetDrCmd(); + } + void set(double val) + { + m_fdm->GetFCS()->SetDrCmd(val); + m_fdm->GetFCS()->Run(true); + } + }; + + class DrPos : public Component + { + public: + DrPos() : Component("DrPos","norm") {}; + double get() const + { + return m_fdm->GetFCS()->GetDrPos(); + } + void set(double val) + { + m_fdm->GetFCS()->SetDrPos(ofRad,val); + } + }; + + class Rpm0 : public Component + { + public: + Rpm0() : Component("Rpm0","rev/min") {}; + double get() const + { + return m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetRPM(); + } + void set(double val) + { + m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->SetRPM(val); + } + }; + + class Rpm1 : public Component + { + public: + Rpm1() : Component("Rpm1","rev/min") {}; + double get() const + { + return m_fdm->GetPropulsion()->GetEngine(1)->GetThruster()->GetRPM(); + } + void set(double val) + { + m_fdm->GetPropulsion()->GetEngine(1)->GetThruster()->SetRPM(val); + } + }; + + class Rpm2 : public Component + { + public: + Rpm2() : Component("Rpmr2","rev/min") {}; + double get() const + { + return m_fdm->GetPropulsion()->GetEngine(2)->GetThruster()->GetRPM(); + } + void set(double val) + { + m_fdm->GetPropulsion()->GetEngine(2)->GetThruster()->SetRPM(val); + } + }; + + class Rpm3 : public Component + { + public: + Rpm3() : Component("Rpm3","rev/min") {}; + double get() const + { + return m_fdm->GetPropulsion()->GetEngine(3)->GetThruster()->GetRPM(); + } + void set(double val) + { + m_fdm->GetPropulsion()->GetEngine(3)->GetThruster()->SetRPM(val); + } + }; + + class PropPitch : public Component + { + public: + PropPitch() : Component("Prop Pitch","deg") {}; + double get() const + { + return m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetPitch(); + } + void set(double val) + { + for (unsigned int i=0;iGetPropulsion()->GetNumEngines();i++) + m_fdm->GetPropulsion()->GetEngine(i)->GetThruster()->SetPitch(val); + } + }; + + class Longitude : public Component + { + public: + Longitude() : Component("Longitude","rad") {}; + double get() const + { + return m_fdm->GetPropagate()->GetLongitude(); + } + void set(double val) + { + m_fdm->GetIC()->SetLongitudeRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetPropagate()->GetVel(2)/(cos(m_fdm->GetPropagate()->GetLatitude())*m_fdm->GetPropagate()->GetRadius()); + } + }; + + class Latitude : public Component + { + public: + Latitude() : Component("Latitude","rad") {}; + double get() const + { + return m_fdm->GetPropagate()->GetLatitude(); + } + void set(double val) + { + m_fdm->GetIC()->SetLatitudeRadIC(val); + } + double getDeriv() const + { + return m_fdm->GetPropagate()->GetVel(1)/(m_fdm->GetPropagate()->GetRadius()); + } + }; + + class Pi : public Component + { + public: + Pi() : Component("P inertial","rad/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetPQRi(1); + } + void set(double val) + { + //Set PQR from PQRi + //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; + m_fdm->GetIC()->SetQRadpsIC(val + \ + (m_fdm->GetPropagate()->GetPQR(1) - m_fdm->GetPropagate()->GetPQRi(1))); + } + double getDeriv() const + { + return m_fdm->GetAccelerations()->GetPQRdot(1); + } + }; + + class Qi : public Component + { + public: + Qi() : Component("Q inertial","rad/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetPQRi(2); + } + void set(double val) + { + //Set PQR from PQRi + //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; + m_fdm->GetIC()->SetQRadpsIC(val + \ + (m_fdm->GetPropagate()->GetPQR(2) - m_fdm->GetPropagate()->GetPQRi(2))); + } + double getDeriv() const + { + return m_fdm->GetAccelerations()->GetPQRdot(2); + } + }; + + class Ri : public Component + { + public: + Ri() : Component("R inertial","rad/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetPQRi(3); + } + void set(double val) + { + //Set PQR from PQRi + //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth; + m_fdm->GetIC()->SetQRadpsIC(val + \ + (m_fdm->GetPropagate()->GetPQR(3) - m_fdm->GetPropagate()->GetPQRi(3))); + } + double getDeriv() const + { + return m_fdm->GetAccelerations()->GetPQRdot(3); + } + }; + + class Vn : public Component + { + public: + Vn() : Component("Vel north","feet/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetVel(1); + } + void set(double val) + { + m_fdm->GetIC()->SetVNorthFpsIC(val); + } + double getDeriv() const + { + //get NED accel from body accel + return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(1); + } + }; + + class Ve : public Component + { + public: + Ve() : Component("Vel east","feet/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetVel(2); + } + void set(double val) + { + m_fdm->GetIC()->SetVEastFpsIC(val); + } + double getDeriv() const + { + //get NED accel from body accel + return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(2); + } + }; + + class Vd : public Component + { + public: + Vd() : Component("Vel down","feet/s") {}; + double get() const + { + return m_fdm->GetPropagate()->GetVel(3); + } + void set(double val) + { + m_fdm->GetIC()->SetVDownFpsIC(val); + } + double getDeriv() const + { + //get NED accel from body accel + return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(3); + } + }; + + class COG : public Component + { + public: + COG() : Component("Course Over Ground","rad") {}; + double get() const + { + //cog = atan2(Ve,Vn) + return atan2(m_fdm->GetPropagate()->GetVel(2),m_fdm->GetPropagate()->GetVel(1)); + } + void set(double val) + { + //set Vn and Ve according to vGround and COG + m_fdm->GetIC()->SetVNorthFpsIC(m_fdm->GetAuxiliary()->GetVground()*cos(val)); + m_fdm->GetIC()->SetVEastFpsIC(m_fdm->GetAuxiliary()->GetVground()*sin(val)); + } + double getDeriv() const + { + double Vn = m_fdm->GetPropagate()->GetVel(1); + double Vndot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(1); + double Ve = m_fdm->GetPropagate()->GetVel(2); + double Vedot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetAccelerations()->GetUVWdot())(2); + + //dCOG/dt = dCOG/dVe*dVe/dt + dCOG/dVn*dVn/dt + return Vn/(Vn*Vn+Ve*Ve)*Vedot - Ve/(Vn*Vn+Ve*Ve)*Vndot; + } + }; + +}; + +// stream output +std::ostream &operator<<(std::ostream &out, const FGStateSpace::Component &c ); +std::ostream &operator<<(std::ostream &out, const FGStateSpace::ComponentVector &v ); +std::ostream &operator<<(std::ostream &out, const FGStateSpace &ss ); +std::ostream &operator<<( std::ostream &out, const std::vector< std::vector > &vec2d ); +std::ostream &operator<<( std::ostream &out, const std::vector &vec ); + +} // JSBSim + +#endif + +// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/math/LagrangeMultiplier.h b/src/FDM/JSBSim/math/LagrangeMultiplier.h old mode 100644 new mode 100755 diff --git a/src/FDM/JSBSim/models/FGAccelerations.cpp b/src/FDM/JSBSim/models/FGAccelerations.cpp index 07a21f73c..605d1974f 100644 --- a/src/FDM/JSBSim/models/FGAccelerations.cpp +++ b/src/FDM/JSBSim/models/FGAccelerations.cpp @@ -60,7 +60,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAccelerations.cpp,v 1.13 2012/02/18 19:11:37 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGAccelerations.cpp,v 1.14 2012/09/15 17:00:56 bcoconni Exp $"; static const char *IdHdr = ID_ACCELERATIONS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -74,6 +74,7 @@ FGAccelerations::FGAccelerations(FGFDMExec* fdmex) Name = "FGAccelerations"; gravType = gtWGS84; gravTorque = false; + HoldDown = 0; vPQRidot.InitMatrix(); vUVWidot.InitMatrix(); @@ -154,9 +155,16 @@ void FGAccelerations::CalculatePQRdot(void) // Compute body frame rotational accelerations based on the current body // moments and the total inertial angular velocity expressed in the body // frame. - - vPQRidot = in.Jinv * (in.Moment - in.vPQRi * (in.J * in.vPQRi)); - vPQRdot = vPQRidot - in.vPQRi * (in.Ti2b * in.vOmegaPlanet); + if (HoldDown) { + // The rotational acceleration in ECI is calculated so that the rotational + // acceleration is zero in the body frame. + vPQRdot.InitMatrix(); + vPQRidot = in.vPQRi * (in.Ti2b * in.vOmegaPlanet); + } + else { + vPQRidot = in.Jinv * (in.Moment - in.vPQRi * (in.J * in.vPQRi)); + vPQRdot = vPQRidot - in.vPQRi * (in.Ti2b * in.vOmegaPlanet); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -191,7 +199,10 @@ void FGAccelerations::CalculateQuatdot(void) void FGAccelerations::CalculateUVWdot(void) { - vBodyAccel = in.Force / in.Mass; + if (HoldDown) + vBodyAccel.InitMatrix(); + else + vBodyAccel = in.Force / in.Mass; vUVWdot = vBodyAccel - (in.vPQR + 2.0 * (in.Ti2b * in.vOmegaPlanet)) * in.vUVW; @@ -211,8 +222,16 @@ void FGAccelerations::CalculateUVWdot(void) break; } - vUVWdot += in.Ti2b * vGravAccel; - vUVWidot = in.Tb2i * vBodyAccel + vGravAccel; + if (HoldDown) { + // The acceleration in ECI is calculated so that the acceleration is zero + // in the body frame. + vUVWidot = -1.0 * (in.Tb2i * vUVWdot); + vUVWdot.InitMatrix(); + } + else { + vUVWdot += in.Ti2b * vGravAccel; + vUVWidot = in.Tb2i * vBodyAccel + vGravAccel; + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -361,6 +380,8 @@ void FGAccelerations::bind(void) PropertyManager->Tie("forces/fbx-gear-lbs", this, eX, (PMF)&FGAccelerations::GetGroundForces); PropertyManager->Tie("forces/fby-gear-lbs", this, eY, (PMF)&FGAccelerations::GetGroundForces); PropertyManager->Tie("forces/fbz-gear-lbs", this, eZ, (PMF)&FGAccelerations::GetGroundForces); + + PropertyManager->Tie("forces/hold-down", this, &FGAccelerations::GetHoldDown, &FGAccelerations::SetHoldDown); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGAccelerations.h b/src/FDM/JSBSim/models/FGAccelerations.h index 42b0f4885..0a83d55a8 100644 --- a/src/FDM/JSBSim/models/FGAccelerations.h +++ b/src/FDM/JSBSim/models/FGAccelerations.h @@ -50,7 +50,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ACCELERATIONS "$Id: FGAccelerations.h,v 1.12 2012/02/19 14:07:27 bcoconni Exp $" +#define ID_ACCELERATIONS "$Id: FGAccelerations.h,v 1.14 2012/09/25 12:44:36 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -97,7 +97,7 @@ CLASS DOCUMENTATION NASA SP-8024, May 1969 @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier - @version $Id: FGAccelerations.h,v 1.12 2012/02/19 14:07:27 bcoconni Exp $ + @version $Id: FGAccelerations.h,v 1.14 2012/09/25 12:44:36 jberndt Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -233,6 +233,7 @@ public: */ const FGColumnVector3& GetBodyAccel(void) const { return vBodyAccel; } + const FGColumnVector3& GetGravAccel(void) const {return vGravAccel; } /** Retrieves a component of the acceleration resulting from the applied forces. Retrieves a component of the ratio between the sum of all forces applied @@ -321,6 +322,16 @@ public: */ void InitializeDerivatives(void); + /** Sets the property forces/hold-down. This allows to do hard 'hold-down' + such as for rockets on a launch pad with engines ignited. + @param hd enables the 'hold-down' function if non-zero + */ + void SetHoldDown(int hd) {HoldDown = hd;} + /** Gets the value of the property forces/hold-down. + @result zero if the 'hold-down' function is disabled, non-zero otherwise. + */ + int GetHoldDown(void) const {return HoldDown;} + struct Inputs { /// The body inertia matrix expressed in the body frame FGMatrix33 J; @@ -382,6 +393,7 @@ private: int gravType; bool gravTorque; + int HoldDown; void CalculatePQRdot(void); void CalculateQuatdot(void); diff --git a/src/FDM/JSBSim/models/FGAerodynamics.cpp b/src/FDM/JSBSim/models/FGAerodynamics.cpp index ed1d639fc..b4df456fd 100644 --- a/src/FDM/JSBSim/models/FGAerodynamics.cpp +++ b/src/FDM/JSBSim/models/FGAerodynamics.cpp @@ -48,7 +48,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.45 2012/04/13 13:25:52 jberndt Exp $"; +static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.46 2012/07/26 04:33:46 jberndt Exp $"; static const char *IdHdr = ID_AERODYNAMICS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -177,15 +177,20 @@ bool FGAerodynamics::Run(bool Holding) // used in the L/D calculation, and we still may want to look at Lift // and Drag. + // JSB 4/27/12 - After use, convert wind axes to produce normal lift + // and drag values - not negative ones! + switch (axisType) { case atBodyXYZ: // Forces already in body axes; no manipulation needed vFw = in.Tb2w*vFnative; + vFw(eDrag)*=-1; vFw(eLift)*=-1; vForces = vFnative; break; case atLiftDrag: // Copy forces into wind axes vFw = vFnative; vFw(eDrag)*=-1; vFw(eLift)*=-1; vForces = in.Tw2b*vFw; + vFw(eDrag)*=-1; vFw(eLift)*=-1; break; case atAxialNormal: // Convert native forces into Axial|Normal|Side system vFw = in.Tb2w*vFnative; @@ -218,13 +223,14 @@ bool FGAerodynamics::Run(bool Holding) vDXYZcg(eY) = in.RPBody(eY) + vDeltaRP(eY); vDXYZcg(eZ) = in.RPBody(eZ) - vDeltaRP(eZ); - vMoments = vDXYZcg*vForces; // M = r X F + vMomentsMRC.InitMatrix(); for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) { for (ctr = 0; ctr < AeroFunctions[axis_ctr+3].size(); ctr++) { - vMoments(axis_ctr+1) += AeroFunctions[axis_ctr+3][ctr]->GetValue(); + vMomentsMRC(axis_ctr+1) += AeroFunctions[axis_ctr+3][ctr]->GetValue(); } } + vMoments = vMomentsMRC + vDXYZcg*vForces; // M = r X F RunPostFunctions(); diff --git a/src/FDM/JSBSim/models/FGAerodynamics.h b/src/FDM/JSBSim/models/FGAerodynamics.h index f4194e97d..1ab848ba1 100644 --- a/src/FDM/JSBSim/models/FGAerodynamics.h +++ b/src/FDM/JSBSim/models/FGAerodynamics.h @@ -52,7 +52,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_AERODYNAMICS "$Id: FGAerodynamics.h,v 1.25 2011/08/04 12:46:32 jberndt Exp $" +#define ID_AERODYNAMICS "$Id: FGAerodynamics.h,v 1.26 2012/07/26 04:33:46 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -109,7 +109,7 @@ CLASS DOCUMENTATION Systems may NOT be combined, or a load error will occur. @author Jon S. Berndt, Tony Peden - @version $Revision: 1.25 $ + @version $Revision: 1.26 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -154,15 +154,24 @@ public: @return the force acting on an axis */ double GetForces(int n) const {return vForces(n);} - /** Gets the total aerodynamic moment vector. + /** Gets the total aerodynamic moment vector about the CG. @return a moment vector reference. */ const FGColumnVector3& GetMoments(void) const {return vMoments;} - /** Gets the aerodynamic moment for an axis. + /** Gets the aerodynamic moment about the CG for an axis. @return the moment about a single axis (as described also in the similar call to GetForces(int n).*/ double GetMoments(int n) const {return vMoments(n);} + /** Gets the total aerodynamic moment vector about the Moment Reference Center. + @return a moment vector reference. */ + const FGColumnVector3& GetMomentsMRC(void) const {return vMomentsMRC;} + + /** Gets the aerodynamic moment about the Moment Reference Center for an axis. + @return the moment about a single axis (as described also in the + similar call to GetForces(int n).*/ + double GetMomentsMRC(int n) const {return vMomentsMRC(n);} + /** Retrieves the aerodynamic forces in the wind axes. @return a reference to a column vector containing the wind axis forces. */ const FGColumnVector3& GetvFw(void) const { return vFw; } @@ -229,6 +238,7 @@ private: FGColumnVector3 vFw; FGColumnVector3 vForces; FGColumnVector3 vMoments; + FGColumnVector3 vMomentsMRC; FGColumnVector3 vDXYZcg; FGColumnVector3 vDeltaRP; double alphaclmax, alphaclmin; diff --git a/src/FDM/JSBSim/models/FGAircraft.cpp b/src/FDM/JSBSim/models/FGAircraft.cpp index a433a56fa..34205ee88 100644 --- a/src/FDM/JSBSim/models/FGAircraft.cpp +++ b/src/FDM/JSBSim/models/FGAircraft.cpp @@ -59,7 +59,7 @@ DEFINITIONS GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.33 2011/08/21 15:06:38 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGAircraft.cpp,v 1.34 2012/09/15 17:00:56 bcoconni Exp $"; static const char *IdHdr = ID_AIRCRAFT; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -77,7 +77,6 @@ FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex) lbarh = lbarv = 0.0; vbarh = vbarv = 0.0; WingIncidence = 0.0; - HoldDown = 0; bind(); @@ -107,25 +106,17 @@ bool FGAircraft::Run(bool Holding) RunPreFunctions(); - vForces.InitMatrix(); - if (!HoldDown) { - vForces += in.AeroForce; - vForces += in.PropForce; - vForces += in.GroundForce; - vForces += in.ExternalForce; - vForces += in.BuoyantForce; - } else { - vForces = in.Tl2b * FGColumnVector3(0,0,-in.Weight); - } + vForces = in.AeroForce; + vForces += in.PropForce; + vForces += in.GroundForce; + vForces += in.ExternalForce; + vForces += in.BuoyantForce; - vMoments.InitMatrix(); - if (!HoldDown) { - vMoments += in.AeroMoment; - vMoments += in.PropMoment; - vMoments += in.GroundMoment; - vMoments += in.ExternalMoment; - vMoments += in.BuoyantMoment; - } + vMoments = in.AeroMoment; + vMoments += in.PropMoment; + vMoments += in.GroundMoment; + vMoments += in.ExternalMoment; + vMoments += in.BuoyantMoment; RunPostFunctions(); @@ -216,7 +207,6 @@ void FGAircraft::bind(void) PropertyManager->Tie("metrics/visualrefpoint-x-in", this, eX, (PMF)&FGAircraft::GetXYZvrp); PropertyManager->Tie("metrics/visualrefpoint-y-in", this, eY, (PMF)&FGAircraft::GetXYZvrp); PropertyManager->Tie("metrics/visualrefpoint-z-in", this, eZ, (PMF)&FGAircraft::GetXYZvrp); - PropertyManager->Tie("forces/hold-down", this, &FGAircraft::GetHoldDown, &FGAircraft::SetHoldDown); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGAircraft.h b/src/FDM/JSBSim/models/FGAircraft.h index 13a499bd5..c36f5accd 100644 --- a/src/FDM/JSBSim/models/FGAircraft.h +++ b/src/FDM/JSBSim/models/FGAircraft.h @@ -50,7 +50,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_AIRCRAFT "$Id: FGAircraft.h,v 1.19 2011/11/10 12:06:14 jberndt Exp $" +#define ID_AIRCRAFT "$Id: FGAircraft.h,v 1.20 2012/09/15 17:00:56 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -91,7 +91,7 @@ CLASS DOCUMENTATION @endcode @author Jon S. Berndt - @version $Id: FGAircraft.h,v 1.19 2011/11/10 12:06:14 jberndt Exp $ + @version $Id: FGAircraft.h,v 1.20 2012/09/15 17:00:56 bcoconni Exp $ @see Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate School, January 1994 @@ -167,8 +167,6 @@ public: double GetXYZvrp(int idx) const { return vXYZvrp(idx); } double GetXYZep(int idx) const { return vXYZep(idx); } void SetAircraftName(const std::string& name) {AircraftName = name;} - void SetHoldDown(int hd) {HoldDown = hd;} - int GetHoldDown(void) const {return HoldDown;} void SetXYZrp(int idx, double value) {vXYZrp(idx) = value;} @@ -188,8 +186,6 @@ public: FGColumnVector3 GroundMoment; FGColumnVector3 ExternalMoment; FGColumnVector3 BuoyantMoment; - FGMatrix33 Tl2b; - double Weight; } in; private: @@ -203,7 +199,6 @@ private: double WingArea, WingSpan, cbar, WingIncidence; double HTailArea, VTailArea, HTailArm, VTailArm; double lbarh,lbarv,vbarh,vbarv; - int HoldDown; std::string AircraftName; void Debug(int from); diff --git a/src/FDM/JSBSim/models/FGAtmosphere.cpp b/src/FDM/JSBSim/models/FGAtmosphere.cpp index 7113fc3ca..4d52bd789 100644 --- a/src/FDM/JSBSim/models/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/models/FGAtmosphere.cpp @@ -50,7 +50,7 @@ INCLUDES namespace JSBSim { -static const char *IdSrc = "$Id: FGAtmosphere.cpp,v 1.51 2012/04/13 13:18:28 jberndt Exp $"; +static const char *IdSrc = "$Id: FGAtmosphere.cpp,v 1.54 2012/09/17 12:38:07 jberndt Exp $"; static const char *IdHdr = ID_ATMOSPHERE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -111,9 +111,21 @@ bool FGAtmosphere::Run(bool Holding) void FGAtmosphere::Calculate(double altitude) { + if (!PropertyManager->HasNode("atmosphere/override/temperature")) Temperature = GetTemperature(altitude); + else + Temperature = PropertyManager->getDoubleValue("atmosphere/override/temperature"); + + if (!PropertyManager->HasNode("atmosphere/override/pressure")) Pressure = GetPressure(altitude); + else + Pressure = PropertyManager->getDoubleValue("atmosphere/override/pressure"); + + if (!PropertyManager->HasNode("atmosphere/override/density")) Density = Pressure/(Reng*Temperature); + else + Density = PropertyManager->getDoubleValue("atmosphere/override/density"); + Soundspeed = sqrt(SHRatio*Reng*(Temperature)); PressureAltitude = altitude; DensityAltitude = altitude; @@ -201,6 +213,30 @@ double FGAtmosphere::ConvertToPSF(double p, ePressure unit) const return targetPressure; } +double FGAtmosphere::ConvertFromPSF(double p, ePressure unit) const +{ + double targetPressure=0; // Pressure in PSF + + switch(unit) { + case ePSF: + targetPressure = p; + break; + case eMillibars: + targetPressure = p/2.08854342; + break; + case ePascals: + targetPressure = p/0.0208854342; + break; + case eInchesHg: + targetPressure = p/70.7180803; + break; + default: + throw("Undefined pressure unit given"); + } + + return targetPressure; +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGAtmosphere::bind(void) diff --git a/src/FDM/JSBSim/models/FGAtmosphere.h b/src/FDM/JSBSim/models/FGAtmosphere.h index 57c6f87f8..b1789a334 100644 --- a/src/FDM/JSBSim/models/FGAtmosphere.h +++ b/src/FDM/JSBSim/models/FGAtmosphere.h @@ -45,7 +45,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ATMOSPHERE "$Id: FGAtmosphere.h,v 1.30 2012/04/13 13:18:28 jberndt Exp $" +#define ID_ATMOSPHERE "$Id: FGAtmosphere.h,v 1.31 2012/08/20 12:28:50 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -74,7 +74,7 @@ CLASS DOCUMENTATION @property atmosphere/a-ratio @author Jon Berndt - @version $Id: FGAtmosphere.h,v 1.30 2012/04/13 13:18:28 jberndt Exp $ + @version $Id: FGAtmosphere.h,v 1.31 2012/08/20 12:28:50 jberndt Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -154,8 +154,8 @@ public: /// Returns the pressure at a specified altitude in psf. virtual double GetPressure(double altitude) const = 0; - /// Returns the sea level pressure in psf. - virtual double GetPressureSL(void) const { return SLpressure; } + // Returns the sea level pressure in target units, default in psf. + virtual double GetPressureSL(ePressure to=ePSF) const { return ConvertFromPSF(SLpressure, to); } /// Returns the ratio of at-altitude pressure over the sea level value. virtual double GetPressureRatio(void) const { return Pressure*rSLpressure; } @@ -235,6 +235,9 @@ protected: // Converts to PSF (pounds per square foot) from one of several unit systems. virtual double ConvertToPSF(double t, ePressure unit=ePSF) const; + // Converts from PSF (pounds per square foot) to one of several unit systems. + virtual double ConvertFromPSF(double t, ePressure unit=ePSF) const; + virtual void bind(void); void Debug(int from); }; diff --git a/src/FDM/JSBSim/models/FGAuxiliary.cpp b/src/FDM/JSBSim/models/FGAuxiliary.cpp old mode 100644 new mode 100755 index d111a46bc..5eed15f61 --- a/src/FDM/JSBSim/models/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/models/FGAuxiliary.cpp @@ -50,7 +50,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.56 2011/12/11 17:03:05 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.60 2012/09/30 16:49:17 bcoconni Exp $"; static const char *IdHdr = ID_AUXILIARY; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -159,6 +159,7 @@ bool FGAuxiliary::Run(bool Holding) double AeroV2 = vAeroUVW(eV)*vAeroUVW(eV); double AeroW2 = vAeroUVW(eW)*vAeroUVW(eW); double mUW = AeroU2 + AeroW2; + double Vtdot = (vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eV)*in.vUVWdot(eV) + vAeroUVW(eW)*in.vUVWdot(eW))/Vt; double Vt2 = Vt*Vt; @@ -168,13 +169,14 @@ bool FGAuxiliary::Run(bool Holding) if (vAeroUVW(eV) != 0.0) beta = mUW > 0.0 ? atan2(vAeroUVW(eV), sqrt(mUW)) : 0.0; - double signU=1; - if (vAeroUVW(eU) < 0.0) signU=-1; + //double signU=1; + //if (vAeroUVW(eU) < 0.0) signU=-1; if ( mUW >= 1.0 ) { adot = (vAeroUVW(eU)*in.vUVWdot(eW) - vAeroUVW(eW)*in.vUVWdot(eU))/mUW; - bdot = (signU*mUW*in.vUVWdot(eV) - - vAeroUVW(eV)*(vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eW)*in.vUVWdot(eW)))/(Vt2*sqrt(mUW)); + // bdot = (signU*mUW*in.vUVWdot(eV) + // - vAeroUVW(eV)*(vAeroUVW(eU)*in.vUVWdot(eU) + vAeroUVW(eW)*in.vUVWdot(eW)))/(Vt2*sqrt(mUW)); + bdot = (in.vUVWdot(eV)*Vt - vAeroUVW(eV)*Vtdot)/(Vt*sqrt(mUW)); } } @@ -224,21 +226,10 @@ bool FGAuxiliary::Run(bool Holding) vPilotAccel.InitMatrix(); vNcg = in.vBodyAccel/in.SLGravity; - if ( Vt > 1.0 ) { - // Nz is Acceleration in "g's", along normal axis (-Z body axis) - Nz = -vNcg(eZ); - vPilotAccel = in.vBodyAccel + in.vPQRdot * in.ToEyePt; - vPilotAccel += in.vPQR * (in.vPQR * in.ToEyePt); - } else { - // The line below handles low velocity (and on-ground) cases, basically - // representing the opposite of the force that the landing gear would - // exert on the ground (which is just the total weight). This eliminates - // any jitter that could be introduced by the landing gear. Theoretically, - // this branch could be eliminated, with a penalty of having a short - // transient at startup (lasting only a fraction of a second). - vPilotAccel = in.Tl2b * FGColumnVector3( 0.0, 0.0, -in.SLGravity ); - Nz = -vPilotAccel(eZ) / in.SLGravity; - } + // Nz is Acceleration in "g's", along normal axis (-Z body axis) + Nz = -vNcg(eZ); + vPilotAccel = in.vBodyAccel + in.vPQRdot * in.ToEyePt; + vPilotAccel += in.vPQR * (in.vPQR * in.ToEyePt); vNwcg = mTb2w * vNcg; vNwcg(eZ) = 1.0 - vNwcg(eZ); @@ -342,8 +333,8 @@ double FGAuxiliary::GetNlf(void) const void FGAuxiliary::CalculateRelativePosition(void) //ToDo: This belongs elsewhere - perhaps in FGPropagate or Exec { const double earth_radius_mt = in.ReferenceRadius*fttom; - lat_relative_position=(in.Latitude - FDMExec->GetIC()->GetLatitudeDegIC() *degtorad)*earth_radius_mt; - lon_relative_position=(in.Longitude - FDMExec->GetIC()->GetLongitudeDegIC()*degtorad)*earth_radius_mt; + lat_relative_position=(in.vLocation.GetLatitude() - FDMExec->GetIC()->GetLatitudeDegIC() *degtorad)*earth_radius_mt; + lon_relative_position=(in.vLocation.GetLongitude() - FDMExec->GetIC()->GetLongitudeDegIC()*degtorad)*earth_radius_mt; relative_position = sqrt(lat_relative_position*lat_relative_position + lon_relative_position*lon_relative_position); }; diff --git a/src/FDM/JSBSim/models/FGAuxiliary.h b/src/FDM/JSBSim/models/FGAuxiliary.h index 01fc0b91f..4f2250e6d 100644 --- a/src/FDM/JSBSim/models/FGAuxiliary.h +++ b/src/FDM/JSBSim/models/FGAuxiliary.h @@ -48,7 +48,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_AUXILIARY "$Id: FGAuxiliary.h,v 1.25 2011/11/12 18:59:11 bcoconni Exp $" +#define ID_AUXILIARY "$Id: FGAuxiliary.h,v 1.27 2012/09/15 11:17:21 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -99,7 +99,7 @@ CLASS DOCUMENTATION to the JSBSim vPQRdot vector, and the w parameter is equivalent to vPQR. @author Tony Peden, Jon Berndt - @version $Id: FGAuxiliary.h,v 1.25 2011/11/12 18:59:11 bcoconni Exp $ + @version $Id: FGAuxiliary.h,v 1.27 2012/09/15 11:17:21 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -265,7 +265,6 @@ public: double Mass; FGMatrix33 Tl2b; FGMatrix33 Tb2l; - FGMatrix33 Tb2w; FGColumnVector3 vPQR; FGColumnVector3 vPQRdot; FGColumnVector3 vUVW; @@ -277,10 +276,6 @@ public: FGColumnVector3 VRPBody; FGColumnVector3 vFw; FGLocation vLocation; - double Latitude; - double Longitude; - double InitialLatitude; - double InitialLongitude; double ReferenceRadius; double CosTht; double SinTht; diff --git a/src/FDM/JSBSim/models/FGExternalForce.cpp b/src/FDM/JSBSim/models/FGExternalForce.cpp old mode 100644 new mode 100755 index f0ef173b5..249325765 --- a/src/FDM/JSBSim/models/FGExternalForce.cpp +++ b/src/FDM/JSBSim/models/FGExternalForce.cpp @@ -60,12 +60,13 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGExternalForce.cpp,v 1.11 2011/10/31 14:54:41 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGExternalForce.cpp,v 1.12 2012/12/23 14:56:58 bcoconni Exp $"; static const char *IdHdr = ID_EXTERNALFORCE; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGExternalForce::FGExternalForce(FGFDMExec *FDMExec, Element *el, int index): FGForce(FDMExec) +FGExternalForce::FGExternalForce(FGFDMExec *FDMExec, Element *el, int index) + : FGForce(FDMExec) { Element* location_element=0; Element* direction_element=0; @@ -77,7 +78,7 @@ FGExternalForce::FGExternalForce(FGFDMExec *FDMExec, Element *el, int index): FG magnitude = 0.0; azimuth = 0.0; - PropertyManager = fdmex->GetPropertyManager(); + FGPropertyManager* PropertyManager = fdmex->GetPropertyManager(); Name = el->GetAttributeValue("name"); BasePropertyName = "external_reactions/" + Name; @@ -91,7 +92,6 @@ FGExternalForce::FGExternalForce(FGFDMExec *FDMExec, Element *el, int index): FG Magnitude_Function = new FGFunction(PropertyManager, function_element); } else { PropertyManager->Tie( BasePropertyName + "/magnitude",(FGExternalForce*)this, &FGExternalForce::GetMagnitude, &FGExternalForce::SetMagnitude); - Magnitude_Node = PropertyManager->GetNode(BasePropertyName + "/magnitude"); } @@ -145,15 +145,12 @@ FGExternalForce::FGExternalForce(const FGExternalForce& extForce) : FGForce(extF Frame = extForce.Frame; vDirection = extForce.vDirection; Name = extForce.Name; - BasePropertyName = extForce.BasePropertyName; - PropertyManager = extForce.PropertyManager; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FGExternalForce::~FGExternalForce() { - unbind( PropertyManager->GetNode("external_reactions")); delete Magnitude_Function; Debug(1); } @@ -178,20 +175,6 @@ const FGColumnVector3& FGExternalForce::GetBodyForces(void) return FGForce::GetBodyForces(); } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGExternalForce::unbind(FGPropertyManager *node) -{ - int N = node->nChildren(); - for (int i=0; igetChild(i)->nChildren() ) { - unbind( (FGPropertyManager*)node->getChild(i) ); - } else if ( node->getChild(i)->isTied() ) { - node->getChild(i)->untie(); - } - } -} - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // The bitmasked value choices are as follows: // unset: In this case (the default) JSBSim would only print diff --git a/src/FDM/JSBSim/models/FGExternalForce.h b/src/FDM/JSBSim/models/FGExternalForce.h old mode 100644 new mode 100755 index 5c8ec266b..1b26e894a --- a/src/FDM/JSBSim/models/FGExternalForce.h +++ b/src/FDM/JSBSim/models/FGExternalForce.h @@ -51,7 +51,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_EXTERNALFORCE "$Id: FGExternalForce.h,v 1.10 2011/10/31 14:54:41 bcoconni Exp $" +#define ID_EXTERNALFORCE "$Id: FGExternalForce.h,v 1.11 2012/12/23 14:56:58 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -168,14 +168,10 @@ private: string Frame; string Name; - FGPropertyManager* PropertyManager; - FGPropertyManager* Magnitude_Node; FGFunction* Magnitude_Function; - string BasePropertyName; FGColumnVector3 vDirection; double magnitude; double azimuth; - void unbind(FGPropertyManager *node); void Debug(int from); }; } diff --git a/src/FDM/JSBSim/models/FGFCS.cpp b/src/FDM/JSBSim/models/FGFCS.cpp index 3f659831f..872a7326d 100644 --- a/src/FDM/JSBSim/models/FGFCS.cpp +++ b/src/FDM/JSBSim/models/FGFCS.cpp @@ -60,11 +60,13 @@ INCLUDES #include "models/flight_control/FGMagnetometer.h" #include "models/flight_control/FGGyro.h" +#include "FGFCSChannel.h" + using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGFCS.cpp,v 1.77 2011/09/25 14:05:40 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGFCS.cpp,v 1.79 2012/12/12 06:19:57 jberndt Exp $"; static const char *IdHdr = ID_FCS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -107,13 +109,8 @@ FGFCS::~FGFCS() unsigned int i; - for (i=0;iGetType() == "LAG" || - Systems[i]->GetType() == "LEAD_LAG" || - Systems[i]->GetType() == "WASHOUT" || - Systems[i]->GetType() == "SECOND_ORDER_FILTER" || - Systems[i]->GetType() == "INTEGRATOR") - { - ((FGFilter*)Systems[i])->ResetPastStates(); - } else if (Systems[i]->GetType() == "PID" ) { - ((FGPID*)Systems[i])->ResetPastStates(); - } - } - - for (unsigned int i=0; iGetType() == "LAG" || - FCSComponents[i]->GetType() == "LEAD_LAG" || - FCSComponents[i]->GetType() == "WASHOUT" || - FCSComponents[i]->GetType() == "SECOND_ORDER_FILTER" || - FCSComponents[i]->GetType() == "INTEGRATOR") - { - ((FGFilter*)FCSComponents[i])->ResetPastStates(); - } else if (FCSComponents[i]->GetType() == "PID" ) { - ((FGPID*)FCSComponents[i])->ResetPastStates(); - } - } - - for (unsigned int i=0; iGetType() == "LAG" || - APComponents[i]->GetType() == "LEAD_LAG" || - APComponents[i]->GetType() == "WASHOUT" || - APComponents[i]->GetType() == "SECOND_ORDER_FILTER" || - APComponents[i]->GetType() == "INTEGRATOR") - { - ((FGFilter*)APComponents[i])->ResetPastStates(); - } else if (APComponents[i]->GetType() == "PID" ) { - ((FGPID*)APComponents[i])->ResetPastStates(); - } - } + // Reset the channels components. + for (unsigned int i=0; iReset(); return true; } @@ -210,14 +171,8 @@ bool FGFCS::Run(bool Holding) SteerPosDeg[i] = gear->GetDefaultSteerAngle( GetDsCmd() ); } - // Execute Systems in order - for (i=0; iRun(); - - // Execute Autopilot - for (i=0; iRun(); - - // Execute Flight Control System - for (i=0; iRun(); + // Execute system channels in order + for (i=0; iExecute(); RunPostFunctions(); @@ -526,12 +481,9 @@ void FGFCS::SetPropFeather(int engineNum, bool setting) bool FGFCS::Load(Element* el, SystemType systype) { string name, file, fname="", interface_property_string, parent_name; - vector *Components; Element *component_element; Element *channel_element; - - Components=0; - + // ToDo: The handling of name and file attributes could be improved, here, // considering that a name can be in the external file, as well. @@ -560,13 +512,10 @@ bool FGFCS::Load(Element* el, SystemType systype) } if (document->GetName() == "autopilot") { - Components = &APComponents; Name = "Autopilot: " + document->GetAttributeValue("name"); } else if (document->GetName() == "flight_control") { - Components = &FCSComponents; Name = "FCS: " + document->GetAttributeValue("name"); } else if (document->GetName() == "system") { - Components = &Systems; Name = "System: " + document->GetAttributeValue("name"); } Debug(2); @@ -609,8 +558,30 @@ bool FGFCS::Load(Element* el, SystemType systype) } channel_element = document->FindElement("channel"); + while (channel_element) { + FGFCSChannel* newChannel = 0; + + string sOnOffProperty = channel_element->GetAttributeValue("execute"); + FGPropertyManager* OnOffPropertyNode = 0; + if (sOnOffProperty.length() > 0) { + OnOffPropertyNode = PropertyManager->GetNode(sOnOffProperty); + if (OnOffPropertyNode == 0) { + cerr << highint << fgred + << "The On/Off property, " << sOnOffProperty << " specified for channel " + << channel_element->GetAttributeValue("name") << " is undefined or not " + << "understood. The simulation will abort" << reset << endl; + throw("Bad system definition"); + } else { + newChannel = new FGFCSChannel(OnOffPropertyNode); + } + } else { + newChannel = new FGFCSChannel(); + } + + SystemChannels.push_back(newChannel); + if (debug_lvl > 0) cout << endl << highint << fgblue << " Channel " << normint << channel_element->GetAttributeValue("name") << reset << endl; @@ -624,34 +595,34 @@ bool FGFCS::Load(Element* el, SystemType systype) (component_element->GetName() == string("second_order_filter")) || (component_element->GetName() == string("integrator")) ) { - Components->push_back(new FGFilter(this, component_element)); + newChannel->Add(new FGFilter(this, component_element)); } else if ((component_element->GetName() == string("pure_gain")) || (component_element->GetName() == string("scheduled_gain")) || (component_element->GetName() == string("aerosurface_scale"))) { - Components->push_back(new FGGain(this, component_element)); + newChannel->Add(new FGGain(this, component_element)); } else if (component_element->GetName() == string("summer")) { - Components->push_back(new FGSummer(this, component_element)); + newChannel->Add(new FGSummer(this, component_element)); } else if (component_element->GetName() == string("deadband")) { - Components->push_back(new FGDeadBand(this, component_element)); + newChannel->Add(new FGDeadBand(this, component_element)); } else if (component_element->GetName() == string("switch")) { - Components->push_back(new FGSwitch(this, component_element)); + newChannel->Add(new FGSwitch(this, component_element)); } else if (component_element->GetName() == string("kinematic")) { - Components->push_back(new FGKinemat(this, component_element)); + newChannel->Add(new FGKinemat(this, component_element)); } else if (component_element->GetName() == string("fcs_function")) { - Components->push_back(new FGFCSFunction(this, component_element)); + newChannel->Add(new FGFCSFunction(this, component_element)); } else if (component_element->GetName() == string("pid")) { - Components->push_back(new FGPID(this, component_element)); + newChannel->Add(new FGPID(this, component_element)); } else if (component_element->GetName() == string("actuator")) { - Components->push_back(new FGActuator(this, component_element)); + newChannel->Add(new FGActuator(this, component_element)); } else if (component_element->GetName() == string("sensor")) { - Components->push_back(new FGSensor(this, component_element)); + newChannel->Add(new FGSensor(this, component_element)); } else if (component_element->GetName() == string("accelerometer")) { - Components->push_back(new FGAccelerometer(this, component_element)); + newChannel->Add(new FGAccelerometer(this, component_element)); } else if (component_element->GetName() == string("magnetometer")) { - Components->push_back(new FGMagnetometer(this, component_element)); + newChannel->Add(new FGMagnetometer(this, component_element)); } else if (component_element->GetName() == string("gyro")) { - Components->push_back(new FGGyro(this, component_element)); + newChannel->Add(new FGGyro(this, component_element)); } else { cerr << "Unknown FCS component: " << component_element->GetName() << endl; } @@ -686,19 +657,21 @@ string FGFCS::FindSystemFullPathname(const string& sysfilename) string fullpath, localpath; string system_filename = sysfilename; string systemPath = FDMExec->GetSystemsPath(); - string aircraftPath = FDMExec->GetFullAircraftPath(); + string aircraftPath = FDMExec->GetFullAircraftPath() + "/"; ifstream system_file; fullpath = systemPath + "/"; - localpath = aircraftPath + "/Systems/"; + localpath = aircraftPath + "Systems/"; if (system_filename.length() <=4 || system_filename.substr(system_filename.length()-4, 4) != ".xml") { system_filename.append(".xml"); } - system_file.open(string(localpath + system_filename).c_str()); + system_file.open(string(aircraftPath + system_filename).c_str()); if ( !system_file.is_open()) { - system_file.open(string(fullpath + system_filename).c_str()); + system_file.open(string(localpath + system_filename).c_str()); + if ( !system_file.is_open()) { + system_file.open(string(fullpath + system_filename).c_str()); if ( !system_file.is_open()) { cerr << " Could not open system file: " << system_filename << " in path " << fullpath << " or " << localpath << endl; @@ -706,8 +679,13 @@ string FGFCS::FindSystemFullPathname(const string& sysfilename) } else { return string(fullpath + system_filename); } + } else { + return string(localpath + system_filename); + } + } else { + return string(aircraftPath + system_filename); } - return string(localpath + system_filename); + return string(""); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -742,34 +720,20 @@ ifstream* FGFCS::FindSystemFile(const string& sysfilename) string FGFCS::GetComponentStrings(const string& delimiter) const { - unsigned int comp; string CompStrings = ""; bool firstime = true; int total_count=0; - for (unsigned int i=0; iGetName(); - total_count++; - } - - for (comp = 0; comp < APComponents.size(); comp++) + for (unsigned int i=0; iGetName(); - total_count++; - } - - for (comp = 0; comp < FCSComponents.size(); comp++) { - if (firstime) firstime = false; - else CompStrings += delimiter; + for (unsigned int c=0; cGetNumComponents(); c++) + { + if (firstime) firstime = false; + else CompStrings += delimiter; - CompStrings += FCSComponents[comp]->GetName(); - total_count++; + CompStrings += SystemChannels[i]->GetComponent(c)->GetName(); + total_count++; + } } return CompStrings; @@ -781,32 +745,19 @@ string FGFCS::GetComponentValues(const string& delimiter) const { std::ostringstream buf; - unsigned int comp; bool firstime = true; int total_count=0; - for (unsigned int i=0; iGetOutput(); - total_count++; - } - - for (comp = 0; comp < APComponents.size(); comp++) { - if (firstime) firstime = false; - else buf << delimiter; - - buf << setprecision(9) << APComponents[comp]->GetOutput(); - total_count++; - } - - for (comp = 0; comp < FCSComponents.size(); comp++) { - if (firstime) firstime = false; - else buf << delimiter; + for (unsigned int i=0; iGetNumComponents(); c++) + { + if (firstime) firstime = false; + else buf << delimiter; - buf << setprecision(9) << FCSComponents[comp]->GetOutput(); - total_count++; + buf << setprecision(9) << SystemChannels[i]->GetComponent(c)->GetOutput(); + total_count++; + } } return buf.str(); diff --git a/src/FDM/JSBSim/models/FGFCS.h b/src/FDM/JSBSim/models/FGFCS.h index 5c485c2bd..5297f9344 100644 --- a/src/FDM/JSBSim/models/FGFCS.h +++ b/src/FDM/JSBSim/models/FGFCS.h @@ -51,7 +51,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FCS "$Id: FGFCS.h,v 1.40 2011/10/31 14:54:41 bcoconni Exp $" +#define ID_FCS "$Id: FGFCS.h,v 1.41 2012/10/15 05:02:29 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -59,6 +59,7 @@ FORWARD DECLARATIONS namespace JSBSim { +class FGFCSChannel; typedef enum { ofRad=0, ofDeg, ofNorm, ofMag , NForms} OutputForm; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -168,7 +169,7 @@ CLASS DOCUMENTATION @property gear/tailhook-pos-norm @author Jon S. Berndt - @version $Revision: 1.40 $ + @version $Revision: 1.41 $ @see FGActuator @see FGDeadBand @see FGFCSFunction @@ -586,10 +587,8 @@ private: double GearCmd,GearPos; double TailhookPos, WingFoldPos; - typedef std::vector FCSCompVec; - FCSCompVec Systems; - FCSCompVec FCSComponents; - FCSCompVec APComponents; + typedef std::vector Channels; + Channels SystemChannels; void bind(void); void bindModel(void); void bindThrottle(unsigned int); diff --git a/src/FDM/JSBSim/models/FGFCSChannel.h b/src/FDM/JSBSim/models/FGFCSChannel.h new file mode 100755 index 000000000..16f13b784 --- /dev/null +++ b/src/FDM/JSBSim/models/FGFCSChannel.h @@ -0,0 +1,128 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGGFCSChannel.h + Author: Jon S. Berndt + Date started: 10/11/12 + + ------------- Copyright (C) 2012 Jon S. Berndt (jon@jsbsim.org) ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU Lesser 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 Lesser General Public License for more + details. + + You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +10/11/12 JSB Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGFCSCHANNEL_H +#define FGFCSCHANNEL_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_FCSCHANNEL "$Id: FGFCSChannel.h,v 1.1 2012/10/15 05:02:29 jberndt Exp $" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +namespace JSBSim { + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + + /** Represents a in a control system definition. + The may be defined within a , or + element. Channels are a way to group sets of components that perform + a specific purpose or algorithm. */ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +typedef std::vector FCSCompVec; + +class FGFCSChannel { +public: + /// Constructor + FGFCSChannel(FGPropertyManager* node=0) : + OnOffNode(node) + { + } + /// Destructor + ~FGFCSChannel() { + for (unsigned int i=0; i= GetNumComponents()) { + std::cerr << "Tried to get nonexistent component" << std::endl; + return 0; + } else { + return FCSComponents[i]; + } + } + /// Reset the components that can be reset + void Reset() { + for (unsigned int i=0; iGetType() == "LAG" || + FCSComponents[i]->GetType() == "LEAD_LAG" || + FCSComponents[i]->GetType() == "WASHOUT" || + FCSComponents[i]->GetType() == "SECOND_ORDER_FILTER" || + FCSComponents[i]->GetType() == "INTEGRATOR") + { + ((FGFilter*)FCSComponents[i])->ResetPastStates(); + } else if (FCSComponents[i]->GetType() == "PID" ) { + ((FGPID*)FCSComponents[i])->ResetPastStates(); + } + } + } + /// Executes all the components in a channel. + void Execute() { + // If there is an on/off property supplied for this channel, check + // the value. If it is true, permit execution to continue. If not, return + // and do not execute the channel. + if (OnOffNode != 0) + if (!OnOffNode->getBoolValue()) return; + + for (unsigned int i=0; iRun(); + } + + private: + FCSCompVec FCSComponents; + const FGPropertyManager* OnOffNode; +}; + +} + +#endif diff --git a/src/FDM/JSBSim/models/FGGroundReactions.cpp b/src/FDM/JSBSim/models/FGGroundReactions.cpp index b334851ce..fe456dd08 100644 --- a/src/FDM/JSBSim/models/FGGroundReactions.cpp +++ b/src/FDM/JSBSim/models/FGGroundReactions.cpp @@ -40,13 +40,14 @@ INCLUDES #include "FGGroundReactions.h" #include "FGLGear.h" +#include "FGAccelerations.h" #include "input_output/FGPropertyManager.h" using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.39 2012/04/01 17:05:51 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGGroundReactions.cpp,v 1.42 2012/12/15 15:16:15 bcoconni Exp $"; static const char *IdHdr = ID_GROUNDREACTIONS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -124,25 +125,36 @@ bool FGGroundReactions::GetWOW(void) const //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGGroundReactions::Load(Element* el) +bool FGGroundReactions::Load(Element* elem) { int num=0; + string fname="", file=""; + string separator = "/"; + + fname = elem->GetAttributeValue("file"); + if (!fname.empty()) { + file = FDMExec->GetFullAircraftPath() + separator + fname; + document = LoadXMLDocument(file); + if (document == 0L) return false; + } else { + document = elem; + } Debug(2); - unsigned int numContacts = el->GetNumElements("contact"); + unsigned int numContacts = document->GetNumElements("contact"); lGear.resize(numContacts); - Element* contact_element = el->FindElement("contact"); + Element* contact_element = document->FindElement("contact"); for (unsigned int idx=0; idxFindNextElement("contact"); + contact_element = document->FindNextElement("contact"); } - FGModel::Load(el); // Perform base class Load + FGModel::Load(document); // Perform base class Load for (unsigned int i=0; ibind(); - PostLoad(el, PropertyManager); + PostLoad(document, PropertyManager); return true; } @@ -169,6 +181,12 @@ string FGGroundReactions::GetGroundReactionStrings(string delimeter) const << name << " wheel rolling velocity (ft/sec)" << delimeter << name << " wheel side velocity (ft/sec)" << delimeter << name << " wheel slip (deg)" << delimeter; + } else { + string name = lGear[i]->GetName(); + buf << name << " WOW" << delimeter + << name << " stroke (ft)" << delimeter + << name << " stroke velocity (ft/sec)" << delimeter + << name << " compress force (lbs)" << delimeter; } } @@ -204,15 +222,23 @@ string FGGroundReactions::GetGroundReactionValues(string delimeter) const << gear->GetWheelRollVel() << delimeter << gear->GetWheelSideVel() << delimeter << gear->GetWheelSlipAngle() << delimeter; + } else { + FGLGear *gear = lGear[i]; + buf << (gear->GetWOW() ? "1" : "0") << delimeter + << setprecision(5) << gear->GetCompLen() << delimeter + << setprecision(6) << gear->GetCompVel() << delimeter + << setprecision(10) << gear->GetCompForce() << delimeter; } } - buf << vForces(eX) << delimeter - << vForces(eY) << delimeter - << vForces(eZ) << delimeter - << vMoments(eX) << delimeter - << vMoments(eY) << delimeter - << vMoments(eZ); + FGAccelerations* Accelerations = FDMExec->GetAccelerations(); + + buf << Accelerations->GetGroundForces(eX) << delimeter + << Accelerations->GetGroundForces(eY) << delimeter + << Accelerations->GetGroundForces(eZ) << delimeter + << Accelerations->GetGroundMoments(eX) << delimeter + << Accelerations->GetGroundMoments(eY) << delimeter + << Accelerations->GetGroundMoments(eZ); return buf.str(); } diff --git a/src/FDM/JSBSim/models/FGGroundReactions.h b/src/FDM/JSBSim/models/FGGroundReactions.h index 5774918ad..503f6bee6 100644 --- a/src/FDM/JSBSim/models/FGGroundReactions.h +++ b/src/FDM/JSBSim/models/FGGroundReactions.h @@ -44,8 +44,9 @@ INCLUDES #include "FGLGear.h" #include "math/FGColumnVector3.h" #include "input_output/FGXMLElement.h" +#include "input_output/FGXMLFileRead.h" -#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.24 2011/08/21 15:13:22 bcoconni Exp $" +#define ID_GROUNDREACTIONS "$Id: FGGroundReactions.h,v 1.25 2012/12/12 06:19:57 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -78,7 +79,7 @@ CLASS DOCUMENTATION CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGGroundReactions : public FGModel +class FGGroundReactions : public FGModel, public FGXMLFileRead { public: FGGroundReactions(FGFDMExec*); diff --git a/src/FDM/JSBSim/models/FGInertial.h b/src/FDM/JSBSim/models/FGInertial.h index 0bf9f460f..f4b5328cc 100644 --- a/src/FDM/JSBSim/models/FGInertial.h +++ b/src/FDM/JSBSim/models/FGInertial.h @@ -47,7 +47,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_INERTIAL "$Id: FGInertial.h,v 1.20 2011/10/31 14:54:41 bcoconni Exp $" +#define ID_INERTIAL "$Id: FGInertial.h,v 1.21 2012/11/23 21:42:29 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -104,7 +104,6 @@ private: double gAccelReference; double RadiusReference; double RotationRate; - double earthPosAngle; double GM; double C2_0; // WGS84 value for the C2,0 coefficient double J2; // WGS84 value for J2 diff --git a/src/FDM/JSBSim/models/FGInput.cpp b/src/FDM/JSBSim/models/FGInput.cpp old mode 100644 new mode 100755 index 6e8848545..87455e3cd --- a/src/FDM/JSBSim/models/FGInput.cpp +++ b/src/FDM/JSBSim/models/FGInput.cpp @@ -53,7 +53,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGInput.cpp,v 1.22 2012/01/21 16:46:09 jberndt Exp $"; +static const char *IdSrc = "$Id: FGInput.cpp,v 1.24 2012/11/23 16:30:36 bcoconni Exp $"; static const char *IdHdr = ID_INPUT; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -63,10 +63,8 @@ CLASS IMPLEMENTATION FGInput::FGInput(FGFDMExec* fdmex) : FGModel(fdmex) { Name = "FGInput"; - sFirstPass = dFirstPass = true; socket = 0; port = 0; - enabled = true; Debug(0); } @@ -133,10 +131,24 @@ bool FGInput::Run(bool Holding) if (command == "set") { // SET PROPERTY - node = PropertyManager->GetNode(argument); - if (node == 0) + if (argument.size() == 0) { + socket->Reply("No property argument supplied.\n"); + break; + } + try { + node = PropertyManager->GetNode(argument); + } catch(...) { + socket->Reply("Badly formed property query\n"); + break; + } + + if (node == 0) { socket->Reply("Unknown property\n"); - else { + break; + } else if (!node->hasValue()) { + socket->Reply("Not a leaf property\n"); + break; + } else { value = atof(str_value.c_str()); node->setDoubleValue(value); } @@ -154,7 +166,11 @@ bool FGInput::Run(bool Holding) socket->Reply("Badly formed property query\n"); break; } + if (node == 0) { + socket->Reply("Unknown property\n"); + break; + } else if (!node->hasValue()) { if (Holding) { // if holding can query property list string query = FDMExec->QueryPropertyCatalog(argument); socket->Reply(query); @@ -217,6 +233,7 @@ bool FGInput::Run(bool Holding) " set {property name} {value}\n" " hold\n" " resume\n" + " iterate {value}\n" " help\n" " quit\n" " info\n\n"); diff --git a/src/FDM/JSBSim/models/FGInput.h b/src/FDM/JSBSim/models/FGInput.h old mode 100644 new mode 100755 index 68f0e0c4a..2421cdaad --- a/src/FDM/JSBSim/models/FGInput.h +++ b/src/FDM/JSBSim/models/FGInput.h @@ -46,7 +46,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_INPUT "$Id: FGInput.h,v 1.9 2011/05/20 03:18:36 jberndt Exp $" +#define ID_INPUT "$Id: FGInput.h,v 1.10 2012/11/17 19:42:53 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -85,13 +85,9 @@ public: @return false if no error */ bool Run(bool Holding); - inline void Enable(void) { enabled = true; } - inline void Disable(void) { enabled = false; } - inline bool Toggle(void) {enabled = !enabled; return enabled;} bool Load(Element* el); private: - bool sFirstPass, dFirstPass, enabled; unsigned int port; FGfdmSocket* socket; std::string data; diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index b56c0d474..c1d606993 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -42,6 +42,7 @@ INCLUDES #include #include +#include #include "FGLGear.h" #include "input_output/FGPropertyManager.h" @@ -60,7 +61,7 @@ DEFINITIONS GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -static const char *IdSrc = "$Id: FGLGear.cpp,v 1.100 2012/04/01 17:05:51 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGLGear.cpp,v 1.103 2013/01/13 12:44:52 bcoconni Exp $"; static const char *IdHdr = ID_LGEAR; // Body To Structural (body frame is rotated 180 deg about Y and lengths are given in @@ -105,29 +106,39 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number, const struct Inputs& dynamicFCoeff = 1.0; } - if (el->FindElement("spring_coeff")) - kSpring = el->FindElementValueAsNumberConvertTo("spring_coeff", "LBS/FT"); - if (el->FindElement("damping_coeff")) { - Element* dampCoeff = el->FindElement("damping_coeff"); - if (dampCoeff->GetAttributeValue("type") == "SQUARE") { - eDampType = dtSquare; - bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT2/SEC2"); - } else { - bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT/SEC"); - } + PropertyManager = fdmex->GetPropertyManager(); + + fStrutForce = 0; + Element* strutForce = el->FindElement("strut_force"); + if (strutForce) { + Element* springFunc = strutForce->FindElement("function"); + fStrutForce = new FGFunction(PropertyManager, springFunc); } + else { + if (el->FindElement("spring_coeff")) + kSpring = el->FindElementValueAsNumberConvertTo("spring_coeff", "LBS/FT"); + if (el->FindElement("damping_coeff")) { + Element* dampCoeff = el->FindElement("damping_coeff"); + if (dampCoeff->GetAttributeValue("type") == "SQUARE") { + eDampType = dtSquare; + bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT2/SEC2"); + } else { + bDamp = el->FindElementValueAsNumberConvertTo("damping_coeff", "LBS/FT/SEC"); + } + } - if (el->FindElement("damping_coeff_rebound")) { - Element* dampCoeffRebound = el->FindElement("damping_coeff_rebound"); - if (dampCoeffRebound->GetAttributeValue("type") == "SQUARE") { - eDampTypeRebound = dtSquare; - bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT2/SEC2"); + if (el->FindElement("damping_coeff_rebound")) { + Element* dampCoeffRebound = el->FindElement("damping_coeff_rebound"); + if (dampCoeffRebound->GetAttributeValue("type") == "SQUARE") { + eDampTypeRebound = dtSquare; + bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT2/SEC2"); + } else { + bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT/SEC"); + } } else { - bDampRebound = el->FindElementValueAsNumberConvertTo("damping_coeff_rebound", "LBS/FT/SEC"); + bDampRebound = bDamp; + eDampTypeRebound = eDampType; } - } else { - bDampRebound = bDamp; - eDampTypeRebound = eDampType; } if (el->FindElement("dynamic_friction")) @@ -153,7 +164,6 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number, const struct Inputs& eSteerType = stSteer; GroundReactions = fdmex->GetGroundReactions(); - PropertyManager = fdmex->GetPropertyManager(); ForceY_Table = 0; Element* force_table = el->FindElement("table"); @@ -240,6 +250,7 @@ FGLGear::FGLGear(Element* el, FGFDMExec* fdmex, int number, const struct Inputs& FGLGear::~FGLGear() { delete ForceY_Table; + delete fStrutForce; Debug(1); } @@ -500,7 +511,12 @@ void FGLGear::ReportTakeoffOrLanding(void) if (debug_lvl > 0) Report(erTakeoff); } - if (lastWOW != WOW) PutMessage("GEAR_CONTACT: " + name, WOW); + if (lastWOW != WOW) + { + ostringstream buf; + buf << "GEAR_CONTACT: " << fdmex->GetSimTime() << " seconds: " << name; + PutMessage(buf.str(), WOW); + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -566,25 +582,29 @@ void FGLGear::ComputeVerticalStrutForce(void) double springForce = 0; double dampForce = 0; - springForce = -compressLength * kSpring; + if (fStrutForce) + StrutForce = min(fStrutForce->GetValue(), (double)0.0); + else { + springForce = -compressLength * kSpring; - if (compressSpeed >= 0.0) { + if (compressSpeed >= 0.0) { - if (eDampType == dtLinear) - dampForce = -compressSpeed * bDamp; - else - dampForce = -compressSpeed * compressSpeed * bDamp; + if (eDampType == dtLinear) + dampForce = -compressSpeed * bDamp; + else + dampForce = -compressSpeed * compressSpeed * bDamp; - } else { + } else { - if (eDampTypeRebound == dtLinear) - dampForce = -compressSpeed * bDampRebound; - else - dampForce = compressSpeed * compressSpeed * bDampRebound; + if (eDampTypeRebound == dtLinear) + dampForce = -compressSpeed * bDampRebound; + else + dampForce = compressSpeed * compressSpeed * bDampRebound; - } + } - StrutForce = min(springForce + dampForce, (double)0.0); + StrutForce = min(springForce + dampForce, (double)0.0); + } // The reaction force of the wheel is always normal to the ground switch (eContactType) { @@ -725,6 +745,8 @@ void FGLGear::bind(void) &FGForce::GetLocationZ, &FGForce::SetLocationZ); property_name = base_property_name + "/compression-ft"; PropertyManager->Tie( property_name.c_str(), &compressLength ); + property_name = base_property_name + "/compression-velocity-fps"; + PropertyManager->Tie( property_name.c_str(), &compressSpeed ); property_name = base_property_name + "/static_friction_coeff"; PropertyManager->Tie( property_name.c_str(), &staticFCoeff ); property_name = base_property_name + "/dynamic_friction_coeff"; diff --git a/src/FDM/JSBSim/models/FGLGear.h b/src/FDM/JSBSim/models/FGLGear.h index c2d38a0c4..7059ad2d8 100644 --- a/src/FDM/JSBSim/models/FGLGear.h +++ b/src/FDM/JSBSim/models/FGLGear.h @@ -48,7 +48,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_LGEAR "$Id: FGLGear.h,v 1.54 2012/04/01 17:05:51 bcoconni Exp $" +#define ID_LGEAR "$Id: FGLGear.h,v 1.56 2012/12/15 15:16:16 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -177,7 +177,7 @@ CLASS DOCUMENTATION @endcode @author Jon S. Berndt - @version $Id: FGLGear.h,v 1.54 2012/04/01 17:05:51 bcoconni Exp $ + @version $Id: FGLGear.h,v 1.56 2012/12/15 15:16:16 bcoconni Exp $ @see Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at NASA-Ames", NASA CR-2497, January 1975 @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics", @@ -291,6 +291,18 @@ public: UpdateForces(); FGColumnVector3 vForce = mTGear.Transposed() * FGForce::GetBodyForces(); return vForce(eY)*cos(SteerAngle) - vForce(eX)*sin(SteerAngle); } + double GetBodyXForce(void) { + UpdateForces(); + return FGForce::GetBodyForces()(eX); + } + double GetBodyYForce(void) { + UpdateForces(); + return FGForce::GetBodyForces()(eY); + } + double GetBodyZForce(void) { + UpdateForces(); + return FGForce::GetBodyForces()(eZ); + } double GetWheelRollVel(void) const { return vWhlVelVec(eX)*cos(SteerAngle) + vWhlVelVec(eY)*sin(SteerAngle); } double GetWheelSideVel(void) const { return vWhlVelVec(eY)*cos(SteerAngle) @@ -313,6 +325,7 @@ private: FGColumnVector3 vWhlVelVec, vGroundWhlVel; // Velocity of this wheel FGColumnVector3 vGroundNormal; FGTable *ForceY_Table; + FGFunction *fStrutForce; double SteerAngle; double kSpring; double bDamp; diff --git a/src/FDM/JSBSim/models/FGMassBalance.cpp b/src/FDM/JSBSim/models/FGMassBalance.cpp index dbd8ab3a6..446aaaca5 100644 --- a/src/FDM/JSBSim/models/FGMassBalance.cpp +++ b/src/FDM/JSBSim/models/FGMassBalance.cpp @@ -49,7 +49,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGMassBalance.cpp,v 1.39 2011/11/09 21:58:26 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGMassBalance.cpp,v 1.42 2012/12/12 06:19:57 jberndt Exp $"; static const char *IdHdr = ID_MASSBALANCE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -98,48 +98,58 @@ bool FGMassBalance::InitModel(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGMassBalance::Load(Element* el) +bool FGMassBalance::Load(Element* elem) { - Element *element; string element_name = ""; double bixx, biyy, bizz, bixy, bixz, biyz; + string fname="", file=""; + string separator = "/"; + + fname = elem->GetAttributeValue("file"); + if (!fname.empty()) { + file = FDMExec->GetFullAircraftPath() + separator + fname; + document = LoadXMLDocument(file); + if (document == 0L) return false; + } else { + document = elem; + } - FGModel::Load(el); // Perform base class Load. + FGModel::Load(document); // Perform base class Load. bixx = biyy = bizz = bixy = bixz = biyz = 0.0; - if (el->FindElement("ixx")) - bixx = el->FindElementValueAsNumberConvertTo("ixx", "SLUG*FT2"); - if (el->FindElement("iyy")) - biyy = el->FindElementValueAsNumberConvertTo("iyy", "SLUG*FT2"); - if (el->FindElement("izz")) - bizz = el->FindElementValueAsNumberConvertTo("izz", "SLUG*FT2"); - if (el->FindElement("ixy")) - bixy = el->FindElementValueAsNumberConvertTo("ixy", "SLUG*FT2"); - if (el->FindElement("ixz")) - bixz = el->FindElementValueAsNumberConvertTo("ixz", "SLUG*FT2"); - if (el->FindElement("iyz")) - biyz = el->FindElementValueAsNumberConvertTo("iyz", "SLUG*FT2"); + if (document->FindElement("ixx")) + bixx = document->FindElementValueAsNumberConvertTo("ixx", "SLUG*FT2"); + if (document->FindElement("iyy")) + biyy = document->FindElementValueAsNumberConvertTo("iyy", "SLUG*FT2"); + if (document->FindElement("izz")) + bizz = document->FindElementValueAsNumberConvertTo("izz", "SLUG*FT2"); + if (document->FindElement("ixy")) + bixy = document->FindElementValueAsNumberConvertTo("ixy", "SLUG*FT2"); + if (document->FindElement("ixz")) + bixz = document->FindElementValueAsNumberConvertTo("ixz", "SLUG*FT2"); + if (document->FindElement("iyz")) + biyz = document->FindElementValueAsNumberConvertTo("iyz", "SLUG*FT2"); SetAircraftBaseInertias(FGMatrix33( bixx, -bixy, bixz, -bixy, biyy, -biyz, bixz, -biyz, bizz )); - if (el->FindElement("emptywt")) { - EmptyWeight = el->FindElementValueAsNumberConvertTo("emptywt", "LBS"); + if (document->FindElement("emptywt")) { + EmptyWeight = document->FindElementValueAsNumberConvertTo("emptywt", "LBS"); } - element = el->FindElement("location"); + Element *element = document->FindElement("location"); while (element) { element_name = element->GetAttributeValue("name"); if (element_name == "CG") vbaseXYZcg = element->FindElementTripletConvertTo("IN"); - element = el->FindNextElement("location"); + element = document->FindNextElement("location"); } // Find all POINTMASS elements that descend from this METRICS branch of the // config file. - element = el->FindElement("pointmass"); + element = document->FindElement("pointmass"); while (element) { AddPointMass(element); - element = el->FindNextElement("pointmass"); + element = document->FindNextElement("pointmass"); } double ChildFDMWeight = 0.0; @@ -152,7 +162,7 @@ bool FGMassBalance::Load(Element* el) Mass = lbtoslug*Weight; - PostLoad(el, PropertyManager); + PostLoad(document, PropertyManager); Debug(2); return true; diff --git a/src/FDM/JSBSim/models/FGMassBalance.h b/src/FDM/JSBSim/models/FGMassBalance.h index 9b11290b1..947fdda08 100644 --- a/src/FDM/JSBSim/models/FGMassBalance.h +++ b/src/FDM/JSBSim/models/FGMassBalance.h @@ -38,18 +38,19 @@ SENTRY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +#include +#include #include "FGModel.h" #include "math/FGColumnVector3.h" #include "math/FGMatrix33.h" #include "input_output/FGXMLElement.h" -#include -#include +#include "input_output/FGXMLFileRead.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_MASSBALANCE "$Id: FGMassBalance.h,v 1.27 2011/11/09 21:58:26 bcoconni Exp $" +#define ID_MASSBALANCE "$Id: FGMassBalance.h,v 1.28 2012/12/12 06:19:57 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONSS @@ -107,7 +108,7 @@ CLASS DOCUMENTATION CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGMassBalance : public FGModel +class FGMassBalance : public FGModel, public FGXMLFileRead { public: diff --git a/src/FDM/JSBSim/models/FGModel.cpp b/src/FDM/JSBSim/models/FGModel.cpp index 81961eb30..79f0cc9e4 100644 --- a/src/FDM/JSBSim/models/FGModel.cpp +++ b/src/FDM/JSBSim/models/FGModel.cpp @@ -46,7 +46,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGModel.cpp,v 1.19 2011/05/22 12:44:30 jberndt Exp $"; +static const char *IdSrc = "$Id: FGModel.cpp,v 1.21 2013/01/14 22:44:52 bcoconni Exp $"; static const char *IdHdr = ID_MODEL; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -66,7 +66,7 @@ FGModel::FGModel(FGFDMExec* fdmex) //must be brought up now. PropertyManager = FDMExec->GetPropertyManager(); - exe_ctr = 0; + exe_ctr = 1; rate = 1; if (debug_lvl & 2) cout << " FGModel Base Class" << endl; @@ -83,6 +83,7 @@ FGModel::~FGModel() bool FGModel::InitModel(void) { + exe_ctr = 1; return true; } diff --git a/src/FDM/JSBSim/models/FGOutput.cpp b/src/FDM/JSBSim/models/FGOutput.cpp index f6cbcd366..28248fc34 100644 --- a/src/FDM/JSBSim/models/FGOutput.cpp +++ b/src/FDM/JSBSim/models/FGOutput.cpp @@ -3,7 +3,7 @@ Module: FGOutput.cpp Author: Jon Berndt Date started: 12/02/98 - Purpose: Manage output of sim parameters to file or stdout + Purpose: Manage output of sim parameters to file, stdout or socket Called by: FGSimExec ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------- @@ -39,110 +39,30 @@ HISTORY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#include -#include -#include -#include - #include "FGOutput.h" #include "FGFDMExec.h" -#include "FGAtmosphere.h" -#include "FGAccelerations.h" -#include "atmosphere/FGWinds.h" -#include "FGFCS.h" -#include "FGAerodynamics.h" -#include "FGGroundReactions.h" -#include "FGExternalReactions.h" -#include "FGBuoyantForces.h" -#include "FGAircraft.h" -#include "FGMassBalance.h" -#include "FGPropagate.h" -#include "FGAuxiliary.h" -#include "FGInertial.h" -#include "FGPropulsion.h" -#include "models/propulsion/FGEngine.h" -#include "models/propulsion/FGTank.h" -#include "models/propulsion/FGPiston.h" - -#if defined(WIN32) && !defined(__CYGWIN__) -# include -#else -# include // htonl() ntohl() -#endif - -static const int endianTest = 1; -#define isLittleEndian (*((char *) &endianTest ) != 0) +#include "input_output/FGOutputSocket.h" +#include "input_output/FGOutputTextFile.h" +#include "input_output/FGOutputFG.h" using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGOutput.cpp,v 1.67 2012/04/27 12:14:56 jberndt Exp $"; +static const char *IdSrc = "$Id: FGOutput.cpp,v 1.71 2012/12/15 16:13:58 bcoconni Exp $"; static const char *IdHdr = ID_OUTPUT; -// (stolen from FGFS native_fdm.cxx) -// The function htond is defined this way due to the way some -// processors and OSes treat floating point values. Some will raise -// an exception whenever a "bad" floating point value is loaded into a -// floating point register. Solaris is notorious for this, but then -// so is LynxOS on the PowerPC. By translating the data in place, -// there is no need to load a FP register with the "corruped" floating -// point value. By doing the BIG_ENDIAN test, I can optimize the -// routine for big-endian processors so it can be as efficient as -// possible -static void htond (double &x) -{ - if ( isLittleEndian ) { - int *Double_Overlay; - int Holding_Buffer; - - Double_Overlay = (int *) &x; - Holding_Buffer = Double_Overlay [0]; - - Double_Overlay [0] = htonl (Double_Overlay [1]); - Double_Overlay [1] = htonl (Holding_Buffer); - } else { - return; - } -} - -// Float version -static void htonf (float &x) -{ - if ( isLittleEndian ) { - int *Float_Overlay; - int Holding_Buffer; - - Float_Overlay = (int *) &x; - Holding_Buffer = Float_Overlay [0]; - - Float_Overlay [0] = htonl (Holding_Buffer); - } else { - return; - } -} - - /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex) { + typedef int (FGOutput::*iOPV)(void) const; + Name = "FGOutput"; - sFirstPass = dFirstPass = true; - socket = 0; - runID_postfix = 0; - Type = otNone; - SubSystems = 0; - enabled = true; - StartNewFile = false; - delimeter = ", "; - BaseFilename = Filename = ""; - DirectivesFile = ""; - output_file_name = ""; - - memset(&fgSockBuf, 0x00, sizeof(fgSockBuf)); + + fdmex->GetPropertyManager()->Tie("simulation/force-output", this, (iOPV)0, &FGOutput::ForceOutput, false); Debug(0); } @@ -151,8 +71,10 @@ FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex) FGOutput::~FGOutput() { - delete socket; - OutputProperties.clear(); + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + delete (*it); + Debug(1); } @@ -160,21 +82,13 @@ FGOutput::~FGOutput() bool FGOutput::InitModel(void) { - if (Filename.size() > 0 && StartNewFile) { - ostringstream buf; - string::size_type dot = BaseFilename.find_last_of('.'); - if (dot != string::npos) { - buf << BaseFilename.substr(0, dot) << '_' << runID_postfix++ << BaseFilename.substr(dot); - } else { - buf << BaseFilename << '_' << runID_postfix++; - } - Filename = buf.str(); - datafile.close(); - StartNewFile = false; - dFirstPass = true; - } + bool ret = false; - return true; + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + ret &= (*it)->InitModel(); + + return ret; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -183,11 +97,10 @@ bool FGOutput::Run(bool Holding) { if (FGModel::Run(Holding)) return true; - if (enabled && !FDMExec->IntegrationSuspended() && !Holding) { - RunPreFunctions(); - Print(); - RunPostFunctions(); - } + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + (*it)->Run(Holding); + return false; } @@ -195,899 +108,181 @@ bool FGOutput::Run(bool Holding) void FGOutput::Print(void) { - if (Type == otSocket) { - SocketOutput(); - } else if (Type == otFlightGear) { - FlightGearSocketOutput(); - } else if (Type == otCSV || Type == otTab) { - DelimitedOutput(Filename); - } else if (Type == otTerminal) { - // Not done yet - } else if (Type == otNone) { - // Do nothing - } else { - // Not a valid type of output - } + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + (*it)->Print(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::SetType(const string& type) +void FGOutput::SetStartNewOutput(void) { - if (type == "CSV") { - Type = otCSV; - delimeter = ","; - } else if (type == "TABULAR") { - Type = otTab; - delimeter = "\t"; - } else if (type == "SOCKET") { - Type = otSocket; - } else if (type == "FLIGHTGEAR") { - Type = otFlightGear; - } else if (type == "TERMINAL") { - Type = otTerminal; - } else if (type != string("NONE")) { - Type = otUnknown; - cerr << "Unknown type of output specified in config file" << endl; - } + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + (*it)->SetStartNewOutput(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::SetProtocol(const string& protocol) +void FGOutput::Enable(void) { - if (protocol == "UDP") Protocol = FGfdmSocket::ptUDP; - else if (protocol == "TCP") Protocol = FGfdmSocket::ptTCP; - else Protocol = FGfdmSocket::ptTCP; // Default to TCP + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + (*it)->Enable(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::DelimitedOutput(const string& fname) +void FGOutput::Disable(void) { - const FGAerodynamics* Aerodynamics = FDMExec->GetAerodynamics(); - const FGAuxiliary* Auxiliary = FDMExec->GetAuxiliary(); - const FGAircraft* Aircraft = FDMExec->GetAircraft(); - const FGAtmosphere* Atmosphere = FDMExec->GetAtmosphere(); - const FGWinds* Winds = FDMExec->GetWinds(); - const FGPropulsion* Propulsion = FDMExec->GetPropulsion(); - const FGMassBalance* MassBalance = FDMExec->GetMassBalance(); - const FGPropagate* Propagate = FDMExec->GetPropagate(); - const FGAccelerations* Accelerations = FDMExec->GetAccelerations(); - const FGFCS* FCS = FDMExec->GetFCS(); - const FGGroundReactions* GroundReactions = FDMExec->GetGroundReactions(); - const FGExternalReactions* ExternalReactions = FDMExec->GetExternalReactions(); - const FGBuoyantForces* BuoyantForces = FDMExec->GetBuoyantForces(); - - streambuf* buffer; - string scratch = ""; - - if (fname == "COUT" || fname == "cout") { - buffer = cout.rdbuf(); - } else { - if (!datafile.is_open()) datafile.open(fname.c_str()); - buffer = datafile.rdbuf(); - } - - ostream outstream(buffer); - - outstream.precision(10); - - if (dFirstPass) { - outstream << "Time"; - if (SubSystems & ssSimulation) { - // Nothing here, yet - } - if (SubSystems & ssAerosurfaces) { - outstream << delimeter; - outstream << "Aileron Command (norm)" + delimeter; - outstream << "Elevator Command (norm)" + delimeter; - outstream << "Rudder Command (norm)" + delimeter; - outstream << "Flap Command (norm)" + delimeter; - outstream << "Left Aileron Position (deg)" + delimeter; - outstream << "Right Aileron Position (deg)" + delimeter; - outstream << "Elevator Position (deg)" + delimeter; - outstream << "Rudder Position (deg)" + delimeter; - outstream << "Flap Position (deg)"; - } - if (SubSystems & ssRates) { - outstream << delimeter; - outstream << "P (deg/s)" + delimeter + "Q (deg/s)" + delimeter + "R (deg/s)" + delimeter; - outstream << "P dot (deg/s^2)" + delimeter + "Q dot (deg/s^2)" + delimeter + "R dot (deg/s^2)" + delimeter; - outstream << "P_{inertial} (deg/s)" + delimeter + "Q_{inertial} (deg/s)" + delimeter + "R_{inertial} (deg/s)"; - } - if (SubSystems & ssVelocities) { - outstream << delimeter; - outstream << "q bar (psf)" + delimeter; - outstream << "Reynolds Number" + delimeter; - outstream << "V_{Total} (ft/s)" + delimeter; - outstream << "V_{Inertial} (ft/s)" + delimeter; - outstream << "UBody" + delimeter + "VBody" + delimeter + "WBody" + delimeter; - outstream << "Aero V_{X Body} (ft/s)" + delimeter + "Aero V_{Y Body} (ft/s)" + delimeter + "Aero V_{Z Body} (ft/s)" + delimeter; - outstream << "V_{X_{inertial}} (ft/s)" + delimeter + "V_{Y_{inertial}} (ft/s)" + delimeter + "V_{Z_{inertial}} (ft/s)" + delimeter; - outstream << "V_{X_{ecef}} (ft/s)" + delimeter + "V_{Y_{ecef}} (ft/s)" + delimeter + "V_{Z_{ecef}} (ft/s)" + delimeter; - outstream << "V_{North} (ft/s)" + delimeter + "V_{East} (ft/s)" + delimeter + "V_{Down} (ft/s)"; - } - if (SubSystems & ssForces) { - outstream << delimeter; - outstream << "F_{Drag} (lbs)" + delimeter + "F_{Side} (lbs)" + delimeter + "F_{Lift} (lbs)" + delimeter; - outstream << "L/D" + delimeter; - outstream << "F_{Aero x} (lbs)" + delimeter + "F_{Aero y} (lbs)" + delimeter + "F_{Aero z} (lbs)" + delimeter; - outstream << "F_{Prop x} (lbs)" + delimeter + "F_{Prop y} (lbs)" + delimeter + "F_{Prop z} (lbs)" + delimeter; - outstream << "F_{Gear x} (lbs)" + delimeter + "F_{Gear y} (lbs)" + delimeter + "F_{Gear z} (lbs)" + delimeter; - outstream << "F_{Ext x} (lbs)" + delimeter + "F_{Ext y} (lbs)" + delimeter + "F_{Ext z} (lbs)" + delimeter; - outstream << "F_{Buoyant x} (lbs)" + delimeter + "F_{Buoyant y} (lbs)" + delimeter + "F_{Buoyant z} (lbs)" + delimeter; - outstream << "F_{Total x} (lbs)" + delimeter + "F_{Total y} (lbs)" + delimeter + "F_{Total z} (lbs)"; - } - if (SubSystems & ssMoments) { - outstream << delimeter; - outstream << "L_{Aero} (ft-lbs)" + delimeter + "M_{Aero} (ft-lbs)" + delimeter + "N_{Aero} (ft-lbs)" + delimeter; - outstream << "L_{Prop} (ft-lbs)" + delimeter + "M_{Prop} (ft-lbs)" + delimeter + "N_{Prop} (ft-lbs)" + delimeter; - outstream << "L_{Gear} (ft-lbs)" + delimeter + "M_{Gear} (ft-lbs)" + delimeter + "N_{Gear} (ft-lbs)" + delimeter; - outstream << "L_{ext} (ft-lbs)" + delimeter + "M_{ext} (ft-lbs)" + delimeter + "N_{ext} (ft-lbs)" + delimeter; - outstream << "L_{Buoyant} (ft-lbs)" + delimeter + "M_{Buoyant} (ft-lbs)" + delimeter + "N_{Buoyant} (ft-lbs)" + delimeter; - outstream << "L_{Total} (ft-lbs)" + delimeter + "M_{Total} (ft-lbs)" + delimeter + "N_{Total} (ft-lbs)"; - } - if (SubSystems & ssAtmosphere) { - outstream << delimeter; - outstream << "Rho (slugs/ft^3)" + delimeter; - outstream << "Absolute Viscosity" + delimeter; - outstream << "Kinematic Viscosity" + delimeter; - outstream << "Temperature (R)" + delimeter; - outstream << "P_{SL} (psf)" + delimeter; - outstream << "P_{Ambient} (psf)" + delimeter; - outstream << "Turbulence Magnitude (ft/sec)" + delimeter; - outstream << "Turbulence X Direction (rad)" + delimeter + "Turbulence Y Direction (rad)" + delimeter + "Turbulence Z Direction (rad)" + delimeter; - outstream << "Wind V_{North} (ft/s)" + delimeter + "Wind V_{East} (ft/s)" + delimeter + "Wind V_{Down} (ft/s)"; - } - if (SubSystems & ssMassProps) { - outstream << delimeter; - outstream << "I_{xx}" + delimeter; - outstream << "I_{xy}" + delimeter; - outstream << "I_{xz}" + delimeter; - outstream << "I_{yx}" + delimeter; - outstream << "I_{yy}" + delimeter; - outstream << "I_{yz}" + delimeter; - outstream << "I_{zx}" + delimeter; - outstream << "I_{zy}" + delimeter; - outstream << "I_{zz}" + delimeter; - outstream << "Mass" + delimeter; - outstream << "X_{cg}" + delimeter + "Y_{cg}" + delimeter + "Z_{cg}"; - } - if (SubSystems & ssPropagate) { - outstream << delimeter; - outstream << "Altitude ASL (ft)" + delimeter; - outstream << "Altitude AGL (ft)" + delimeter; - outstream << "Phi (deg)" + delimeter + "Theta (deg)" + delimeter + "Psi (deg)" + delimeter; - outstream << "Alpha (deg)" + delimeter; - outstream << "Beta (deg)" + delimeter; - outstream << "Latitude (deg)" + delimeter; - outstream << "Longitude (deg)" + delimeter; - outstream << "X_{ECI} (ft)" + delimeter + "Y_{ECI} (ft)" + delimeter + "Z_{ECI} (ft)" + delimeter; - outstream << "X_{ECEF} (ft)" + delimeter + "Y_{ECEF} (ft)" + delimeter + "Z_{ECEF} (ft)" + delimeter; - outstream << "Earth Position Angle (deg)" + delimeter; - outstream << "Distance AGL (ft)" + delimeter; - outstream << "Terrain Elevation (ft)"; - } - if (SubSystems & ssAeroFunctions) { - scratch = Aerodynamics->GetAeroFunctionStrings(delimeter); - if (scratch.length() != 0) outstream << delimeter << scratch; - } - if (SubSystems & ssFCS) { - scratch = FCS->GetComponentStrings(delimeter); - if (scratch.length() != 0) outstream << delimeter << scratch; - } - if (SubSystems & ssGroundReactions) { - outstream << delimeter; - outstream << GroundReactions->GetGroundReactionStrings(delimeter); - } - if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { - outstream << delimeter; - outstream << Propulsion->GetPropulsionStrings(delimeter); - } - if (OutputProperties.size() > 0) { - for (unsigned int i=0;iGetPrintableName(); - } - } - - outstream << endl; - dFirstPass = false; - } + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + (*it)->Disable(); +} - outstream << FDMExec->GetSimTime(); - if (SubSystems & ssSimulation) { - } - if (SubSystems & ssAerosurfaces) { - outstream << delimeter; - outstream << FCS->GetDaCmd() << delimeter; - outstream << FCS->GetDeCmd() << delimeter; - outstream << FCS->GetDrCmd() << delimeter; - outstream << FCS->GetDfCmd() << delimeter; - outstream << FCS->GetDaLPos(ofDeg) << delimeter; - outstream << FCS->GetDaRPos(ofDeg) << delimeter; - outstream << FCS->GetDePos(ofDeg) << delimeter; - outstream << FCS->GetDrPos(ofDeg) << delimeter; - outstream << FCS->GetDfPos(ofDeg); - } - if (SubSystems & ssRates) { - outstream << delimeter; - outstream << (radtodeg*Propagate->GetPQR()).Dump(delimeter) << delimeter; - outstream << (radtodeg*Accelerations->GetPQRdot()).Dump(delimeter) << delimeter; - outstream << (radtodeg*Propagate->GetPQRi()).Dump(delimeter); - } - if (SubSystems & ssVelocities) { - outstream << delimeter; - outstream << Auxiliary->Getqbar() << delimeter; - outstream << Auxiliary->GetReynoldsNumber() << delimeter; - outstream << setprecision(12) << Auxiliary->GetVt() << delimeter; - outstream << Propagate->GetInertialVelocityMagnitude() << delimeter; - outstream << setprecision(12) << Propagate->GetUVW().Dump(delimeter) << delimeter; - outstream << Auxiliary->GetAeroUVW().Dump(delimeter) << delimeter; - outstream << Propagate->GetInertialVelocity().Dump(delimeter) << delimeter; - outstream << Propagate->GetECEFVelocity().Dump(delimeter) << delimeter; - outstream << Propagate->GetVel().Dump(delimeter); - outstream.precision(10); - } - if (SubSystems & ssForces) { - outstream << delimeter; - outstream << Aerodynamics->GetvFw().Dump(delimeter) << delimeter; - outstream << Aerodynamics->GetLoD() << delimeter; - outstream << Aerodynamics->GetForces().Dump(delimeter) << delimeter; - outstream << Propulsion->GetForces().Dump(delimeter) << delimeter; - outstream << GroundReactions->GetForces().Dump(delimeter) << delimeter; - outstream << ExternalReactions->GetForces().Dump(delimeter) << delimeter; - outstream << BuoyantForces->GetForces().Dump(delimeter) << delimeter; - outstream << Aircraft->GetForces().Dump(delimeter); - } - if (SubSystems & ssMoments) { - outstream << delimeter; - outstream << Aerodynamics->GetMoments().Dump(delimeter) << delimeter; - outstream << Propulsion->GetMoments().Dump(delimeter) << delimeter; - outstream << GroundReactions->GetMoments().Dump(delimeter) << delimeter; - outstream << ExternalReactions->GetMoments().Dump(delimeter) << delimeter; - outstream << BuoyantForces->GetMoments().Dump(delimeter) << delimeter; - outstream << Aircraft->GetMoments().Dump(delimeter); - } - if (SubSystems & ssAtmosphere) { - outstream << delimeter; - outstream << Atmosphere->GetDensity() << delimeter; - outstream << Atmosphere->GetAbsoluteViscosity() << delimeter; - outstream << Atmosphere->GetKinematicViscosity() << delimeter; - outstream << Atmosphere->GetTemperature() << delimeter; - outstream << Atmosphere->GetPressureSL() << delimeter; - outstream << Atmosphere->GetPressure() << delimeter; - outstream << Winds->GetTurbMagnitude() << delimeter; - outstream << Winds->GetTurbDirection().Dump(delimeter) << delimeter; - outstream << Winds->GetTotalWindNED().Dump(delimeter); - } - if (SubSystems & ssMassProps) { - outstream << delimeter; - outstream << MassBalance->GetJ().Dump(delimeter) << delimeter; - outstream << MassBalance->GetMass() << delimeter; - outstream << MassBalance->GetXYZcg().Dump(delimeter); - } - if (SubSystems & ssPropagate) { - outstream.precision(14); - outstream << delimeter; - outstream << Propagate->GetAltitudeASL() << delimeter; - outstream << Propagate->GetDistanceAGL() << delimeter; - outstream << (radtodeg*Propagate->GetEuler()).Dump(delimeter) << delimeter; - outstream << Auxiliary->Getalpha(inDegrees) << delimeter; - outstream << Auxiliary->Getbeta(inDegrees) << delimeter; - outstream << Propagate->GetLocation().GetLatitudeDeg() << delimeter; - outstream << Propagate->GetLocation().GetLongitudeDeg() << delimeter; - outstream.precision(18); - outstream << ((FGColumnVector3)Propagate->GetInertialPosition()).Dump(delimeter) << delimeter; - outstream << ((FGColumnVector3)Propagate->GetLocation()).Dump(delimeter) << delimeter; - outstream.precision(14); - outstream << Propagate->GetEarthPositionAngleDeg() << delimeter; - outstream << Propagate->GetDistanceAGL() << delimeter; - outstream << Propagate->GetTerrainElevation(); - outstream.precision(10); - } - if (SubSystems & ssAeroFunctions) { - scratch = Aerodynamics->GetAeroFunctionValues(delimeter); - if (scratch.length() != 0) outstream << delimeter << scratch; - } - if (SubSystems & ssFCS) { - scratch = FCS->GetComponentValues(delimeter); - if (scratch.length() != 0) outstream << delimeter << scratch; - } - if (SubSystems & ssGroundReactions) { - outstream << delimeter; - outstream << GroundReactions->GetGroundReactionValues(delimeter); - } - if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { - outstream << delimeter; - outstream << Propulsion->GetPropulsionValues(delimeter); - } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - outstream.precision(18); - for (unsigned int i=0;igetDoubleValue(); - } - outstream.precision(10); +bool FGOutput::Toggle(int idx) +{ + if (idx >= (int)0 && idx < (int)OutputTypes.size()) + return OutputTypes[idx]->Toggle(); - outstream << endl; - outstream.flush(); + return false; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::SocketDataFill(FGNetFDM* net) +void FGOutput::SetRate(double rate) { - const FGAuxiliary* Auxiliary = FDMExec->GetAuxiliary(); - const FGPropulsion* Propulsion = FDMExec->GetPropulsion(); - const FGPropagate* Propagate = FDMExec->GetPropagate(); - const FGFCS* FCS = FDMExec->GetFCS(); - const FGGroundReactions* GroundReactions = FDMExec->GetGroundReactions(); - unsigned int i; - - // Version - net->version = FG_NET_FDM_VERSION; - - // Positions - net->longitude = Propagate->GetLocation().GetLongitude(); // geodetic (radians) - net->latitude = Propagate->GetLocation().GetLatitude(); // geodetic (radians) - net->altitude = Propagate->GetAltitudeASL()*0.3048; // altitude, above sea level (meters) - net->agl = (float)(Propagate->GetDistanceAGL()*0.3048); // altitude, above ground level (meters) - - net->phi = (float)(Propagate->GetEuler(ePhi)); // roll (radians) - net->theta = (float)(Propagate->GetEuler(eTht)); // pitch (radians) - net->psi = (float)(Propagate->GetEuler(ePsi)); // yaw or true heading (radians) - - net->alpha = (float)(Auxiliary->Getalpha()); // angle of attack (radians) - net->beta = (float)(Auxiliary->Getbeta()); // side slip angle (radians) - - // Velocities - net->phidot = (float)(Auxiliary->GetEulerRates(ePhi)); // roll rate (radians/sec) - net->thetadot = (float)(Auxiliary->GetEulerRates(eTht)); // pitch rate (radians/sec) - net->psidot = (float)(Auxiliary->GetEulerRates(ePsi)); // yaw rate (radians/sec) - net->vcas = (float)(Auxiliary->GetVcalibratedFPS()); // VCAS, ft/sec - net->climb_rate = (float)(Propagate->Gethdot()); // altitude rate, ft/sec - net->v_north = (float)(Propagate->GetVel(eNorth)); // north vel in NED frame, fps - net->v_east = (float)(Propagate->GetVel(eEast)); // east vel in NED frame, fps - net->v_down = (float)(Propagate->GetVel(eDown)); // down vel in NED frame, fps -//---ADD METHOD TO CALCULATE THESE TERMS--- - net->v_wind_body_north = (float)(Propagate->GetVel(eNorth)); // north vel in NED relative to airmass, fps - net->v_wind_body_east = (float)(Propagate->GetVel(eEast)); // east vel in NED relative to airmass, fps - net->v_wind_body_down = (float)(Propagate->GetVel(eDown)); // down vel in NED relative to airmass, fps - - // Accelerations - net->A_X_pilot = (float)(Auxiliary->GetPilotAccel(1)); // X body accel, ft/s/s - net->A_Y_pilot = (float)(Auxiliary->GetPilotAccel(2)); // Y body accel, ft/s/s - net->A_Z_pilot = (float)(Auxiliary->GetPilotAccel(3)); // Z body accel, ft/s/s - - // Stall - net->stall_warning = 0.0; // 0.0 - 1.0 indicating the amount of stall - net->slip_deg = (float)(Auxiliary->Getbeta(inDegrees)); // slip ball deflection, deg - - // Engine status - net->num_engines = Propulsion->GetNumEngines(); // Number of valid engines - - for (i=0; inum_engines; i++) { - if (Propulsion->GetEngine(i)->GetRunning()) - net->eng_state[i] = 2; // Engine state running - else if (Propulsion->GetEngine(i)->GetCranking()) - net->eng_state[i] = 1; // Engine state cranking - else - net->eng_state[i] = 0; // Engine state off - - switch (Propulsion->GetEngine(i)->GetType()) { - case (FGEngine::etRocket): - break; - case (FGEngine::etPiston): - net->rpm[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getRPM()); - net->fuel_flow[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getFuelFlow_gph()); - net->fuel_px[i] = 0; // Fuel pressure, psi (N/A in current model) - net->egt[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->GetEGT()); - net->cht[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getCylinderHeadTemp_degF()); - net->mp_osi[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getManifoldPressure_inHg()); - net->oil_temp[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getOilTemp_degF()); - net->oil_px[i] = (float)(((FGPiston *)Propulsion->GetEngine(i))->getOilPressure_psi()); - net->tit[i] = 0; // Turbine Inlet Temperature (N/A for piston) - break; - case (FGEngine::etTurbine): - break; - case (FGEngine::etTurboprop): - break; - case (FGEngine::etElectric): - break; - case (FGEngine::etUnknown): - break; - } - } - - // Consumables - net->num_tanks = Propulsion->GetNumTanks(); // Max number of fuel tanks - - for (i=0; inum_tanks; i++) { - net->fuel_quantity[i] = (float)(((FGTank *)Propulsion->GetTank(i))->GetContents()); - } + vector::iterator it; + for (it = OutputTypes.begin(); it != OutputTypes.end(); ++it) + (*it)->SetRate(rate); +} - // Gear status - net->num_wheels = GroundReactions->GetNumGearUnits(); - - for (i=0; inum_wheels; i++) { - net->wow[i] = GroundReactions->GetGearUnit(i)->GetWOW(); - if (GroundReactions->GetGearUnit(i)->GetGearUnitDown()) - net->gear_pos[i] = 1; //gear down, using FCS convention - else - net->gear_pos[i] = 0; //gear up, using FCS convention - net->gear_steer[i] = (float)(GroundReactions->GetGearUnit(i)->GetSteerNorm()); - net->gear_compression[i] = (float)(GroundReactions->GetGearUnit(i)->GetCompLen()); - } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - // Environment - net->cur_time = (long int)1234567890; // Friday, Feb 13, 2009, 23:31:30 UTC (not processed by FGFS anyway) - net->warp = 0; // offset in seconds to unix time - net->visibility = 25000.0; // visibility in meters (for env. effects) - - // Control surface positions (normalized values) - net->elevator = (float)(FCS->GetDePos(ofNorm)); // Norm Elevator Pos, -- - net->elevator_trim_tab = (float)(FCS->GetPitchTrimCmd()); // Norm Elev Trim Tab Pos, -- - net->left_flap = (float)(FCS->GetDfPos(ofNorm)); // Norm Flap Pos, -- - net->right_flap = (float)(FCS->GetDfPos(ofNorm)); // Norm Flap Pos, -- - net->left_aileron = (float)(FCS->GetDaLPos(ofNorm)); // Norm L Aileron Pos, -- - net->right_aileron = (float)(FCS->GetDaRPos(ofNorm)); // Norm R Aileron Pos, -- - net->rudder = (float)(FCS->GetDrPos(ofNorm)); // Norm Rudder Pos, -- - net->nose_wheel = (float)(FCS->GetDrPos(ofNorm)); // *** FIX *** Using Rudder Pos for NWS, -- - net->speedbrake = (float)(FCS->GetDsbPos(ofNorm)); // Norm Speedbrake Pos, -- - net->spoilers = (float)(FCS->GetDspPos(ofNorm)); // Norm Spoiler Pos, -- - - // Convert the net buffer to network format - if ( isLittleEndian ) { - net->version = htonl(net->version); - - htond(net->longitude); - htond(net->latitude); - htond(net->altitude); - htonf(net->agl); - htonf(net->phi); - htonf(net->theta); - htonf(net->psi); - htonf(net->alpha); - htonf(net->beta); - - htonf(net->phidot); - htonf(net->thetadot); - htonf(net->psidot); - htonf(net->vcas); - htonf(net->climb_rate); - htonf(net->v_north); - htonf(net->v_east); - htonf(net->v_down); - htonf(net->v_wind_body_north); - htonf(net->v_wind_body_east); - htonf(net->v_wind_body_down); - - htonf(net->A_X_pilot); - htonf(net->A_Y_pilot); - htonf(net->A_Z_pilot); - - htonf(net->stall_warning); - htonf(net->slip_deg); - - for (i=0; inum_engines; ++i ) { - net->eng_state[i] = htonl(net->eng_state[i]); - htonf(net->rpm[i]); - htonf(net->fuel_flow[i]); - htonf(net->fuel_px[i]); - htonf(net->egt[i]); - htonf(net->cht[i]); - htonf(net->mp_osi[i]); - htonf(net->tit[i]); - htonf(net->oil_temp[i]); - htonf(net->oil_px[i]); - } - net->num_engines = htonl(net->num_engines); - - for (i=0; inum_tanks; ++i ) { - htonf(net->fuel_quantity[i]); - } - net->num_tanks = htonl(net->num_tanks); - - for (i=0; inum_wheels; ++i ) { - net->wow[i] = htonl(net->wow[i]); - htonf(net->gear_pos[i]); - htonf(net->gear_steer[i]); - htonf(net->gear_compression[i]); - } - net->num_wheels = htonl(net->num_wheels); - - net->cur_time = htonl( net->cur_time ); - net->warp = htonl( net->warp ); - htonf(net->visibility); - - htonf(net->elevator); - htonf(net->elevator_trim_tab); - htonf(net->left_flap); - htonf(net->right_flap); - htonf(net->left_aileron); - htonf(net->right_aileron); - htonf(net->rudder); - htonf(net->nose_wheel); - htonf(net->speedbrake); - htonf(net->spoilers); - } +void FGOutput::ForceOutput(int idx) +{ + if (idx >= (int)0 && idx < (int)OutputTypes.size()) + OutputTypes[idx]->Print(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::FlightGearSocketOutput(void) +bool FGOutput::SetOutputName(unsigned int idx, const std::string& name) { - int length = sizeof(fgSockBuf); - - if (socket == NULL) return; - if (!socket->GetConnectStatus()) return; + if (idx >= OutputTypes.size()) return false; - SocketDataFill(&fgSockBuf); - socket->Send((char *)&fgSockBuf, length); + OutputTypes[idx]->SetOutputName(name); + return true; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::SocketOutput(void) +string FGOutput::GetOutputName(unsigned int idx) const { - const FGAerodynamics* Aerodynamics = FDMExec->GetAerodynamics(); - const FGAuxiliary* Auxiliary = FDMExec->GetAuxiliary(); - const FGPropulsion* Propulsion = FDMExec->GetPropulsion(); - const FGMassBalance* MassBalance = FDMExec->GetMassBalance(); - const FGPropagate* Propagate = FDMExec->GetPropagate(); - const FGAccelerations* Accelerations = FDMExec->GetAccelerations(); - const FGFCS* FCS = FDMExec->GetFCS(); - const FGAtmosphere* Atmosphere = FDMExec->GetAtmosphere(); - const FGWinds* Winds = FDMExec->GetWinds(); - const FGAircraft* Aircraft = FDMExec->GetAircraft(); - const FGGroundReactions* GroundReactions = FDMExec->GetGroundReactions(); - - string asciiData, scratch; - - if (socket == NULL) return; - if (!socket->GetConnectStatus()) return; - - socket->Clear(); - if (sFirstPass) { - socket->Clear(""); - socket->Append("Time"); - - if (SubSystems & ssAerosurfaces) { - socket->Append("Aileron Command"); - socket->Append("Elevator Command"); - socket->Append("Rudder Command"); - socket->Append("Flap Command"); - socket->Append("Left Aileron Position"); - socket->Append("Right Aileron Position"); - socket->Append("Elevator Position"); - socket->Append("Rudder Position"); - socket->Append("Flap Position"); - } + string name; - if (SubSystems & ssRates) { - socket->Append("P"); - socket->Append("Q"); - socket->Append("R"); - socket->Append("PDot"); - socket->Append("QDot"); - socket->Append("RDot"); - } + if (idx < OutputTypes.size()) + name = OutputTypes[idx]->GetOutputName(); + return name; +} - if (SubSystems & ssVelocities) { - socket->Append("QBar"); - socket->Append("Vtotal"); - socket->Append("UBody"); - socket->Append("VBody"); - socket->Append("WBody"); - socket->Append("UAero"); - socket->Append("VAero"); - socket->Append("WAero"); - socket->Append("Vn"); - socket->Append("Ve"); - socket->Append("Vd"); - } - if (SubSystems & ssForces) { - socket->Append("F_Drag"); - socket->Append("F_Side"); - socket->Append("F_Lift"); - socket->Append("LoD"); - socket->Append("Fx"); - socket->Append("Fy"); - socket->Append("Fz"); - } - if (SubSystems & ssMoments) { - socket->Append("L"); - socket->Append("M"); - socket->Append("N"); - } - if (SubSystems & ssAtmosphere) { - socket->Append("Rho"); - socket->Append("SL pressure"); - socket->Append("Ambient pressure"); - socket->Append("Turbulence Magnitude"); - socket->Append("Turbulence Direction X"); - socket->Append("Turbulence Direction Y"); - socket->Append("Turbulence Direction Z"); - socket->Append("NWind"); - socket->Append("EWind"); - socket->Append("DWind"); - } - if (SubSystems & ssMassProps) { - socket->Append("Ixx"); - socket->Append("Ixy"); - socket->Append("Ixz"); - socket->Append("Iyx"); - socket->Append("Iyy"); - socket->Append("Iyz"); - socket->Append("Izx"); - socket->Append("Izy"); - socket->Append("Izz"); - socket->Append("Mass"); - socket->Append("Xcg"); - socket->Append("Ycg"); - socket->Append("Zcg"); - } - if (SubSystems & ssPropagate) { - socket->Append("Altitude"); - socket->Append("Phi (deg)"); - socket->Append("Tht (deg)"); - socket->Append("Psi (deg)"); - socket->Append("Alpha (deg)"); - socket->Append("Beta (deg)"); - socket->Append("Latitude (deg)"); - socket->Append("Longitude (deg)"); - } - if (SubSystems & ssAeroFunctions) { - scratch = Aerodynamics->GetAeroFunctionStrings(","); - if (scratch.length() != 0) socket->Append(scratch); - } - if (SubSystems & ssFCS) { - scratch = FCS->GetComponentStrings(","); - if (scratch.length() != 0) socket->Append(scratch); - } - if (SubSystems & ssGroundReactions) { - socket->Append(GroundReactions->GetGroundReactionStrings(",")); - } - if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { - socket->Append(Propulsion->GetPropulsionStrings(",")); - } - if (OutputProperties.size() > 0) { - for (unsigned int i=0;iAppend(OutputProperties[i]->GetPrintableName()); - } - } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - sFirstPass = false; - socket->Send(); - } +bool FGOutput::SetDirectivesFile(const std::string& fname) +{ + Element* document = LoadXMLDocument(fname); + bool result = Load(document); - socket->Clear(); - socket->Append(FDMExec->GetSimTime()); - - if (SubSystems & ssAerosurfaces) { - socket->Append(FCS->GetDaCmd()); - socket->Append(FCS->GetDeCmd()); - socket->Append(FCS->GetDrCmd()); - socket->Append(FCS->GetDfCmd()); - socket->Append(FCS->GetDaLPos()); - socket->Append(FCS->GetDaRPos()); - socket->Append(FCS->GetDePos()); - socket->Append(FCS->GetDrPos()); - socket->Append(FCS->GetDfPos()); - } - if (SubSystems & ssRates) { - socket->Append(radtodeg*Propagate->GetPQR(eP)); - socket->Append(radtodeg*Propagate->GetPQR(eQ)); - socket->Append(radtodeg*Propagate->GetPQR(eR)); - socket->Append(radtodeg*Accelerations->GetPQRdot(eP)); - socket->Append(radtodeg*Accelerations->GetPQRdot(eQ)); - socket->Append(radtodeg*Accelerations->GetPQRdot(eR)); - } - if (SubSystems & ssVelocities) { - socket->Append(Auxiliary->Getqbar()); - socket->Append(Auxiliary->GetVt()); - socket->Append(Propagate->GetUVW(eU)); - socket->Append(Propagate->GetUVW(eV)); - socket->Append(Propagate->GetUVW(eW)); - socket->Append(Auxiliary->GetAeroUVW(eU)); - socket->Append(Auxiliary->GetAeroUVW(eV)); - socket->Append(Auxiliary->GetAeroUVW(eW)); - socket->Append(Propagate->GetVel(eNorth)); - socket->Append(Propagate->GetVel(eEast)); - socket->Append(Propagate->GetVel(eDown)); - } - if (SubSystems & ssForces) { - socket->Append(Aerodynamics->GetvFw()(eDrag)); - socket->Append(Aerodynamics->GetvFw()(eSide)); - socket->Append(Aerodynamics->GetvFw()(eLift)); - socket->Append(Aerodynamics->GetLoD()); - socket->Append(Aircraft->GetForces(eX)); - socket->Append(Aircraft->GetForces(eY)); - socket->Append(Aircraft->GetForces(eZ)); - } - if (SubSystems & ssMoments) { - socket->Append(Aircraft->GetMoments(eL)); - socket->Append(Aircraft->GetMoments(eM)); - socket->Append(Aircraft->GetMoments(eN)); - } - if (SubSystems & ssAtmosphere) { - socket->Append(Atmosphere->GetDensity()); - socket->Append(Atmosphere->GetPressureSL()); - socket->Append(Atmosphere->GetPressure()); - socket->Append(Winds->GetTurbMagnitude()); - socket->Append(Winds->GetTurbDirection().Dump(",")); - socket->Append(Winds->GetTotalWindNED().Dump(",")); - } - if (SubSystems & ssMassProps) { - socket->Append(MassBalance->GetJ()(1,1)); - socket->Append(MassBalance->GetJ()(1,2)); - socket->Append(MassBalance->GetJ()(1,3)); - socket->Append(MassBalance->GetJ()(2,1)); - socket->Append(MassBalance->GetJ()(2,2)); - socket->Append(MassBalance->GetJ()(2,3)); - socket->Append(MassBalance->GetJ()(3,1)); - socket->Append(MassBalance->GetJ()(3,2)); - socket->Append(MassBalance->GetJ()(3,3)); - socket->Append(MassBalance->GetMass()); - socket->Append(MassBalance->GetXYZcg()(eX)); - socket->Append(MassBalance->GetXYZcg()(eY)); - socket->Append(MassBalance->GetXYZcg()(eZ)); - } - if (SubSystems & ssPropagate) { - socket->Append(Propagate->GetAltitudeASL()); - socket->Append(radtodeg*Propagate->GetEuler(ePhi)); - socket->Append(radtodeg*Propagate->GetEuler(eTht)); - socket->Append(radtodeg*Propagate->GetEuler(ePsi)); - socket->Append(Auxiliary->Getalpha(inDegrees)); - socket->Append(Auxiliary->Getbeta(inDegrees)); - socket->Append(Propagate->GetLocation().GetLatitudeDeg()); - socket->Append(Propagate->GetLocation().GetLongitudeDeg()); - } - if (SubSystems & ssAeroFunctions) { - scratch = Aerodynamics->GetAeroFunctionValues(","); - if (scratch.length() != 0) socket->Append(scratch); - } - if (SubSystems & ssFCS) { - scratch = FCS->GetComponentValues(","); - if (scratch.length() != 0) socket->Append(scratch); - } - if (SubSystems & ssGroundReactions) { - socket->Append(GroundReactions->GetGroundReactionValues(",")); - } - if (SubSystems & ssPropulsion && Propulsion->GetNumEngines() > 0) { - socket->Append(Propulsion->GetPropulsionValues(",")); - } + ResetParser(); - for (unsigned int i=0;iAppend(OutputProperties[i]->getDoubleValue()); - } + if (!result) + cerr << endl << "Aircraft output element has problems in file " << fname << endl; - socket->Send(); + return result; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGOutput::SocketStatusOutput(const string& out_str) +bool FGOutput::Load(int subSystems, std::string protocol, std::string type, + std::string port, std::string name, double outRate, + std::vector & outputProperties) { - string asciiData; + unsigned int idx = OutputTypes.size(); + FGOutputType* Output = 0; - if (socket == NULL) return; + if (debug_lvl > 0) cout << endl << " Output data set: " << idx << " "; - socket->Clear(); - asciiData = string("") + out_str; - socket->Append(asciiData.c_str()); - socket->Send(); -} + if (type == "CSV") { + FGOutputTextFile* OutputTextFile = new FGOutputTextFile(FDMExec); + OutputTextFile->SetDelimiter(","); + Output = OutputTextFile; + } else if (type == "TABULAR") { + FGOutputTextFile* OutputTextFile = new FGOutputTextFile(FDMExec); + OutputTextFile->SetDelimiter("\t"); + Output = OutputTextFile; + } else if (type == "SOCKET") { + Output = new FGOutputSocket(FDMExec); + name += ":" + port + "/" + protocol; + } else if (type == "FLIGHTGEAR") { + Output = new FGOutputFG(FDMExec); + name += ":" + port + "/" + protocol; + } else if (type == "TERMINAL") { + // Not done yet + } else if (type != string("NONE")) { + cerr << "Unknown type of output specified in config file" << endl; + } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + if (!Output) return false; -bool FGOutput::Load(int subSystems, std::string protocol, std::string type, std::string port, std::string name, double outRate, std::vector & outputProperties) -{ - SetType(type); - SetRate(outRate); - SubSystems = subSystems; - OutputProperties = outputProperties; - - if (((Type == otCSV) || (Type == otTab)) && (name != "cout") && (name !="COUT")) - name = FDMExec->GetRootDir() + name; - - if (!port.empty() && (Type == otSocket || Type == otFlightGear)) { - SetProtocol(protocol); - socket = new FGfdmSocket(name, atoi(port.c_str()), Protocol); - } else { - BaseFilename = Filename = name; - } + Output->SetIdx(idx); + Output->SetOutputName(name); + Output->SetRate(outRate); + Output->SetSubSystems(subSystems); + Output->SetOutputProperties(outputProperties); + + OutputTypes.push_back(Output); Debug(2); return true; } -bool FGOutput::Load(Element* element) -{ - int subSystems = 0; - Element *property_element; - std::vector outputProperties; - - if (!DirectivesFile.empty()) { // A directives filename from the command line overrides - output_file_name = DirectivesFile; // one found in the config file. - document = LoadXMLDocument(output_file_name); - } else if (!element->GetAttributeValue("file").empty()) { - output_file_name = FDMExec->GetRootDir() + element->GetAttributeValue("file"); - document = LoadXMLDocument(output_file_name); - } else { - document = element; - } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +bool FGOutput::Load(Element* document) +{ if (!document) return false; + unsigned int idx = OutputTypes.size(); string type = document->GetAttributeValue("type"); - string name = document->GetAttributeValue("name"); - string port = document->GetAttributeValue("port"); - string protocol = document->GetAttributeValue("protocol"); - if (!document->GetAttributeValue("rate").empty()) { - rate = document->GetAttributeValueAsNumber("rate"); - } else { - rate = 1; - } + FGOutputType* Output = 0; - if (document->FindElementValue("simulation") == string("ON")) - subSystems += ssSimulation; - if (document->FindElementValue("aerosurfaces") == string("ON")) - subSystems += ssAerosurfaces; - if (document->FindElementValue("rates") == string("ON")) - subSystems += ssRates; - if (document->FindElementValue("velocities") == string("ON")) - subSystems += ssVelocities; - if (document->FindElementValue("forces") == string("ON")) - subSystems += ssForces; - if (document->FindElementValue("moments") == string("ON")) - subSystems += ssMoments; - if (document->FindElementValue("atmosphere") == string("ON")) - subSystems += ssAtmosphere; - if (document->FindElementValue("massprops") == string("ON")) - subSystems += ssMassProps; - if (document->FindElementValue("position") == string("ON")) - subSystems += ssPropagate; - if (document->FindElementValue("coefficients") == string("ON") || document->FindElementValue("aerodynamics") == string("ON")) - subSystems += ssAeroFunctions; - if (document->FindElementValue("ground_reactions") == string("ON")) - subSystems += ssGroundReactions; - if (document->FindElementValue("fcs") == string("ON")) - subSystems += ssFCS; - if (document->FindElementValue("propulsion") == string("ON")) - subSystems += ssPropulsion; - property_element = document->FindElement("property"); - while (property_element) { - string property_str = property_element->GetDataLine(); - FGPropertyManager* node = PropertyManager->GetNode(property_str); - if (!node) { - cerr << fgred << highint << endl << " No property by the name " - << property_str << " has been defined. This property will " << endl - << " not be logged. You should check your configuration file." - << reset << endl; - } else { - outputProperties.push_back(node); - } - property_element = document->FindNextElement("property"); - } + if (debug_lvl > 0) cout << endl << " Output data set: " << idx << " "; - return Load(subSystems, protocol, type, port, name, rate, outputProperties); -} + if (type == "CSV") { + Output = new FGOutputTextFile(FDMExec); + } else if (type == "TABULAR") { + Output = new FGOutputTextFile(FDMExec); + } else if (type == "SOCKET") { + Output = new FGOutputSocket(FDMExec); + } else if (type == "FLIGHTGEAR") { + Output = new FGOutputFG(FDMExec); + } else if (type == "TERMINAL") { + // Not done yet + } else if (type != string("NONE")) { + cerr << "Unknown type of output specified in config file" << endl; + } + if (!Output) return false; + Output->SetIdx(idx); + Output->Load(document); -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + OutputTypes.push_back(Output); -void FGOutput::SetRate(double rtHz) -{ - rtHz = rtHz>1000?1000:(rtHz<0?0:rtHz); - if (rtHz > 0) { - rate = (int)(0.5 + 1.0/(FDMExec->GetDeltaT()*rtHz)); - Enable(); - } else { - rate = 1; - Disable(); - } + Debug(2); + return true; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -1120,43 +315,6 @@ void FGOutput::Debug(int from) } if (from == 2) { - if (output_file_name.empty()) - cout << " " << "Output parameters read inline" << endl; - else - cout << " Output parameters read from file: " << output_file_name << endl; - - if (Filename == "cout" || Filename == "COUT") { - scratch = " Log output goes to screen console"; - } else if (!Filename.empty()) { - scratch = " Log output goes to file: " + Filename; - } - switch (Type) { - case otCSV: - cout << scratch << " in CSV format output at rate " << 1/(FDMExec->GetDeltaT()*rate) << " Hz" << endl; - break; - case otNone: - default: - cout << " No log output" << endl; - break; - } - - if (SubSystems & ssSimulation) cout << " Simulation parameters logged" << endl; - if (SubSystems & ssAerosurfaces) cout << " Aerosurface parameters logged" << endl; - if (SubSystems & ssRates) cout << " Rate parameters logged" << endl; - if (SubSystems & ssVelocities) cout << " Velocity parameters logged" << endl; - if (SubSystems & ssForces) cout << " Force parameters logged" << endl; - if (SubSystems & ssMoments) cout << " Moments parameters logged" << endl; - if (SubSystems & ssAtmosphere) cout << " Atmosphere parameters logged" << endl; - if (SubSystems & ssMassProps) cout << " Mass parameters logged" << endl; - if (SubSystems & ssAeroFunctions) cout << " Coefficient parameters logged" << endl; - if (SubSystems & ssPropagate) cout << " Propagate parameters logged" << endl; - if (SubSystems & ssGroundReactions) cout << " Ground parameters logged" << endl; - if (SubSystems & ssFCS) cout << " FCS parameters logged" << endl; - if (SubSystems & ssPropulsion) cout << " Propulsion parameters logged" << endl; - if (OutputProperties.size() > 0) cout << " Properties logged:" << endl; - for (unsigned int i=0;iGetName() << endl; - } } } if (debug_lvl & 2 ) { // Instantiation/Destruction notification diff --git a/src/FDM/JSBSim/models/FGOutput.h b/src/FDM/JSBSim/models/FGOutput.h index fe8ac1137..ce2674010 100644 --- a/src/FDM/JSBSim/models/FGOutput.h +++ b/src/FDM/JSBSim/models/FGOutput.h @@ -27,6 +27,7 @@ HISTORY -------------------------------------------------------------------------------- 12/02/98 JSB Created 11/09/07 HDW Added FlightGear Socket Interface +09/10/11 BC Broke Down the Code in Several Classes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY @@ -40,18 +41,14 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGModel.h" - -#include - +#include "input_output/FGOutputType.h" #include "input_output/FGXMLFileRead.h" -#include "input_output/net_fdm.hxx" -#include "input_output/FGfdmSocket.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_OUTPUT "$Id: FGOutput.h,v 1.25 2012/02/07 23:15:37 bcoconni Exp $" +#define ID_OUTPUT "$Id: FGOutput.h,v 1.26 2012/09/05 21:49:19 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -59,8 +56,6 @@ FORWARD DECLARATIONS namespace JSBSim { -class FGfdmSocket; - /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DOCUMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -124,7 +119,10 @@ CLASS DOCUMENTATION propulsion ON|OFF NOTE that Time is always output with the data. - @version $Id: FGOutput.h,v 1.25 2012/02/07 23:15:37 bcoconni Exp $ + + The class FGOutput is the manager of the outputs requested by the user. It + manages a list of instances derived from the abstract class FGOutputType. + @version $Id: FGOutput.h,v 1.26 2012/09/05 21:49:19 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -137,72 +135,96 @@ public: FGOutput(FGFDMExec*); ~FGOutput(); + /** Initializes the instance. This method is called by FGFDMExec::RunIC(). + This is were the initialization of all classes derived from FGOutputType + takes place. It is important that this method is not called prior + to FGFDMExec::RunIC() so that the initialization process can be executed + properly. + @result true if the execution succeeded. */ bool InitModel(void); - /** Runs the Output model; called by the Executive - Can pass in a value indicating if the executive is directing the simulation to Hold. - @param Holding if true, the executive has been directed to hold the sim from - advancing time. Some models may ignore this flag, such as the Input - model, which may need to be active to listen on a socket for the - "Resume" command to be given. + /** Runs the Output model; called by the Executive. + Can pass in a value indicating if the executive is directing the + simulation to Hold. + @param Holding if true, the executive has been directed to hold the sim + from advancing time. Some models may ignore this flag, such + as the Input model, which may need to be active to listen + on a socket for the "Resume" command to be given. @return false if no error */ bool Run(bool Holding); - + /** Makes all the output instances to generate their ouput. This method does + not check that the time step at which the output is requested is + consistent with the output rate RATE_IN_HZ. Although Print is not a + relevant name for outputs like SOCKET or FLIGHGEAR, it has been kept for + backward compatibility. */ void Print(void); - void DelimitedOutput(const std::string&); - void SocketOutput(void); - void FlightGearSocketOutput(void); - void SocketStatusOutput(const std::string&); - void SocketDataFill(FGNetFDM* net); - - void SetType(const std::string& type); - void SetProtocol(const std::string& protocol); - void SetPort(const std::string& port); - void SetStartNewFile(bool tt) {StartNewFile = tt;} - void SetSubsystems(int tt) {SubSystems = tt;} - void SetOutputFileName(const std::string& fname) {Filename = fname;} - void SetDirectivesFile(const std::string& fname) {DirectivesFile = fname;} - void SetRate(double rt); - void Enable(void) { enabled = true; } - void Disable(void) { enabled = false; } - bool Toggle(void) {enabled = !enabled; return enabled;} - + /** Force an output instance to generate its output. The code executed is + basically the same than the code of the method Print() except that the + ouput is limited to the instance identified by the parameter of the + method. + @param idx ID of the instance that will generate its ouput */ + void ForceOutput(int idx); + /** Reset the output prior to a restart of the simulation. This method should + be called when the simulation is restarted with, for example, new initial + conditions. When this method is executed the output instances can take + special actions such as closing the current output file and open a new + one with a different name. */ + void SetStartNewOutput(void); + /** Overwrites the name identifier under which the output will be logged. + This method is taken into account if it is called between Load() and + FGFDMExec::RunIC() otherwise it is ignored until the next call to + SetStartNewOutput(). + @param idx ID of the instance which name identifier will be changed + @param name new name + @result false if the instance does not exists. */ + bool SetOutputName(unsigned int idx, const std::string& name); + /** Adds a new output instance to the Output Manager. The definition of the + new output instance is read from a file. + @param fname the name of the file from which the ouput directives should + be read. + @return true if the execution succeeded. */ + bool SetDirectivesFile(const std::string& fname); + /// Enables the output generation for all output instances. + void Enable(void); + /// Disables the output generation for all output instances. + void Disable(void); + /** Toggles the output generation for an ouput instances. + @param idx ID of the output instance which output generation will be + toggled. + @result false if the instance does not exist otherwise returns the status + of the output generation (i.e. true if the output has been + enabled, false if the output has been disabled) */ + bool Toggle(int idx); + /** Modifies the output rate for all output instances. + @param rate new output rate in Hz */ + void SetRate(double rate); + /** Load the output directives and adds a new output instance to the Output + Manager list. + @param el XMLElement that is pointing to the output directives + @result true if the execution succeeded. */ bool Load(Element* el); - bool Load(int subSystems, std::string protocol, std::string type, std::string port, - std::string name, double outRate, - std::vector & outputProperties); - string GetOutputFileName(void) const {return Filename;} - - /// Subsystem types for specifying which will be output in the FDM data logging - enum eSubSystems { - /** Subsystem: Simulation (= 1) */ ssSimulation = 1, - /** Subsystem: Aerosurfaces (= 2) */ ssAerosurfaces = 2, - /** Subsystem: Body rates (= 4) */ ssRates = 4, - /** Subsystem: Velocities (= 8) */ ssVelocities = 8, - /** Subsystem: Forces (= 16) */ ssForces = 16, - /** Subsystem: Moments (= 32) */ ssMoments = 32, - /** Subsystem: Atmosphere (= 64) */ ssAtmosphere = 64, - /** Subsystem: Mass Properties (= 128) */ ssMassProps = 128, - /** Subsystem: Coefficients (= 256) */ ssAeroFunctions = 256, - /** Subsystem: Propagate (= 512) */ ssPropagate = 512, - /** Subsystem: Ground Reactions (= 1024) */ ssGroundReactions = 1024, - /** Subsystem: FCS (= 2048) */ ssFCS = 2048, - /** Subsystem: Propulsion (= 4096) */ ssPropulsion = 4096 - } subsystems; - - FGNetFDM fgSockBuf; + /** Load the output directives and adds a new output instance to the Output + Manager list. Unlike the Load() method, the new output instance is not + generated from output directives read in a XML file but from a list of + parameters. + @param subSystems bitfield that describes the activated subsystems + @param protocol network protocol for outputs directed to sockets + @param type type of output + @param port port to which the socket will be directed + @param name file name to which the output will be directed + @param outRate output rate in Hz + @param outputProperties list of properties that should be output + @result true if the execution succeeded. */ + bool Load(int subSystems, std::string protocol, std::string type, + std::string port, std::string name, double outRate, + std::vector & outputProperties); + /** Get the name identifier to which the output will be directed. + @param idx ID of the output instance from which the name identifier must + be obtained + @result the name identifier.*/ + string GetOutputName(unsigned int idx) const; private: - enum {otNone, otCSV, otTab, otSocket, otTerminal, otFlightGear, otUnknown} Type; - FGfdmSocket::ProtocolType Protocol; - bool sFirstPass, dFirstPass, enabled; - int SubSystems; - int runID_postfix; - bool StartNewFile; - std::string output_file_name, delimeter, BaseFilename, Filename, DirectivesFile; - std::string Port; - std::ofstream datafile; - FGfdmSocket* socket; - std::vector OutputProperties; + vector OutputTypes; void Debug(int from); }; diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 3c77bca28..a2181e56e 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -77,7 +77,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.105 2012/03/26 21:26:11 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.111 2013/01/19 13:49:37 bcoconni Exp $"; static const char *IdHdr = ID_PROPAGATE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -91,8 +91,6 @@ FGPropagate::FGPropagate(FGFDMExec* fdmex) Debug(0); Name = "FGPropagate"; - vInertialVelocity.InitMatrix(); - /// These define the indices use to select the various integrators. // eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, eAdamsBashforth3, eAdamsBashforth4}; @@ -125,8 +123,6 @@ bool FGPropagate::InitModel(void) VState.vLocation.SetEllipse(in.SemiMajor, in.SemiMinor); VState.vLocation.SetAltitudeAGL(4.0, FDMExec->GetSimTime()); - vInertialVelocity.InitMatrix(); - VState.dqPQRidot.resize(4, FGColumnVector3(0.0,0.0,0.0)); VState.dqUVWidot.resize(4, FGColumnVector3(0.0,0.0,0.0)); VState.dqInertialVelocity.resize(4, FGColumnVector3(0.0,0.0,0.0)); @@ -442,11 +438,14 @@ void FGPropagate::UpdateBodyMatrices(void) Tb2l = Tl2b.Transposed(); // body to local frame transform Tec2b = Ti2b * Tec2i; // ECEF to body frame transform Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform + + Qec2b = Tec2b.GetQuaternion(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) { +void FGPropagate::SetInertialOrientation(const FGQuaternion& Qi) +{ VState.qAttitudeECI = Qi; VState.qAttitudeECI.Normalize(); UpdateBodyMatrices(); @@ -593,11 +592,44 @@ void FGPropagate::DumpState(void) cout << " ECEF: " << (VState.vPQR*radtodeg).Dump(", ") << " (p,q,r in deg/s)" << endl; } +//****************************************************************************** + +void FGPropagate::WriteStateFile(int num) +{ + string filename = FDMExec->GetFullAircraftPath(); + + if (filename.empty()) + filename = "initfile.xml"; + else + filename.append("/initfile.xml"); + + ofstream outfile(filename.c_str()); + + if (outfile.is_open()) { + outfile << "" << endl; + outfile << "" << endl; + outfile << " " << VState.vUVW(eU) << " " << endl; + outfile << " " << VState.vUVW(eV) << " " << endl; + outfile << " " << VState.vUVW(eW) << " " << endl; + outfile << " " << VState.qAttitudeLocal.GetEuler(ePhi)*radtodeg << " " << endl; + outfile << " " << VState.qAttitudeLocal.GetEuler(eTht)*radtodeg << " " << endl; + outfile << " " << VState.qAttitudeLocal.GetEuler(ePsi)*radtodeg << " " << endl; + outfile << " " << VState.vLocation.GetLongitudeDeg() << " " << endl; + outfile << " " << VState.vLocation.GetLatitudeDeg() << " " << endl; + outfile << " " << GetDistanceAGL() << " " << endl; + outfile << "" << endl; + outfile.close(); + } else { + cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl; + } +} + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::bind(void) { typedef double (FGPropagate::*PMF)(int) const; + typedef int (FGPropagate::*iPMF)(void) const; PropertyManager->Tie("velocities/h-dot-fps", this, &FGPropagate::Gethdot); @@ -649,6 +681,8 @@ void FGPropagate::bind(void) PropertyManager->Tie("simulation/integrator/rate/translational", (int*)&integrator_translational_rate); PropertyManager->Tie("simulation/integrator/position/rotational", (int*)&integrator_rotational_position); PropertyManager->Tie("simulation/integrator/position/translational", (int*)&integrator_translational_position); + + PropertyManager->Tie("simulation/write-state-file", this, (iPMF)0, &FGPropagate::WriteStateFile); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index 1a683b218..20841c10d 100644 --- a/src/FDM/JSBSim/models/FGPropagate.h +++ b/src/FDM/JSBSim/models/FGPropagate.h @@ -49,7 +49,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.69 2012/04/29 13:27:51 bcoconni Exp $" +#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.74 2013/01/19 13:49:37 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -93,7 +93,7 @@ CLASS DOCUMENTATION @endcode @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier - @version $Id: FGPropagate.h,v 1.69 2012/04/29 13:27:51 bcoconni Exp $ + @version $Id: FGPropagate.h,v 1.74 2013/01/19 13:49:37 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -472,9 +472,15 @@ public: void SetInertialVelocity(const FGColumnVector3& Vi); void SetInertialRates(const FGColumnVector3& vRates); + /** Returns the quaternion that goes from Local to Body. */ const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; } + + /** Returns the quaternion that goes from ECI to Body. */ const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; } + /** Returns the quaternion that goes from ECEF to Body. */ + const FGQuaternion GetQuaternionECEF(void) const { return Qec2b; } + void SetPQR(unsigned int i, double val) { VState.vPQR(i) = val; VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet; @@ -542,7 +548,6 @@ public: FGQuaternion vQtrndot; FGColumnVector3 vUVWidot; FGColumnVector3 vOmegaPlanet; - double RefRadius; double SemiMajor; double SemiMinor; double DeltaT; @@ -555,7 +560,6 @@ private: struct VehicleState VState; FGColumnVector3 vVel; - FGColumnVector3 vInertialVelocity; FGColumnVector3 vLocation; FGMatrix33 Tec2b; FGMatrix33 Tb2ec; @@ -570,6 +574,8 @@ private: FGMatrix33 Ti2l; FGMatrix33 Tl2i; + FGQuaternion Qec2b; + double VehicleRadius; FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity; @@ -597,6 +603,7 @@ private: void UpdateBodyMatrices(void); void UpdateVehicleState(void); + void WriteStateFile(int num); void bind(void); void Debug(int from); }; diff --git a/src/FDM/JSBSim/models/FGPropulsion.cpp b/src/FDM/JSBSim/models/FGPropulsion.cpp index 85453af7e..731e06812 100644 --- a/src/FDM/JSBSim/models/FGPropulsion.cpp +++ b/src/FDM/JSBSim/models/FGPropulsion.cpp @@ -66,7 +66,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropulsion.cpp,v 1.62 2012/07/19 03:50:19 jberndt Exp $"; +static const char *IdSrc = "$Id: FGPropulsion.cpp,v 1.69 2012/12/12 06:19:57 jberndt Exp $"; static const char *IdHdr = ID_PROPULSION; extern short debug_lvl; @@ -371,12 +371,25 @@ void FGPropulsion::InitRunning(int n) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGPropulsion::Load(Element* el) +bool FGPropulsion::Load(Element* elem) { string type, engine_filename; + string separator = "/"; + Element *el=0; + FGXMLParse main_file_parser; Debug(2); + string fname="", file=""; + fname = elem->GetAttributeValue("file"); + if (!fname.empty()) { + file = FDMExec->GetFullAircraftPath() + separator + fname; + el = LoadXMLDocument(file, main_file_parser); + if (el == 0L) return false; + } else { + el = elem; + } + FGModel::Load(el); // Perform base class Load. // Process tank definitions first to establish the number of fuel tanks @@ -572,6 +585,9 @@ string FGPropulsion::GetPropulsionTankReport() { string out=""; stringstream outstream; + + /*const FGMatrix33& mTkI =*/ CalculateTankInertias(); + for (unsigned int i=0; iGetContents() << setw(8) << tank->GetXYZ(eX) << setw(8) << tank->GetXYZ(eY) << setw(8) << tank->GetXYZ(eZ) - << setw(12) << "*" << setw(12) << "*" - << setw(12) << "*" << endl; + << setw(12) << tank->GetIxx() << setw(12) << tank->GetIyy() + << setw(12) << tank->GetIzz() << endl; } return outstream.str(); } @@ -628,10 +644,8 @@ const FGMatrix33& FGPropulsion::CalculateTankInertias(void) tankJ = FGMatrix33(); for (unsigned int i=0; iGetXYZ(); - tankJ += FDMExec->GetMassBalance()->GetPointmassInertia( lbtoslug * Tanks[i]->GetContents(), - vTankStructVec); + Tanks[i]->GetXYZ()); tankJ(1,1) += Tanks[i]->GetIxx(); tankJ(2,2) += Tanks[i]->GetIyy(); tankJ(3,3) += Tanks[i]->GetIzz(); diff --git a/src/FDM/JSBSim/models/atmosphere/FGWinds.cpp b/src/FDM/JSBSim/models/atmosphere/FGWinds.cpp index cb933eee3..ef732c514 100644 --- a/src/FDM/JSBSim/models/atmosphere/FGWinds.cpp +++ b/src/FDM/JSBSim/models/atmosphere/FGWinds.cpp @@ -51,7 +51,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGWinds.cpp,v 1.7 2011/12/11 17:03:05 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGWinds.cpp,v 1.8 2012/12/02 12:59:19 bcoconni Exp $"; static const char *IdHdr = ID_WINDS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -436,7 +436,7 @@ void FGWinds::NumberOfUpDownburstCells(int num) for (unsigned int i=0; i= 0) { - for (unsigned int i=0; iringLatitude, UpDownBurstCells[i]->ringLongitude); + /*double d =*/ DistanceFromRingCenter(UpDownBurstCells[i]->ringLatitude, UpDownBurstCells[i]->ringLongitude); } } diff --git a/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp old mode 100644 new mode 100755 index 028509deb..9f321a220 --- a/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGAccelerometer.cpp @@ -50,7 +50,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.9 2011/07/17 13:51:23 jberndt Exp $"; +static const char *IdSrc = "$Id: FGAccelerometer.cpp,v 1.11 2012/11/17 18:03:19 jberndt Exp $"; static const char *IdHdr = ID_ACCELEROMETER; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -90,11 +90,9 @@ bool FGAccelerometer::Run(void ) vRadius = MassBalance->StructuralToBody(vLocation); - //gravitational forces - vAccel = Propagate->GetTl2b() * FGColumnVector3(0, 0, Inertial->gravity()); - //aircraft forces - vAccel += (Accelerations->GetUVWdot() + vAccel = (Accelerations->GetBodyAccel() + + Propagate->GetTi2b() * Accelerations->GetGravAccel() + Accelerations->GetPQRdot() * vRadius + Propagate->GetPQR() * (Propagate->GetPQR() * vRadius)); @@ -105,6 +103,8 @@ bool FGAccelerometer::Run(void ) ProcessSensorSignal(); + if (IsOutput) SetOutput(); + return true; } diff --git a/src/FDM/JSBSim/models/flight_control/FGActuator.cpp b/src/FDM/JSBSim/models/flight_control/FGActuator.cpp index 8f65597a1..f739a76a7 100644 --- a/src/FDM/JSBSim/models/flight_control/FGActuator.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGActuator.cpp @@ -43,7 +43,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGActuator.cpp,v 1.23 2012/04/08 15:04:41 jberndt Exp $"; +static const char *IdSrc = "$Id: FGActuator.cpp,v 1.25 2013/01/12 19:24:05 jberndt Exp $"; static const char *IdHdr = ID_ACTUATOR; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -62,7 +62,9 @@ FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, eleme PreviousRateLimOutput = 0.0; PreviousLagInput = PreviousLagOutput = 0.0; bias = lag = hysteresis_width = deadband_width = 0.0; - rate_limit = 0.0; // no limit + rate_limited = false; + rate_limit = rate_limit_incr = rate_limit_decr = 0.0; // no limit + rate_limit_incr_prop = rate_limit_decr_prop = 0; fail_zero = fail_hardover = fail_stuck = false; ca = cb = 0.0; initialized = 0; @@ -74,9 +76,42 @@ FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, eleme if ( element->FindElement("hysteresis_width") ) { hysteresis_width = element->FindElementValueAsNumber("hysteresis_width"); } - if ( element->FindElement("rate_limit") ) { + + // There can be a single rate limit specified, or increasing and + // decreasing rate limits specified, and rate limits can be numeric, or + // a property. + Element* ratelim_el = element->FindElement("rate_limit"); + while ( ratelim_el ) { + rate_limited = true; + FGPropertyManager* rate_limit_prop=0; + + string rate_limit_str = ratelim_el->GetDataLine(); + trim(rate_limit_str); + if (is_number(rate_limit_str)) { rate_limit = fabs(element->FindElementValueAsNumber("rate_limit")); + } else { + if (rate_limit_str[0] == '-') rate_limit_str.erase(0,1); + rate_limit_prop = PropertyManager->GetNode(rate_limit_str, true); + if (rate_limit_prop == 0) + std::cerr << "No such property, " << rate_limit_str << " for rate limiting" << std::endl; + } + + if (ratelim_el->HasAttribute("sense")) { + string sense = ratelim_el->GetAttributeValue("sense"); + if (sense.substr(0,4) == "incr") { + if (rate_limit_prop != 0) rate_limit_incr_prop = rate_limit_prop; + else rate_limit_incr = rate_limit; + } else if (sense.substr(0,4) == "decr") { + if (rate_limit_prop != 0) rate_limit_decr_prop = rate_limit_prop; + else rate_limit_decr = -rate_limit; } + } else { + rate_limit_incr = rate_limit; + rate_limit_decr = -rate_limit; + } + ratelim_el = element->FindNextElement("rate_limit"); + } + if ( element->FindElement("bias") ) { bias = element->FindElementValueAsNumber("bias"); } @@ -136,8 +171,8 @@ bool FGActuator::Run(void ) if (clip) { saturated = false; - if (Output >= clipmax) saturated = true; - else if (Output <= clipmin) saturated = true; + if (Output >= clipmax && clipmax != 0) saturated = true; + else if (Output <= clipmin && clipmin != 0) saturated = true; } if (IsOutput) SetOutput(); @@ -196,9 +231,12 @@ void FGActuator::RateLimit(void) double input = Output; if ( initialized ) { double delta = input - PreviousRateLimOutput; - if (fabs(delta) > dt * rate_limit) { - double signed_rate_limit = delta > 0.0 ? rate_limit : -rate_limit; - Output = PreviousRateLimOutput + signed_rate_limit * dt; + if (rate_limit_incr_prop != 0) rate_limit_incr = rate_limit_incr_prop->getDoubleValue(); + if (rate_limit_decr_prop != 0) rate_limit_decr = rate_limit_decr_prop->getDoubleValue(); + if (delta > dt * rate_limit_incr) { + Output = PreviousRateLimOutput + rate_limit_incr * dt; + } else if (delta < dt * rate_limit_decr) { + Output = PreviousRateLimOutput + rate_limit_decr * dt; } } PreviousRateLimOutput = Output; @@ -276,7 +314,18 @@ void FGActuator::Debug(int from) cout << " OUTPUT: " << OutputNodes[i]->getName() << endl; } if (bias != 0.0) cout << " Bias: " << bias << endl; - if (rate_limit != 0) cout << " Rate limit: " << rate_limit << endl; + if (rate_limited) { + if (rate_limit_incr_prop != 0) { + cout << " Increasing rate limit: " << rate_limit_incr_prop->GetName() << endl; + } else { + cout << " Increasing rate limit: " << rate_limit_incr << endl; + } + if (rate_limit_decr_prop != 0) { + cout << " Decreasing rate limit: " << rate_limit_decr_prop->GetName() << endl; + } else { + cout << " Decreasing rate limit: " << rate_limit_decr << endl; + } + } if (lag != 0) cout << " Actuator lag: " << lag << endl; if (hysteresis_width != 0) cout << " Hysteresis width: " << hysteresis_width << endl; if (deadband_width != 0) cout << " Deadband width: " << deadband_width << endl; diff --git a/src/FDM/JSBSim/models/flight_control/FGActuator.h b/src/FDM/JSBSim/models/flight_control/FGActuator.h index 522fab8d1..650af2f9a 100644 --- a/src/FDM/JSBSim/models/flight_control/FGActuator.h +++ b/src/FDM/JSBSim/models/flight_control/FGActuator.h @@ -44,7 +44,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ACTUATOR "$Id: FGActuator.h,v 1.13 2012/04/08 15:04:41 jberndt Exp $" +#define ID_ACTUATOR "$Id: FGActuator.h,v 1.14 2013/01/12 19:24:05 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -75,13 +75,22 @@ CLASS DOCUMENTATION There are also several malfunctions that can be applied to the actuator by setting a property to true or false (or 1 or 0). + Rate limits can be specified either as a single number or property. If a + single is supplied (with no "sense" attribute) then the actuator + is rate limited at +/- the specified rate limit. If the element + is supplied with a "sense" attribute of either "incr[easing]" or + "decr[easing]" then the actuator is limited to the provided numeric or property + value) exactly as provided. + Syntax: @code {[-]property} number - number + [ {property name | value} ] + [ {property name | value} + {property name | value} ] number number number @@ -111,7 +120,7 @@ Example: @endcode @author Jon S. Berndt -@version $Revision: 1.13 $ +@version $Revision: 1.14 $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -147,7 +156,12 @@ public: private: double span; double bias; + bool rate_limited; double rate_limit; + double rate_limit_incr; + double rate_limit_decr; + FGPropertyManager* rate_limit_incr_prop; + FGPropertyManager* rate_limit_decr_prop; double hysteresis_width; double deadband_width; double lag; diff --git a/src/FDM/JSBSim/models/flight_control/FGDeadBand.cpp b/src/FDM/JSBSim/models/flight_control/FGDeadBand.cpp index 361afb66a..5507ecfd3 100644 --- a/src/FDM/JSBSim/models/flight_control/FGDeadBand.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGDeadBand.cpp @@ -46,7 +46,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGDeadBand.cpp,v 1.9 2009/10/24 22:59:30 jberndt Exp $"; +static const char *IdSrc = "$Id: FGDeadBand.cpp,v 1.12 2012/11/17 18:03:19 jberndt Exp $"; static const char *IdHdr = ID_DEADBAND; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -111,7 +111,6 @@ bool FGDeadBand::Run(void ) } Clip(); - if (IsOutput) SetOutput(); return true; diff --git a/src/FDM/JSBSim/models/flight_control/FGFCSFunction.cpp b/src/FDM/JSBSim/models/flight_control/FGFCSFunction.cpp old mode 100644 new mode 100755 index a9143f42a..28032d306 --- a/src/FDM/JSBSim/models/flight_control/FGFCSFunction.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGFCSFunction.cpp @@ -45,7 +45,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGFCSFunction.cpp,v 1.9 2009/10/24 22:59:30 jberndt Exp $"; +static const char *IdSrc = "$Id: FGFCSFunction.cpp,v 1.12 2012/11/17 18:03:19 jberndt Exp $"; static const char *IdHdr = ID_FCSFUNCTION; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -89,7 +89,6 @@ bool FGFCSFunction::Run(void ) } Clip(); - if (IsOutput) SetOutput(); return true; diff --git a/src/FDM/JSBSim/models/flight_control/FGGyro.cpp b/src/FDM/JSBSim/models/flight_control/FGGyro.cpp old mode 100644 new mode 100755 index 7ee6abe44..19d8de26c --- a/src/FDM/JSBSim/models/flight_control/FGGyro.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGGyro.cpp @@ -46,7 +46,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGGyro.cpp,v 1.6 2011/07/17 13:51:23 jberndt Exp $"; +static const char *IdSrc = "$Id: FGGyro.cpp,v 1.7 2012/11/17 18:03:19 jberndt Exp $"; static const char *IdHdr = ID_GYRO; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -81,6 +81,8 @@ bool FGGyro::Run(void ) ProcessSensorSignal(); + if (IsOutput) SetOutput(); + return true; } diff --git a/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp b/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp old mode 100644 new mode 100755 index 3c04e68fa..e0a54a7e2 --- a/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGMagnetometer.cpp @@ -47,7 +47,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGMagnetometer.cpp,v 1.5 2009/10/24 22:59:30 jberndt Exp $"; +static const char *IdSrc = "$Id: FGMagnetometer.cpp,v 1.6 2012/11/17 18:03:19 jberndt Exp $"; static const char *IdHdr = ID_MAGNETOMETER; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -135,6 +135,8 @@ bool FGMagnetometer::Run(void ) ProcessSensorSignal(); + if (IsOutput) SetOutput(); + return true; } diff --git a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp old mode 100644 new mode 100755 index bccf5187e..9aedc65b0 --- a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp @@ -46,7 +46,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGSensor.cpp,v 1.23 2011/08/18 12:42:17 jberndt Exp $"; +static const char *IdSrc = "$Id: FGSensor.cpp,v 1.24 2012/11/17 18:03:19 jberndt Exp $"; static const char *IdHdr = ID_SENSOR; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -143,6 +143,8 @@ bool FGSensor::Run(void) ProcessSensorSignal(); + if (IsOutput) SetOutput(); + return true; } diff --git a/src/FDM/JSBSim/models/flight_control/FGSwitch.cpp b/src/FDM/JSBSim/models/flight_control/FGSwitch.cpp index 45b0a9921..fa28935d9 100644 --- a/src/FDM/JSBSim/models/flight_control/FGSwitch.cpp +++ b/src/FDM/JSBSim/models/flight_control/FGSwitch.cpp @@ -63,13 +63,12 @@ INCLUDES #include "FGSwitch.h" #include -#include using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGSwitch.cpp,v 1.22 2011/06/17 12:12:19 jberndt Exp $"; +static const char *IdSrc = "$Id: FGSwitch.cpp,v 1.25 2012/12/02 12:59:19 bcoconni Exp $"; static const char *IdHdr = ID_SWITCH; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -78,77 +77,35 @@ CLASS IMPLEMENTATION FGSwitch::FGSwitch(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element) { - string value, logic; + string value; struct test *current_test; - Element *test_element, *condition_element; + Element *test_element; FGFCSComponent::bind(); // Bind() this component here in case it is used // in its own definition for a sample-and-hold - test_element = element->GetElement(); - while (test_element) { - if (test_element->GetName() == "default") { - current_test = new struct test; - current_test->Logic = eDefault; - tests.push_back(current_test); - } else if (test_element->GetName() == "test") { // here's a test - current_test = new struct test; - logic = test_element->GetAttributeValue("logic"); - if (logic == "OR") current_test->Logic = eOR; - else if (logic == "AND") current_test->Logic = eAND; - else if (logic.size() == 0) current_test->Logic = eAND; // default - else { // error - cerr << "Unrecognized LOGIC token " << logic << " in switch component: " << Name << endl; - } - for (unsigned int i=0; iGetNumDataLines(); i++) { - string input_data = test_element->GetDataLine(i); - if (input_data.size() <= 1) { - // Make sure there are no bad data lines that consist solely of whitespace - cerr << fgred << " Bad data line in switch component: " << Name << reset << endl; - continue; - } - current_test->conditions.push_back(new FGCondition(input_data, PropertyManager)); - } - - condition_element = test_element->GetElement(); // retrieve condition groups - while (condition_element) { - current_test->conditions.push_back(new FGCondition(condition_element, PropertyManager)); - condition_element = test_element->GetNextElement(); - } - - tests.push_back(current_test); - } + int num = element->GetNumElements("test"); + if (element->FindElement("default")) num++; + tests.reserve(num); + + test_element = element->FindElement("default"); + if (test_element) { + current_test = new struct test; + current_test->condition = 0; + value = test_element->GetAttributeValue("value"); + current_test->setTestValue(value, Name, PropertyManager); + current_test->Default = true; + tests.push_back(current_test); + } - string el_name = test_element->GetName(); - if ( el_name != "output" - && el_name != "description" - && el_name != "delay" ) - { - value = test_element->GetAttributeValue("value"); - if (value.empty()) { - cerr << "No VALUE supplied for switch component: " << Name << endl; - } else { - if (is_number(value)) { - current_test->OutputVal = atof(value.c_str()); - } else { - // "value" must be a property if execution passes to here. - if (value[0] == '-') { - current_test->sign = -1.0; - value.erase(0,1); - } else { - current_test->sign = 1.0; - } - FGPropertyManager *node = PropertyManager->GetNode(value, false); - if (node) { - current_test->OutputProp = new FGPropertyValue(node); - } else { - current_test->OutputProp = new FGPropertyValue(value, - PropertyManager); - } - } - } - } - test_element = element->GetNextElement(); + test_element = element->FindElement("test"); + while (test_element) { + current_test = new struct test; + current_test->condition = new FGCondition(test_element, PropertyManager); + value = test_element->GetAttributeValue("value"); + current_test->setTestValue(value, Name, PropertyManager); + tests.push_back(current_test); + test_element = element->FindNextElement("test"); } Debug(0); @@ -159,7 +116,7 @@ FGSwitch::FGSwitch(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element) FGSwitch::~FGSwitch() { for (unsigned int i=0; iconditions.size(); j++) delete tests[i]->conditions[j]; + delete tests[i]->condition; delete tests[i]->OutputProp; delete tests[i]; } @@ -175,20 +132,10 @@ bool FGSwitch::Run(void ) double default_output=0.0; for (unsigned int i=0; iLogic == eDefault) { + if (tests[i]->Default) { default_output = tests[i]->GetValue(); - } else if (tests[i]->Logic == eAND) { - pass = true; - for (unsigned int j=0; jconditions.size(); j++) { - if (!tests[i]->conditions[j]->Evaluate()) pass = false; - } - } else if (tests[i]->Logic == eOR) { - pass = false; - for (unsigned int j=0; jconditions.size(); j++) { - if (tests[i]->conditions[j]->Evaluate()) pass = true; - } } else { - cerr << "Invalid logic test" << endl; + pass = tests[i]->condition->Evaluate(); } if (pass) { @@ -229,50 +176,26 @@ void FGSwitch::Debug(int from) { string comp, scratch; string indent = " "; - bool first = false; + //bool first = false; if (debug_lvl <= 0) return; if (debug_lvl & 1) { // Standard console startup message output if (from == 0) { // Constructor for (unsigned int i=0; iLogic) { - case (elUndef): - comp = " UNSET "; - cerr << "Unset logic for test condition" << endl; - break; - case (eAND): - comp = " AND "; - break; - case (eOR): - comp=" OR "; - break; - case (eDefault): - scratch = " by default."; - break; - default: - comp = " UNKNOWN "; - cerr << "Unknown logic for test condition" << endl; - } - - if (tests[i]->OutputProp != 0L) - if (tests[i]->sign < 0) - cout << indent << "Switch VALUE is - " << tests[i]->OutputProp->GetName() << scratch << endl; - else - cout << indent << "Switch VALUE is " << tests[i]->OutputProp->GetName() << scratch << endl; - else - cout << indent << "Switch VALUE is " << tests[i]->OutputVal << scratch << endl; - - first = true; - for (unsigned int j=0; jconditions.size(); j++) { - if (!first) cout << indent << comp << " "; - else cout << indent << " "; - first = false; - tests[i]->conditions[j]->PrintCondition(); - cout << endl; + if (tests[i]->Default) { + if (tests[i]->OutputProp == 0) { + cout << " Switch default value is: " << tests[i]->OutputVal; + } else { + cout << " Switch default value is: " << tests[i]->OutputProp->GetName(); + } + } else { + if (tests[i]->OutputProp == 0) { + cout << " Switch takes test " << i << " value (" << tests[i]->OutputVal << ")" << endl; + } else { + cout << " Switch takes test " << i << " value (" << tests[i]->OutputProp->GetName() << ")" << endl; + } + tests[i]->condition->PrintCondition(" "); } cout << endl; } diff --git a/src/FDM/JSBSim/models/flight_control/FGSwitch.h b/src/FDM/JSBSim/models/flight_control/FGSwitch.h index cca141b0f..238e6776c 100644 --- a/src/FDM/JSBSim/models/flight_control/FGSwitch.h +++ b/src/FDM/JSBSim/models/flight_control/FGSwitch.h @@ -37,6 +37,8 @@ SENTRY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ +#include +#include #include "FGFCSComponent.h" #include "input_output/FGXMLElement.h" #include "math/FGCondition.h" @@ -46,7 +48,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_SWITCH "$Id: FGSwitch.h,v 1.14 2011/04/05 20:20:21 andgi Exp $" +#define ID_SWITCH "$Id: FGSwitch.h,v 1.15 2012/10/27 20:29:01 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -125,7 +127,7 @@ ap/attitude_hold takes the value 1), the value of the switch component will be whatever value fcs/roll-ap-error-summer is. @author Jon S. Berndt -@version $Id: FGSwitch.h,v 1.14 2011/04/05 20:20:21 andgi Exp $ +@version $Id: FGSwitch.h,v 1.15 2012/10/27 20:29:01 jberndt Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -148,14 +150,11 @@ public: @return true - always*/ bool Run(void); - enum eLogic {elUndef=0, eAND, eOR, eDefault}; - enum eComparison {ecUndef=0, eEQ, eNE, eGT, eGE, eLT, eLE}; - private: struct test { - vector conditions; - eLogic Logic; + FGCondition* condition; + bool Default; double OutputVal; FGPropertyValue *OutputProp; float sign; @@ -166,12 +165,36 @@ private: } test(void) { // constructor for the test structure - Logic = elUndef; + Default = false; OutputVal = 0.0; OutputProp = 0L; sign = 1.0; } + void setTestValue(string value, string Name, FGPropertyManager* propMan) { + if (value.empty()) { + std::cerr << "No VALUE supplied for switch component: " << Name << std::endl; + } else { + if (is_number(value)) { + OutputVal = atof(value.c_str()); + } else { + // "value" must be a property if execution passes to here. + if (value[0] == '-') { + sign = -1.0; + value.erase(0,1); + } else { + sign = 1.0; + } + FGPropertyManager *node = propMan->GetNode(value, false); + if (node) { + OutputProp = new FGPropertyValue(node); + } else { + OutputProp = new FGPropertyValue(value, propMan); + } + } + } + } + }; vector tests; diff --git a/src/FDM/JSBSim/models/propulsion/FGEngine.cpp b/src/FDM/JSBSim/models/propulsion/FGEngine.cpp index 7dbec50a4..e4a9cb77e 100644 --- a/src/FDM/JSBSim/models/propulsion/FGEngine.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGEngine.cpp @@ -53,7 +53,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGEngine.cpp,v 1.50 2012/03/17 20:46:29 jentron Exp $"; +static const char *IdSrc = "$Id: FGEngine.cpp,v 1.52 2013/01/12 19:24:45 jberndt Exp $"; static const char *IdHdr = ID_ENGINE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -89,7 +89,8 @@ FGEngine::FGEngine(FGFDMExec* exec, Element* engine_element, int engine_number, local_element = engine_element->GetParent()->FindElement("location"); if (local_element) location = local_element->FindElementTripletConvertTo("IN"); - else cerr << "No engine location found for this engine." << endl; +// else cerr << "No engine location found for this engine." << endl; +// Jon: The engine location is not important - the nozzle location is. local_element = engine_element->GetParent()->FindElement("orient"); if (local_element) orientation = local_element->FindElementTripletConvertTo("RAD"); diff --git a/src/FDM/JSBSim/models/propulsion/FGEngine.h b/src/FDM/JSBSim/models/propulsion/FGEngine.h index 0ec53c884..de08492cc 100644 --- a/src/FDM/JSBSim/models/propulsion/FGEngine.h +++ b/src/FDM/JSBSim/models/propulsion/FGEngine.h @@ -55,7 +55,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ENGINE "$Id: FGEngine.h,v 1.35 2012/04/14 18:10:44 bcoconni Exp $" +#define ID_ENGINE "$Id: FGEngine.h,v 1.36 2012/07/29 12:04:09 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -113,7 +113,7 @@ CLASS DOCUMENTATION documentation for engine and thruster classes. @author Jon S. Berndt - @version $Id: FGEngine.h,v 1.35 2012/04/14 18:10:44 bcoconni Exp $ + @version $Id: FGEngine.h,v 1.36 2012/07/29 12:04:09 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -149,7 +149,6 @@ public: vector MixturePos; vector PropAdvance; vector PropFeather; - FGColumnVector3 vXYZcg; // CG coordinates expressed in the structural frame double TotalDeltaT; }; diff --git a/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp b/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp index 52635b7ef..9d341ef82 100644 --- a/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGPropeller.cpp @@ -45,7 +45,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropeller.cpp,v 1.44 2012/04/29 13:10:46 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGPropeller.cpp,v 1.46 2013/01/06 22:11:42 jentron Exp $"; static const char *IdHdr = ID_PROPELLER; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -245,8 +245,11 @@ double FGPropeller::Calculate(double EnginePower) // (see H. Glauert, "The Elements of Airfoil and Airscrew Theory", // 2nd edition, §16.3, pp. 219-221) - if ((Vel+2.0*Vinduced)*Vel < 0.0) - Vinduced = 0.0; // We cannot calculate the induced velocity so let's assume it is zero. + if ((Vel+2.0*Vinduced)*Vel < 0.0) { + // The momentum theory is no longer applicable so let's assume the induced + // saturates to -0.5*Vel so that the total velocity Vel+2*Vinduced equals 0. + Vinduced = -0.5*Vel; + } // P-factor is simulated by a shift of the acting location of the thrust. // The shift is a multiple of the angle between the propeller shaft axis @@ -355,17 +358,10 @@ double FGPropeller::GetPowerRequired(void) // Apply optional Mach effects from CP_MACH table if (CpMach) cPReq *= CpMach->GetValue(HelicalTipMach); - if (RPS > 0.1) { - PowerRequired = cPReq*RPS*RPS*RPS*D5*rho; - vTorque(eX) = -Sense*PowerRequired / (RPS*2.0*M_PI); - } else { - // For a stationary prop we have to estimate torque first. - double CL = (90.0 - Pitch) / 20.0; - if (CL > 1.5) CL = 1.5; - double BladeArea = Diameter * Diameter / 32.0 * numBlades; - vTorque(eX) = -Sense*BladeArea*Diameter*fabs(Vel)*Vel*rho*0.19*CL; - PowerRequired = Sense*(vTorque(eX))*0.2*M_PI; - } + double local_RPS = RPS < 0.01 ? 0.01 : RPS; + + PowerRequired = cPReq*local_RPS*RPS*local_RPS*D5*rho; + vTorque(eX) = -Sense*PowerRequired / (local_RPS*2.0*M_PI); return PowerRequired; } diff --git a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp index c5f3e5017..9693c3f0f 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp @@ -47,7 +47,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGRocket.cpp,v 1.27 2012/04/08 15:19:08 jberndt Exp $"; +static const char *IdSrc = "$Id: FGRocket.cpp,v 1.29 2013/01/12 21:11:59 jberndt Exp $"; static const char *IdHdr = ID_ROCKET; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -70,9 +70,10 @@ FGRocket::FGRocket(FGFDMExec* exec, Element *el, int engine_number, struct Input SLOxiFlowMax = SLFuelFlowMax = PropFlowMax = 0.0; MxR = 0.0; BuildupTime = 0.0; - It = 0.0; + It = ItVac = 0.0; ThrustVariation = 0.0; TotalIspVariation = 0.0; + VacThrust = 0.0; Flameout = false; // Defaults @@ -207,6 +208,7 @@ void FGRocket::Calculate(void) LoadThrusterInputs(); It += Thruster->Calculate(VacThrust) * in.TotalDeltaT; + ItVac += VacThrust * in.TotalDeltaT; RunPostFunctions(); } @@ -249,6 +251,7 @@ string FGRocket::GetEngineLabels(const string& delimiter) std::ostringstream buf; buf << Name << " Total Impulse (engine " << EngineNumber << " in psf)" << delimiter + << Name << " Total Vacuum Impulse (engine " << EngineNumber << " in psf)" << delimiter << Thruster->GetThrusterLabels(EngineNumber, delimiter); return buf.str(); @@ -260,7 +263,9 @@ string FGRocket::GetEngineValues(const string& delimiter) { std::ostringstream buf; - buf << It << delimiter << Thruster->GetThrusterValues(EngineNumber, delimiter); + buf << It << delimiter + << ItVac << delimiter + << Thruster->GetThrusterValues(EngineNumber, delimiter); return buf.str(); } diff --git a/src/FDM/JSBSim/models/propulsion/FGRocket.h b/src/FDM/JSBSim/models/propulsion/FGRocket.h index e7714aced..8b9a3334e 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRocket.h +++ b/src/FDM/JSBSim/models/propulsion/FGRocket.h @@ -47,7 +47,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ROCKET "$Id: FGRocket.h,v 1.18 2012/04/08 15:19:08 jberndt Exp $" +#define ID_ROCKET "$Id: FGRocket.h,v 1.19 2012/09/17 12:29:13 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -119,7 +119,7 @@ for the rocket engine to be throttle up to 1. At that time, the solid rocket fuel begins burning and thrust is provided. @author Jon S. Berndt - $Id: FGRocket.h,v 1.18 2012/04/08 15:19:08 jberndt Exp $ + $Id: FGRocket.h,v 1.19 2012/09/17 12:29:13 jberndt Exp $ @see FGNozzle, FGThruster, FGForce, @@ -158,9 +158,13 @@ public: double CalcOxidizerNeed(void); /** Gets the total impulse of the rocket. - @return The cumulative total impulse of the rocket up to this time.*/ + @return The cumulative actual total impulse of the rocket up to this time.*/ double GetTotalImpulse(void) const {return It;} + /** Gets the total impulse of the rocket. + @return The cumulative vacuum total impulse of the rocket up to this time.*/ + double GetVacTotalImpulse(void) const {return ItVac;} + /** Gets the flame-out status. The engine will "flame out" if the throttle is set below the minimum sustainable-thrust setting. @@ -214,7 +218,8 @@ private: void bindmodel(void); double Isp; // Vacuum Isp - double It; + double It; // Total actual Isp + double ItVac; // Total Vacuum Isp double MxR; // Mixture Ratio double BurnTime; double ThrustVariation; diff --git a/src/FDM/JSBSim/models/propulsion/FGTank.cpp b/src/FDM/JSBSim/models/propulsion/FGTank.cpp index 6c53d56f2..a6ac568aa 100644 --- a/src/FDM/JSBSim/models/propulsion/FGTank.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGTank.cpp @@ -47,7 +47,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGTank.cpp,v 1.33 2011/10/31 14:54:41 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGTank.cpp,v 1.36 2013/01/12 19:25:30 jberndt Exp $"; static const char *IdHdr = ID_TANK; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -64,6 +64,7 @@ FGTank::FGTank(FGFDMExec* exec, Element* el, int tank_number) Density = 6.6; InitialTemperature = Temperature = -9999.0; Ixx = Iyy = Izz = 0.0; + InertiaFactor = 1.0; Radius = Contents = Standpipe = Length = InnerRadius = 0.0; PreviousUsed = 0.0; ExternalFlow = 0.0; @@ -92,6 +93,8 @@ FGTank::FGTank(FGFDMExec* exec, Element* el, int tank_number) if (el->FindElement("radius")) Radius = el->FindElementValueAsNumberConvertTo("radius", "IN"); + if (el->FindElement("inertia_factor")) + InertiaFactor = el->FindElementValueAsNumber("inertia_factor"); if (el->FindElement("capacity")) Capacity = el->FindElementValueAsNumberConvertTo("capacity", "LBS"); if (el->FindElement("contents")) @@ -152,14 +155,18 @@ FGTank::FGTank(FGFDMExec* exec, Element* el, int tank_number) exit(-1); } Density = (Contents*lbtoslug)/Volume; // slugs/in^3 - CalculateInertias(); } + CalculateInertias(); + string property_name, base_property_name; base_property_name = CreateIndexedPropertyName("propulsion/tank", TankNumber); property_name = base_property_name + "/contents-lbs"; PropertyManager->Tie( property_name.c_str(), (FGTank*)this, &FGTank::GetContents, &FGTank::SetContents ); + property_name = base_property_name + "/pct-full"; + PropertyManager->Tie( property_name.c_str(), (FGTank*)this, &FGTank::GetPctFull); + property_name = base_property_name + "/priority"; PropertyManager->Tie( property_name.c_str(), (FGTank*)this, &FGTank::GetPriority, &FGTank::SetPriority ); @@ -293,7 +300,19 @@ double FGTank::Calculate(double dt, double TAT_C) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // This function calculates the moments of inertia for a solid propellant // grain - either an end burning cylindrical grain or a bored cylindrical -// grain. +// grain, as well as liquid propellants IF a tank radius and inertia factor +// are given. +// +// From NASA CR-383, the MoI of a tank with liquid propellant is specified +// for baffled and non-baffled tanks as a ratio compared to that in which the +// propellant is solid. The more baffles, the more "rigid" the propellant and +// the higher the ratio (up to 1.0). For a cube tank with five baffles, the +// ratio ranges from 0.5 to 0.7. For a cube tank with no baffles, the ratio is +// roughly 0.18. One might estimate that for a spherical tank with no baffles +// the ratio might be somewhere around 0.10 to 0.15. Cylindrical tanks with or +// without baffles might have biased moment of inertia effects based on the +// baffle layout and tank geometry. A vector inertia_factor may be supported +// at some point. void FGTank::CalculateInertias(void) { @@ -301,8 +320,12 @@ void FGTank::CalculateInertias(void) double RadSumSqr; double Rad2 = Radius*Radius; + if (grainType != gtUNKNOWN) { // assume solid propellant + if (Density > 0.0) { Volume = (Contents*lbtoslug)/Density; // in^3 + } else if (Contents <= 0.0) { + Volume = 0; } else { cerr << endl << " Solid propellant grain density is zero!" << endl << endl; exit(-1); @@ -327,6 +350,11 @@ void FGTank::CalculateInertias(void) } Izz = Iyy; + } else { // assume liquid propellant + + if (Radius > 0.0) Ixx = Iyy = Izz = Mass * InertiaFactor * 0.4 * Radius * Radius / 144.0; + + } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/propulsion/FGTank.h b/src/FDM/JSBSim/models/propulsion/FGTank.h index c1cfa020f..fc9532f86 100644 --- a/src/FDM/JSBSim/models/propulsion/FGTank.h +++ b/src/FDM/JSBSim/models/propulsion/FGTank.h @@ -52,7 +52,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_TANK "$Id: FGTank.h,v 1.26 2011/10/31 14:54:41 bcoconni Exp $" +#define ID_TANK "$Id: FGTank.h,v 1.27 2012/08/11 14:57:38 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -317,6 +317,7 @@ private: double Ixx; double Iyy; double Izz; + double InertiaFactor; double PctFull; double Contents, InitialContents; double PreviousUsed; diff --git a/src/FDM/JSBSim/models/propulsion/FGThruster.cpp b/src/FDM/JSBSim/models/propulsion/FGThruster.cpp index 7d4d55504..7bd53efb9 100644 --- a/src/FDM/JSBSim/models/propulsion/FGThruster.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGThruster.cpp @@ -45,7 +45,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGThruster.cpp,v 1.16 2012/03/18 15:48:36 jentron Exp $"; +static const char *IdSrc = "$Id: FGThruster.cpp,v 1.18 2013/01/12 21:11:59 jberndt Exp $"; static const char *IdHdr = ID_THRUSTER; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -57,7 +57,7 @@ FGThruster::FGThruster(FGFDMExec *FDMExec, Element *el, int num ): FGForce(FDMEx { Element* thruster_element = el->GetParent(); Element* element; - FGColumnVector3 location, orientation; + FGColumnVector3 location, orientation, pointing; Type = ttDirect; SetTransformType(FGForce::tCustom); @@ -66,6 +66,7 @@ FGThruster::FGThruster(FGFDMExec *FDMExec, Element *el, int num ): FGForce(FDMEx GearRatio = 1.0; ReverserAngle = 0.0; + Thrust = 0.0; EngineNum = num; PropertyManager = FDMExec->GetPropertyManager(); @@ -76,15 +77,28 @@ FGThruster::FGThruster(FGFDMExec *FDMExec, Element *el, int num ): FGForce(FDMEx if (element) location = element->FindElementTripletConvertTo("IN"); else cerr << fgred << " No thruster location found." << reset << endl; - element = thruster_element->FindElement("orient"); - if (element) orientation = element->FindElementTripletConvertTo("RAD"); - else cerr << " No thruster orientation found." << endl; - SetLocation(location); - SetAnglesToBody(orientation); string property_name, base_property_name; base_property_name = CreateIndexedPropertyName("propulsion/engine", EngineNum); + + element = thruster_element->FindElement("pointing"); + if (element) { + + // This defines a fixed nozzle that has no public interface property to gimbal or reverse it. + pointing = element->FindElementTripletConvertTo("RAD"); // The specification of RAD here is superfluous, + // and simply precludes a conversion. + mT.InitMatrix(); + mT(1,1) = pointing(1); + mT(2,1) = pointing(2); + mT(3,1) = pointing(3); + + } else { + + element = thruster_element->FindElement("orient"); + if (element) orientation = element->FindElementTripletConvertTo("RAD"); + + SetAnglesToBody(orientation); property_name = base_property_name + "/pitch-angle-rad"; PropertyManager->Tie( property_name.c_str(), (FGForce *)this, &FGForce::GetPitch, &FGForce::SetPitch); property_name = base_property_name + "/yaw-angle-rad"; @@ -98,6 +112,9 @@ FGThruster::FGThruster(FGFDMExec *FDMExec, Element *el, int num ): FGForce(FDMEx &FGThruster::SetReverserAngle); } + } + + Debug(0); } diff --git a/src/FDM/JSBSim/models/propulsion/FGTurbine.cpp b/src/FDM/JSBSim/models/propulsion/FGTurbine.cpp index b5712ad26..21880d73a 100644 --- a/src/FDM/JSBSim/models/propulsion/FGTurbine.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGTurbine.cpp @@ -49,7 +49,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGTurbine.cpp,v 1.36 2011/09/25 23:56:11 jentron Exp $"; +static const char *IdSrc = "$Id: FGTurbine.cpp,v 1.37 2012/11/17 19:38:43 jberndt Exp $"; static const char *IdHdr = ID_TURBINE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -139,13 +139,12 @@ void FGTurbine::Calculate(void) N1 = IdleN1 + ThrottlePos * N1_factor; OilTemp_degK = 366.0; Cutoff = false; - } - else { + } else { phase = tpOff; Cutoff = true; EGT_degC = TAT; - } } + } if (!Running && Cutoff && Starter) { if (phase == tpOff) phase = tpSpinUp; -- 2.39.5