diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index 686455669..047b58827 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -393,11 +393,8 @@ void FGFDMExec::LoadInputs(unsigned int idx) Auxiliary->in.SinTht = Propagate->GetSinEuler(eTht); Auxiliary->in.CosPhi = Propagate->GetCosEuler(ePhi); Auxiliary->in.SinPhi = Propagate->GetSinEuler(ePhi); - Auxiliary->in.Psi = Propagate->GetEuler(ePsi); Auxiliary->in.TotalWindNED = Winds->GetTotalWindNED(); Auxiliary->in.TurbPQR = Winds->GetTurbPQR(); - Auxiliary->in.WindPsi = Winds->GetWindPsi(); - Auxiliary->in.Vwind = Winds->GetTotalWindNED().Magnitude(); break; case eSystems: // Dynamic inputs come into the components that FCS manages through properties diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp index 75e7807f4..bb6cf9aab 100644 --- a/src/FDM/JSBSim/FGJSBBase.cpp +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -41,6 +41,7 @@ INCLUDES #include #include #include +#include "models/FGAtmosphere.h" using namespace std; @@ -289,49 +290,47 @@ double FGJSBBase::PitotTotalPressure(double mach, double p) // The denominator below is zero for Mach ~ 0.38, for which // we'll never be here, so we're safe - return p*166.92158*pow(mach,7.0)/pow(7*mach*mach-1,2.5); + return p*166.92158009316827*pow(mach,7.0)/pow(7*mach*mach-1,2.5); } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGJSBBase::VcalibratedFromMach(double mach, double p, double psl, double rhosl) -{ - double pt = PitotTotalPressure(mach, p); - double A = pow(((pt-p)/psl+1), 1./3.5); +// Based on the formulas in the US Air Force Aircraft Performance Flight Testing +// Manual (AFFTC-TIH-99-01). In particular sections 4.6 to 4.8. - return sqrt(7*psl/rhosl*(A-1)); +double FGJSBBase::MachFromImpactPressure(double qc, double p) +{ + double A = qc / p + 1; + double M = sqrt(5.0*(pow(A, 1. / 3.5) - 1)); // Equation (4.12) + + if (M > 1.0) + for (unsigned int i = 0; i<10; i++) + M = 0.8812848543473311*sqrt(A*pow(1 - 1.0 / (7.0*M*M), 2.5)); // Equation (4.17) + + return M; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGJSBBase::MachFromVcalibrated(double vcas, double p, double psl, double rhosl) +double FGJSBBase::VcalibratedFromMach(double mach, double p) { - double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1); + double asl = FGAtmosphere::StdDaySLsoundspeed; + double psl = FGAtmosphere::StdDaySLpressure; + double qc = PitotTotalPressure(mach, p) - p; - if (pt/p < 1.89293) - return sqrt(5.0*(pow(pt/p, 1./3.5) -1)); // Mach < 1 - else { - // Mach >= 1 - double mach = sqrt(0.77666*pt/p); // Initial guess is based on a quadratic approximation of the Rayleigh formula - double delta = 1.; - double target = pt/(166.92158*p); - int iter = 0; + return asl * MachFromImpactPressure(qc, psl); +} - // Find the root with Newton-Raphson. Since the differential is never zero, - // the function is monotonic and has only one root with a multiplicity of one. - // Convergence is certain. - while (delta > 1E-5 && iter < 10) { - double m2 = mach*mach; // Mach^2 - double m6 = m2*m2*m2; // Mach^6 - delta = mach*m6/pow(7.0*m2-1.0,2.5) - target; - double diff = 7.0*m6*(2.0*m2-1)/pow(7.0*m2-1.0,3.5); // Never zero when Mach >= 1 - mach -= delta/diff; - iter++; - } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - return mach; - } +double FGJSBBase::MachFromVcalibrated(double vcas, double p) +{ + double asl = FGAtmosphere::StdDaySLsoundspeed; + double psl = FGAtmosphere::StdDaySLpressure; + double qc = PitotTotalPressure(vcas / asl, psl) - psl; + + return MachFromImpactPressure(qc, p); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/FGJSBBase.h b/src/FDM/JSBSim/FGJSBBase.h index ad0705a0f..f8d3f8513 100644 --- a/src/FDM/JSBSim/FGJSBBase.h +++ b/src/FDM/JSBSim/FGJSBBase.h @@ -262,28 +262,31 @@ public: * @return The total pressure in front of the Pitot tube in psf */ static double PitotTotalPressure(double mach, double p); - /** Calculate the calibrated airspeed from the Mach number. It uses the - * Rayleigh formula for supersonic speeds (See "Introduction to Aerodynamics - * of a Compressible Fluid - H.W. Liepmann, A.E. Puckett - Wiley & sons - * (1947)" ยง5.4 pp 75-80) + /** Compute the Mach number from the differential pressure (qc) and the + * static pressure. Based on the formulas in the US Air Force Aircraft + * Performance Flight Testing Manual (AFFTC-TIH-99-01). + * @param qc The differential/impact pressure + * @param p Pressure in psf + * @return The Mach number */ + static double MachFromImpactPressure(double qc, double p); + + /** Calculate the calibrated airspeed from the Mach number. Based on the + * formulas in the US Air Force Aircraft Performance Flight Testing + * Manual (AFFTC-TIH-99-01). * @param mach The Mach number * @param p Pressure in psf - * @param psl Pressure at sea level in psf - * @param rhosl Density at sea level in slugs/ft^3 * @return The calibrated airspeed (CAS) in ft/s * */ - static double VcalibratedFromMach(double mach, double p, double psl, double rhosl); + static double VcalibratedFromMach(double mach, double p); - /** Calculate the Mach number from the calibrated airspeed. For subsonic - * speeds, the reversed formula has a closed form. For supersonic speeds, the - * Rayleigh formula is reversed by the Newton-Raphson algorithm. + /** Calculate the Mach number from the calibrated airspeed.Based on the + * formulas in the US Air Force Aircraft Performance Flight Testing + * Manual (AFFTC-TIH-99-01). * @param vcas The calibrated airspeed (CAS) in ft/s * @param p Pressure in psf - * @param psl Pressure at sea level in psf - * @param rhosl Density at sea level in slugs/ft^3 * @return The Mach number * */ - static double MachFromVcalibrated(double vcas, double p, double psl, double rhosl); + static double MachFromVcalibrated(double vcas, double p); /** Finite precision comparison. @param a first value to compare diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp index 6c0d59464..ff9582d6e 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp @@ -53,6 +53,7 @@ INCLUDES #include "models/FGAircraft.h" #include "models/FGAccelerations.h" #include "input_output/FGXMLFileRead.h" +#include "FGTrim.h" using namespace std; @@ -148,7 +149,7 @@ void FGInitialCondition::InitializeIC(void) lastAltitudeSet = setasl; lastLatitudeSet = setgeoc; enginesRunning = 0; - needTrim = 0; + trimRequested = TrimMode::tNone; } //****************************************************************************** @@ -178,9 +179,7 @@ void FGInitialCondition::SetVcalibratedKtsIC(double vcas) { double altitudeASL = position.GetAltitudeASL(); double pressure = Atmosphere->GetPressure(altitudeASL); - double pressureSL = Atmosphere->GetPressureSL(); - double rhoSL = Atmosphere->GetDensitySL(); - double mach = MachFromVcalibrated(fabs(vcas)*ktstofps, pressure, pressureSL, rhoSL); + double mach = MachFromVcalibrated(fabs(vcas)*ktstofps, pressure); double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL); SetVtrueFpsIC(mach * soundSpeed); @@ -691,13 +690,12 @@ void FGInitialCondition::SetAltitudeASLFtIC(double alt) { double altitudeASL = position.GetAltitudeASL(); double pressure = Atmosphere->GetPressure(altitudeASL); - double pressureSL = Atmosphere->GetPressureSL(); double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL); double rho = Atmosphere->GetDensity(altitudeASL); double rhoSL = Atmosphere->GetDensitySL(); double mach0 = vt / soundSpeed; - double vc0 = VcalibratedFromMach(mach0, pressure, pressureSL, rhoSL); + double vc0 = VcalibratedFromMach(mach0, pressure); double ve0 = vt * sqrt(rho/rhoSL); double geodLatitude = position.GetGeodLatitudeRad(); @@ -715,7 +713,7 @@ void FGInitialCondition::SetAltitudeASLFtIC(double alt) switch(lastSpeedSet) { case setvc: - mach0 = MachFromVcalibrated(vc0, pressure, pressureSL, rhoSL); + mach0 = MachFromVcalibrated(vc0, pressure); SetVtrueFpsIC(mach0 * soundSpeed); break; case setmach: @@ -834,12 +832,10 @@ double FGInitialCondition::GetVcalibratedKtsIC(void) const { double altitudeASL = position.GetAltitudeASL(); double pressure = Atmosphere->GetPressure(altitudeASL); - double pressureSL = Atmosphere->GetPressureSL(); - double rhoSL = Atmosphere->GetDensitySL(); double soundSpeed = Atmosphere->GetSoundSpeed(altitudeASL); double mach = vt / soundSpeed; - return fpstokts * VcalibratedFromMach(mach, pressure, pressureSL, rhoSL); + return fpstokts * VcalibratedFromMach(mach, pressure); } //****************************************************************************** @@ -916,7 +912,8 @@ bool FGInitialCondition::Load(const SGPath& rstfile, bool useStoredPath) // Check to see if any engines are specified to be initialized in a running state Element* running_elements = document->FindElement("running"); while (running_elements) { - enginesRunning |= 1 << int(running_elements->GetDataAsNumber()); + int engineNumber = int(running_elements->GetDataAsNumber()); + enginesRunning |= engineNumber == -1 ? engineNumber : 1 << engineNumber; running_elements = document->FindNextElement("running"); } @@ -979,6 +976,27 @@ bool FGInitialCondition::LoadLatitude(Element* position_el) //****************************************************************************** +void FGInitialCondition::SetTrimRequest(std::string trim) +{ + std::string& trimOption = to_lower(trim); + if (trimOption == "1") + trimRequested = TrimMode::tGround; // For backwards compatabiity + else if (trimOption == "longitudinal") + trimRequested = TrimMode::tLongitudinal; + else if (trimOption == "full") + trimRequested = TrimMode::tFull; + else if (trimOption == "ground") + trimRequested = TrimMode::tGround; + else if (trimOption == "pullup") + trimRequested = TrimMode::tPullup; + else if (trimOption == "custom") + trimRequested = TrimMode::tCustom; + else if (trimOption == "turn") + trimRequested = TrimMode::tTurn; +} + +//****************************************************************************** + bool FGInitialCondition::Load_v1(Element* document) { bool result = true; @@ -1047,7 +1065,7 @@ bool FGInitialCondition::Load_v1(Element* document) if (document->FindElement("targetNlf")) SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf")); if (document->FindElement("trim")) - needTrim = document->FindElementValueAsNumber("trim"); + SetTrimRequest(document->FindElementValue("trim")); // Refer to Stevens and Lewis, 1.5-14a, pg. 49. // This is the rotation rate of the "Local" frame, expressed in the local frame. @@ -1486,6 +1504,11 @@ void FGInitialCondition::bind(FGPropertyManager* PropertyManager) true); PropertyManager->Tie("ic/geod-alt-ft", &position, &FGLocation::GetGeodAltitude); + + PropertyManager->Tie("ic/targetNlf", this, + &FGInitialCondition::GetTargetNlfIC, + &FGInitialCondition::SetTargetNlfIC, + true); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.h b/src/FDM/JSBSim/initialization/FGInitialCondition.h index 96ff69fe8..58b93e227 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.h +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.h @@ -163,8 +163,8 @@ CLASS DOCUMENTATION - vc (calibrated airspeed, ft/sec) - mach (mach) - vground (ground speed, ft/sec) - - running (-1 for all engines, 0 for no engines, 1 ... n for specific engines) - - trim (0 for no trim, 1 for ground trim) + - trim (0 for no trim, 1 for ground trim, 'Longitudinal', 'Full', 'Ground', 'Pullup', 'Custom', 'Turn') + - running (-1 for all engines, 0 ... n-1 for specific engines)

Properties

@property ic/vc-kts (read/write) Calibrated airspeed initial condition in knots @@ -679,8 +679,8 @@ public: bool IsEngineRunning(unsigned int n) const { return (enginesRunning & (1 << n)) != 0; } /** Does initialization file call for trim ? - @return true if initialization file (version 1) called for trim. */ - bool NeedTrim(void) const { return needTrim == 0 ? false : true; } + @return Trim type, if any requested (version 1). */ + int TrimRequested(void) const { return trimRequested; } void bind(FGPropertyManager* pm); @@ -701,7 +701,7 @@ private: altitudeset lastAltitudeSet; latitudeset lastLatitudeSet; unsigned int enginesRunning; - int needTrim; + int trimRequested; FGFDMExec *fdmex; FGAtmosphere* Atmosphere; @@ -721,6 +721,7 @@ private: void calcThetaBeta(double alfa, const FGColumnVector3& _vt_NED); double ComputeGeodAltitude(double geodLatitude); bool LoadLatitude(Element* position_el); + void SetTrimRequest(std::string trim); void Debug(int from); }; } diff --git a/src/FDM/JSBSim/initialization/FGTrim.cpp b/src/FDM/JSBSim/initialization/FGTrim.cpp index 0dabc461b..2fff1cb8a 100644 --- a/src/FDM/JSBSim/initialization/FGTrim.cpp +++ b/src/FDM/JSBSim/initialization/FGTrim.cpp @@ -76,7 +76,7 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) gamma_fallback=false; mode=tt; xlo=xhi=alo=ahi=0.0; - targetNlf=1.0; + targetNlf=fgic.GetTargetNlfIC(); debug_axis=tAll; SetMode(tt); if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl; diff --git a/src/FDM/JSBSim/input_output/FGInputSocket.cpp b/src/FDM/JSBSim/input_output/FGInputSocket.cpp index 5bda60430..f5a9510e9 100644 --- a/src/FDM/JSBSim/input_output/FGInputSocket.cpp +++ b/src/FDM/JSBSim/input_output/FGInputSocket.cpp @@ -57,9 +57,8 @@ CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ FGInputSocket::FGInputSocket(FGFDMExec* fdmex) : - FGInputType(fdmex), - socket(0), - SockProtocol(FGfdmSocket::ptTCP) + FGInputType(fdmex), socket(0), SockProtocol(FGfdmSocket::ptTCP), + BlockingInput(false) { } @@ -84,6 +83,10 @@ bool FGInputSocket::Load(Element* el) return false; } + string action = el->GetAttributeValue("action"); + if (to_upper(action) == "BLOCKING_INPUT") + BlockingInput = true; + return true; } @@ -116,7 +119,9 @@ void FGInputSocket::Read(bool Holding) if (socket == 0) return; if (!socket->GetConnectStatus()) return; - data = socket->Receive(); // get socket transmission if present + if (BlockingInput) + socket->WaitUntilReadable(); // block until a transmission is received + data = socket->Receive(); // read data if (data.size() > 0) { // parse lines @@ -142,7 +147,7 @@ void FGInputSocket::Read(bool Holding) } } - if (command == "set") { // SET PROPERTY + if (command == "set") { // SET PROPERTY if (argument.size() == 0) { socket->Reply("No property argument supplied.\n"); @@ -165,7 +170,7 @@ void FGInputSocket::Read(bool Holding) value = atof(str_value.c_str()); node->setDoubleValue(value); } - socket->Reply(""); + socket->Reply("set successful\n"); } else if (command == "get") { // GET PROPERTY @@ -196,17 +201,17 @@ void FGInputSocket::Read(bool Holding) socket->Reply(buf.str()); } - } else if (command == "hold") { // PAUSE + } else if (command == "hold") { // PAUSE FDMExec->Hold(); - socket->Reply(""); + socket->Reply("Holding\n"); } else if (command == "resume") { // RESUME FDMExec->Resume(); - socket->Reply(""); + socket->Reply("Resuming\n"); - } else if (command == "iterate") { // ITERATE + } else if (command == "iterate") { // ITERATE int argumentInt; istringstream (argument) >> argumentInt; @@ -220,15 +225,15 @@ void FGInputSocket::Read(bool Holding) } FDMExec->EnableIncrementThenHold( argumentInt ); FDMExec->Resume(); - socket->Reply(""); + socket->Reply("Iterations performed\n"); - } else if (command == "quit") { // QUIT + } else if (command == "quit") { // QUIT // close the socket connection - socket->Reply(""); + socket->Reply("Closing connection\n"); socket->Close(); - } else if (command == "info") { // INFO + } else if (command == "info") { // INFO // get info about the sim run and/or aircraft, etc. ostringstream info; @@ -238,7 +243,7 @@ void FGInputSocket::Read(bool Holding) info << "Simulation time: " << setw(8) << setprecision(3) << FDMExec->GetSimTime() << endl; socket->Reply(info.str()); - } else if (command == "help") { // HELP + } else if (command == "help") { // HELP socket->Reply( " JSBSim Server commands:\n\n" diff --git a/src/FDM/JSBSim/input_output/FGInputSocket.h b/src/FDM/JSBSim/input_output/FGInputSocket.h index 904ea3451..97c4e5df6 100644 --- a/src/FDM/JSBSim/input_output/FGInputSocket.h +++ b/src/FDM/JSBSim/input_output/FGInputSocket.h @@ -88,6 +88,7 @@ protected: FGfdmSocket* socket; FGfdmSocket::ProtocolType SockProtocol; std::string data; + bool BlockingInput; }; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/input_output/FGXMLElement.cpp b/src/FDM/JSBSim/input_output/FGXMLElement.cpp index 986adbf72..55e0fc09f 100644 --- a/src/FDM/JSBSim/input_output/FGXMLElement.cpp +++ b/src/FDM/JSBSim/input_output/FGXMLElement.cpp @@ -124,6 +124,7 @@ Element::Element(const string& nm) convert["KTS"]["FT/SEC"] = 1.68781; convert["FT/SEC"]["KTS"] = 1.0/convert["KTS"]["FT/SEC"]; convert["M/S"]["FT/S"] = 3.2808399; + convert["M/S"]["KTS"] = convert["M/S"]["FT/S"]/convert["KTS"]["FT/SEC"]; convert["M/SEC"]["FT/SEC"] = 3.2808399; convert["FT/S"]["M/S"] = 1.0/convert["M/S"]["FT/S"]; convert["M/SEC"]["FT/SEC"] = 3.2808399; @@ -283,7 +284,7 @@ double Element::GetAttributeValueAsNumber(const string& attr) << attribute << endl; exit(-1); } - + return (number); } } @@ -473,8 +474,8 @@ double Element::FindElementValueAsNumberConvertTo(const string& el, const string << value << " DEG is outside the range [ -360 DEG ; +360 DEG ]" << endl; } - - + + if (!supplied_units.empty()) { value *= convert[supplied_units][target_units]; } @@ -564,7 +565,7 @@ FGColumnVector3 Element::FindElementTripletConvertTo( const string& target_units } else { triplet(1) = 0.0; } - + item = FindElement("y"); if (!item) item = FindElement("pitch"); diff --git a/src/FDM/JSBSim/input_output/FGfdmSocket.cpp b/src/FDM/JSBSim/input_output/FGfdmSocket.cpp index 1cbac0f77..ba84668d3 100644 --- a/src/FDM/JSBSim/input_output/FGfdmSocket.cpp +++ b/src/FDM/JSBSim/input_output/FGfdmSocket.cpp @@ -357,10 +357,23 @@ void FGfdmSocket::Send(const char *data, int length) void FGfdmSocket::WaitUntilReadable(void) { + if (sckt_in <= 0) + return; + fd_set fds; FD_ZERO(&fds); FD_SET(sckt_in, &fds); select(sckt_in+1, &fds, NULL, NULL, NULL); + + /* + If you want to check select return status: + + int recVal = select(sckt_in+1, &fds, NULL, NULL, NULL); + recVal: received value + 0, if socket timeout + -1, if socket error + anithing else, if socket is readable + */ } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/math/LagrangeMultiplier.h b/src/FDM/JSBSim/math/LagrangeMultiplier.h index e5cbe586c..b5b8f3c29 100644 --- a/src/FDM/JSBSim/math/LagrangeMultiplier.h +++ b/src/FDM/JSBSim/math/LagrangeMultiplier.h @@ -50,7 +50,7 @@ CLASS DECLARATION struct LagrangeMultiplier { FGColumnVector3 ForceJacobian; - FGColumnVector3 MomentJacobian; + FGColumnVector3 LeverArm; double Min; double Max; double value; diff --git a/src/FDM/JSBSim/models/FGAccelerations.cpp b/src/FDM/JSBSim/models/FGAccelerations.cpp index 905206ad6..936456cdc 100644 --- a/src/FDM/JSBSim/models/FGAccelerations.cpp +++ b/src/FDM/JSBSim/models/FGAccelerations.cpp @@ -118,7 +118,7 @@ bool FGAccelerations::Run(bool Holding) CalculateUVWdot(); // Translational rate derivative if (!FDMExec->GetHoldDown()) - ResolveFrictionForces(in.DeltaT * rate); // Update rate derivatives with friction forces + CalculateFrictionForces(in.DeltaT * rate); // Update rate derivatives with friction forces Debug(2); return false; @@ -233,7 +233,7 @@ void FGAccelerations::SetHoldDown(bool hd) } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// Resolves the contact forces just before integrating the EOM. +// Computes the contact forces just before integrating the EOM. // This routine is using Lagrange multipliers and the projected Gauss-Seidel // (PGS) method. // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence", @@ -245,11 +245,8 @@ void FGAccelerations::SetHoldDown(bool hd) // The friction forces are resolved in the body frame relative to the origin // (Earth center). -void FGAccelerations::ResolveFrictionForces(double dt) +void FGAccelerations::CalculateFrictionForces(double dt) { - const double invMass = 1.0 / in.Mass; - const FGMatrix33& Jinv = in.Jinv; - FGColumnVector3 vdot, wdot; vector& multipliers = *in.MultipliersList; size_t n = multipliers.size(); @@ -264,25 +261,30 @@ void FGAccelerations::ResolveFrictionForces(double dt) // Assemble the linear system of equations for (unsigned int i=0; i < n; i++) { - FGColumnVector3 v1 = invMass * multipliers[i]->ForceJacobian; - FGColumnVector3 v2 = Jinv * multipliers[i]->MomentJacobian; // Should be J^-T but J is symmetric and so is J^-1 + FGColumnVector3 U = multipliers[i]->ForceJacobian; + FGColumnVector3 r = multipliers[i]->LeverArm; + FGColumnVector3 v1 = U / in.Mass; + FGColumnVector3 v2 = in.Jinv * (r*U); // Should be J^-T but J is symmetric and so is J^-1 for (unsigned int j=0; j < i; j++) a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of Jac^T*M^-1*Jac - for (unsigned int j=i; j < n; j++) - a[i*n+j] = DotProduct(v1, multipliers[j]->ForceJacobian) - + DotProduct(v2, multipliers[j]->MomentJacobian); + + for (unsigned int j=i; j < n; j++) { + U = multipliers[j]->ForceJacobian; + r = multipliers[j]->LeverArm; + a[i*n+j] = DotProduct(U, v1 + v2*r); + } } // Assemble the RHS member // Translation - vdot = vUVWdot; + FGColumnVector3 vdot = vUVWdot; if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground vdot += (in.vUVW - in.Tec2b * in.TerrainVelocity) / dt; // Rotation - wdot = vPQRdot; + FGColumnVector3 wdot = vPQRdot; if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground wdot += (in.vPQR - in.Tec2b * in.TerrainAngularVel) / dt; @@ -291,12 +293,14 @@ void FGAccelerations::ResolveFrictionForces(double dt) // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save // a division computation at each iteration of Gauss-Seidel. for (unsigned int i=0; i < n; i++) { - double d = 1.0 / a[i*n+i]; + double d = a[i*n+i]; + FGColumnVector3 U = multipliers[i]->ForceJacobian; + FGColumnVector3 r = multipliers[i]->LeverArm; + + rhs[i] = -DotProduct(U, vdot + wdot*r)/d; - rhs[i] = -(DotProduct(multipliers[i]->ForceJacobian, vdot) - +DotProduct(multipliers[i]->MomentJacobian, wdot))*d; for (unsigned int j=0; j < n; j++) - a[i*n+j] *= d; + a[i*n+j] /= d; } // Resolve the Lagrange multipliers with the projected Gauss-Seidel method @@ -323,12 +327,16 @@ void FGAccelerations::ResolveFrictionForces(double dt) for (unsigned int i=0; i< n; i++) { double lambda = multipliers[i]->value; - vFrictionForces += lambda * multipliers[i]->ForceJacobian; - vFrictionMoments += lambda * multipliers[i]->MomentJacobian; + FGColumnVector3 U = multipliers[i]->ForceJacobian; + FGColumnVector3 r = multipliers[i]->LeverArm; + + FGColumnVector3 F = lambda * U; + vFrictionForces += F; + vFrictionMoments += r * F; } - FGColumnVector3 accel = invMass * vFrictionForces; - FGColumnVector3 omegadot = Jinv * vFrictionMoments; + FGColumnVector3 accel = vFrictionForces / in.Mass; + FGColumnVector3 omegadot = in.Jinv * vFrictionMoments; vBodyAccel += accel; vUVWdot += accel; @@ -344,7 +352,7 @@ void FGAccelerations::InitializeDerivatives(void) // Make an initial run and set past values CalculatePQRdot(); // Angular rate derivative CalculateUVWdot(); // Translational rate derivative - ResolveFrictionForces(0.); // Update rate derivatives with friction forces + CalculateFrictionForces(0.); // Update rate derivatives with friction forces } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/models/FGAccelerations.h b/src/FDM/JSBSim/models/FGAccelerations.h index 5ae56def8..827d09851 100644 --- a/src/FDM/JSBSim/models/FGAccelerations.h +++ b/src/FDM/JSBSim/models/FGAccelerations.h @@ -59,8 +59,6 @@ CLASS DOCUMENTATION - Calculate the angular accelerations - Calculate the translational accelerations - - Calculate the angular rate - - Calculate the translational velocity This class is collecting all the forces and the moments acting on the body to calculate the corresponding accelerations according to Newton's second @@ -100,7 +98,7 @@ class FGAccelerations : public FGModel { public: /** Constructor. @param Executive a pointer to the parent executive object */ - FGAccelerations(FGFDMExec* Executive); + explicit FGAccelerations(FGFDMExec* Executive); /// Destructor ~FGAccelerations(); @@ -391,7 +389,7 @@ private: void CalculatePQRdot(void); void CalculateUVWdot(void); - void ResolveFrictionForces(double dt); + void CalculateFrictionForces(double dt); void bind(void); void Debug(int from); diff --git a/src/FDM/JSBSim/models/FGAtmosphere.cpp b/src/FDM/JSBSim/models/FGAtmosphere.cpp index cd78b87e0..ebf58e437 100644 --- a/src/FDM/JSBSim/models/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/models/FGAtmosphere.cpp @@ -65,6 +65,10 @@ double FGAtmosphere::Reng = Rstar / Mair; const double FGAtmosphere::SHRatio = 1.40; +const double FGAtmosphere::StdDaySLtemperature = 518.67; +const double FGAtmosphere::StdDaySLpressure = 2116.228; +const double FGAtmosphere::StdDaySLsoundspeed = sqrt(SHRatio*Reng*StdDaySLtemperature); + FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex), PressureAltitude(0.0), // ft DensityAltitude(0.0), // ft @@ -91,10 +95,10 @@ bool FGAtmosphere::InitModel(void) if (!FGModel::InitModel()) return false; Calculate(0.0); - SLtemperature = Temperature = 518.67; - SLpressure = Pressure = 2116.228; + SLtemperature = Temperature = StdDaySLtemperature; + SLpressure = Pressure = StdDaySLpressure; SLdensity = Density = Pressure/(Reng*Temperature); - SLsoundspeed = Soundspeed = sqrt(SHRatio*Reng*Temperature); + SLsoundspeed = Soundspeed = StdDaySLsoundspeed; return true; } diff --git a/src/FDM/JSBSim/models/FGAtmosphere.h b/src/FDM/JSBSim/models/FGAtmosphere.h index b3eaa2551..381a7c078 100644 --- a/src/FDM/JSBSim/models/FGAtmosphere.h +++ b/src/FDM/JSBSim/models/FGAtmosphere.h @@ -211,6 +211,10 @@ public: double altitudeASL; } in; + static const double StdDaySLtemperature; + static const double StdDaySLpressure; + static const double StdDaySLsoundspeed; + protected: double SLtemperature, SLdensity, SLpressure, SLsoundspeed; // Sea level conditions double Temperature, Density, Pressure, Soundspeed; // Current actual conditions at altitude diff --git a/src/FDM/JSBSim/models/FGAuxiliary.cpp b/src/FDM/JSBSim/models/FGAuxiliary.cpp index 35b24d23e..46cdf1d3b 100644 --- a/src/FDM/JSBSim/models/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/models/FGAuxiliary.cpp @@ -59,9 +59,9 @@ CLASS IMPLEMENTATION FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex) { Name = "FGAuxiliary"; - pt = 1.0; - tat = 1.0; - tatc = RankineToCelsius(tat); + pt = 2116.23; // ISA SL pressure + tatc = 15.0; // ISA SL temperature + tat = 518.67; vcas = veas = 0.0; qbar = qbarUW = qbarUV = 0.0; @@ -81,7 +81,6 @@ FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex) vAeroUVW.InitMatrix(); vAeroPQR.InitMatrix(); vMachUVW.InitMatrix(); - vEuler.InitMatrix(); vEulerRates.InitMatrix(); bind(); @@ -117,7 +116,6 @@ bool FGAuxiliary::InitModel(void) vAeroUVW.InitMatrix(); vAeroPQR.InitMatrix(); vMachUVW.InitMatrix(); - vEuler.InitMatrix(); vEulerRates.InitMatrix(); return true; @@ -204,7 +202,7 @@ bool FGAuxiliary::Run(bool Holding) pt = PitotTotalPressure(Mach, in.Pressure); if (abs(Mach) > 0.0) { - vcas = VcalibratedFromMach(Mach, in.Pressure, in.PressureSL, in.DensitySL); + vcas = VcalibratedFromMach(Mach, in.Pressure); veas = sqrt(2 * qbar / in.DensitySL); } else @@ -273,34 +271,12 @@ void FGAuxiliary::UpdateWindMatrices(void) mTb2w = mTw2b.Transposed(); } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// -// A positive headwind is blowing with you, a negative headwind is blowing against you. -// psi is the direction the wind is blowing *towards*. -// ToDo: should this simply be in the atmosphere class? Same with Get Crosswind. - -double FGAuxiliary::GetHeadWind(void) const -{ - return in.Vwind * cos(in.WindPsi - in.Psi); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// -// A positive crosswind is blowing towards the right (from teh perspective of the -// pilot). A negative crosswind is blowing towards the -Y direction (left). -// psi is the direction the wind is blowing *towards*. - -double FGAuxiliary::GetCrossWind(void) const -{ - return in.Vwind * sin(in.WindPsi - in.Psi); -} - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% double FGAuxiliary::GetNlf(void) const { if (in.Mass != 0) - return (-in.vFw(3))/(in.Mass*slugtolb); + return (in.vFw(3))/(in.Mass*slugtolb); else return 0.; } @@ -375,8 +351,6 @@ void FGAuxiliary::bind(void) PropertyManager->Tie("accelerations/Nz", this, &FGAuxiliary::GetNz); PropertyManager->Tie("accelerations/Ny", this, &FGAuxiliary::GetNy); PropertyManager->Tie("forces/load-factor", this, &FGAuxiliary::GetNlf); - /* PropertyManager->Tie("atmosphere/headwind-fps", this, &FGAuxiliary::GetHeadWind, true); - PropertyManager->Tie("atmosphere/crosswind-fps", this, &FGAuxiliary::GetCrossWind, true); */ PropertyManager->Tie("aero/alpha-rad", this, (PF)&FGAuxiliary::Getalpha); PropertyManager->Tie("aero/beta-rad", this, (PF)&FGAuxiliary::Getbeta); PropertyManager->Tie("aero/mag-beta-rad", this, (PF)&FGAuxiliary::GetMagBeta); diff --git a/src/FDM/JSBSim/models/FGAuxiliary.h b/src/FDM/JSBSim/models/FGAuxiliary.h index fb3a210fa..c592997d8 100644 --- a/src/FDM/JSBSim/models/FGAuxiliary.h +++ b/src/FDM/JSBSim/models/FGAuxiliary.h @@ -234,9 +234,6 @@ public: else return BadUnits(); } - double GetHeadWind(void) const; - double GetCrossWind(void) const; - // Time routines, SET and GET functions, used by FGMSIS atmosphere void SetDayOfYear (int doy) { day_of_year = doy; } @@ -282,11 +279,8 @@ public: double SinTht; double CosPhi; double SinPhi; - double Psi; FGColumnVector3 TotalWindNED; FGColumnVector3 TurbPQR; - double WindPsi; - double Vwind; } in; private: @@ -302,7 +296,6 @@ private: FGColumnVector3 vNwcg; FGColumnVector3 vAeroPQR; FGColumnVector3 vAeroUVW; - FGColumnVector3 vEuler; FGColumnVector3 vEulerRates; FGColumnVector3 vMachUVW; FGLocation vLocationVRP; diff --git a/src/FDM/JSBSim/models/FGLGear.cpp b/src/FDM/JSBSim/models/FGLGear.cpp index 816d6125d..25cd5de61 100644 --- a/src/FDM/JSBSim/models/FGLGear.cpp +++ b/src/FDM/JSBSim/models/FGLGear.cpp @@ -264,7 +264,7 @@ void FGLGear::ResetToIC(void) // Initialize Lagrange multipliers for (int i=0; i < 3; i++) { LMultiplier[i].ForceJacobian.InitMatrix(); - LMultiplier[i].MomentJacobian.InitMatrix(); + LMultiplier[i].LeverArm.InitMatrix(); LMultiplier[i].Min = 0.0; LMultiplier[i].Max = 0.0; LMultiplier[i].value = 0.0; @@ -675,29 +675,31 @@ void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec) velocityDirection.Normalize(); LMultiplier[ftDynamic].ForceJacobian = mT * velocityDirection; - LMultiplier[ftDynamic].MomentJacobian = vWhlContactVec * LMultiplier[ftDynamic].ForceJacobian; LMultiplier[ftDynamic].Max = 0.; LMultiplier[ftDynamic].Min = -fabs(staticFFactor * dynamicFCoeff * vFn(eZ)); + LMultiplier[ftDynamic].LeverArm = vWhlContactVec; - // The Lagrange multiplier value obtained from the previous iteration is kept - // This is supposed to accelerate the convergence of the projected Gauss-Seidel - // algorithm. The code just below is to make sure that the initial value - // is consistent with the current friction coefficient and normal reaction. + // The Lagrange multiplier value obtained from the previous iteration is + // kept. This is supposed to accelerate the convergence of the projected + // Gauss-Seidel algorithm. The code just below is to make sure that the + // initial value is consistent with the current friction coefficient and + // normal reaction. LMultiplier[ftDynamic].value = Constrain(LMultiplier[ftDynamic].Min, LMultiplier[ftDynamic].value, LMultiplier[ftDynamic].Max); GroundReactions->RegisterLagrangeMultiplier(&LMultiplier[ftDynamic]); } else { - // Static friction is used for ctSTRUCTURE when the contact point is not moving. - // It is always used for ctBOGEY elements because the friction coefficients - // of a tyre depend on the direction of the movement (roll & side directions). - // This cannot be handled properly by the so-called "dynamic friction". + // Static friction is used for ctSTRUCTURE when the contact point is not + // moving. It is always used for ctBOGEY elements because the friction + // coefficients of a tyre depend on the direction of the movement (roll & + // side directions). This cannot be handled properly by the so-called + // "dynamic friction". StaticFriction = true; LMultiplier[ftRoll].ForceJacobian = mT * FGColumnVector3(1.,0.,0.); LMultiplier[ftSide].ForceJacobian = mT * FGColumnVector3(0.,1.,0.); - LMultiplier[ftRoll].MomentJacobian = vWhlContactVec * LMultiplier[ftRoll].ForceJacobian; - LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian; + LMultiplier[ftRoll].LeverArm = vWhlContactVec; + LMultiplier[ftSide].LeverArm = vWhlContactVec; switch(eContactType) { case ctBOGEY: @@ -713,10 +715,11 @@ void FGLGear::ComputeJacobian(const FGColumnVector3& vWhlContactVec) LMultiplier[ftRoll].Min = -LMultiplier[ftRoll].Max; LMultiplier[ftSide].Min = -LMultiplier[ftSide].Max; - // The Lagrange multiplier value obtained from the previous iteration is kept - // This is supposed to accelerate the convergence of the projected Gauss-Seidel - // algorithm. The code just below is to make sure that the initial value - // is consistent with the current friction coefficient and normal reaction. + // The Lagrange multiplier value obtained from the previous iteration is + // kept. This is supposed to accelerate the convergence of the projected + // Gauss-Seidel algorithm. The code just below is to make sure that the + // initial value is consistent with the current friction coefficient and + // normal reaction. LMultiplier[ftRoll].value = Constrain(LMultiplier[ftRoll].Min, LMultiplier[ftRoll].value, LMultiplier[ftRoll].Max); LMultiplier[ftSide].value = Constrain(LMultiplier[ftSide].Min, LMultiplier[ftSide].value, LMultiplier[ftSide].Max); diff --git a/src/FDM/JSBSim/models/propulsion/FGPiston.cpp b/src/FDM/JSBSim/models/propulsion/FGPiston.cpp index c41d29cbc..a7885ecc3 100644 --- a/src/FDM/JSBSim/models/propulsion/FGPiston.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGPiston.cpp @@ -388,6 +388,8 @@ FGPiston::FGPiston(FGFDMExec* exec, Element* el, int engine_number, struct Input property_name = base_property_name + "/boostloss-hp"; PropertyManager->Tie(property_name, &BoostLossHP); } + property_name = base_property_name + "/AFR"; + PropertyManager->Tie(property_name, this, &FGPiston::getAFR); // Set up and sanity-check the turbo/supercharging configuration based on the input values. if (TakeoffBoost > RatedBoost[0]) bTakeoffBoost = true; @@ -749,8 +751,6 @@ void FGPiston::doFuelFlow(void) { double thi_sea_level = 1.3 * in.MixturePos[EngineNumber]; // Allows an AFR of infinity:1 to 11.3075:1 equivalence_ratio = thi_sea_level * 101325.0 / p_amb; -// double AFR = 10+(12*(1-in.Mixture[EngineNumber]));// mixture 10:1 to 22:1 -// m_dot_fuel = m_dot_air / AFR; m_dot_fuel = (m_dot_air * equivalence_ratio) / 14.7; FuelFlowRate = m_dot_fuel * 2.2046; // kg to lb if(Starved) // There is no fuel, so zero out the flows we've calculated so far diff --git a/src/FDM/JSBSim/models/propulsion/FGPiston.h b/src/FDM/JSBSim/models/propulsion/FGPiston.h index 8c2668d3f..0dd2dad55 100644 --- a/src/FDM/JSBSim/models/propulsion/FGPiston.h +++ b/src/FDM/JSBSim/models/propulsion/FGPiston.h @@ -46,7 +46,6 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_PISTON "$Id: FGPiston.h,v 1.39 2017/04/29 11:16:46 ehofman Exp $" #define FG_MAX_BOOST_SPEEDS 3 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -215,7 +214,6 @@ boostspeed they refer to: @author David Megginson (initial porting and additional code) @author Ron Jensen (additional engine code) @see Taylor, Charles Fayette, "The Internal Combustion Engine in Theory and Practice" - @version $Id: FGPiston.h,v 1.39 2017/04/29 11:16:46 ehofman Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -249,6 +247,7 @@ public: double getOilPressure_psi(void) const {return OilPressure_psi;} double getOilTemp_degF (void) const {return KelvinToFahrenheit(OilTemp_degK);} double getRPM(void) const {return RPM;} + double getAFR(void) const {return m_dot_fuel > 0.0 ? m_dot_air / m_dot_fuel : INFINITY;} protected: