Sync'ed JSBSim

- Fixed the Calibrated Air Speed (CAS) computations for supersonic velocities
- Fixed the Nlf (Normal load factor) sign
- Nlf can now be specified as an initial condition via the property ic/targetNlf
- Added blocking sockets to the input features
- Added a property to piston engines to get the AFR (Air to Fuel Ratio)
- Added conversion from m/s to ft/s
- Restored the initial conditions for engines running (-1 means all engines)

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

src/FDM/JSBSim/FGJSBBase.cpp

@@ -41,6 +41,7 @@ INCLUDES
 #include <iostream>
 #include <sstream>
 #include <cstdlib>
+#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)
+// 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.
-  double pt = PitotTotalPressure(mach, p);
-  double A = pow(((pt-p)/psl+1), 1./3.5);
 
-  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 asl * MachFromImpactPressure(qc, psl);  
-    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;
-
-    // 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);
 }
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 

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
+  /** Compute the Mach number from the differential pressure (qc) and the
-  *   Rayleigh formula for supersonic speeds (See "Introduction to Aerodynamics
+  *   static pressure. Based on the formulas in the US Air Force Aircraft 
-  *   of a Compressible Fluid - H.W. Liepmann, A.E. Puckett - Wiley & sons
+  *   Performance Flight Testing Manual (AFFTC-TIH-99-01).
-  *   (1947)" §5.4 pp 75-80)
+  *   @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
+  /** Calculate the Mach number from the calibrated airspeed.Based on the
-  * speeds, the reversed formula has a closed form. For supersonic speeds, the
+  *   formulas in the US Air Force Aircraft Performance Flight Testing 
-  * Rayleigh formula is reversed by the Newton-Raphson algorithm.
+  *   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

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 mach = MachFromVcalibrated(fabs(vcas)*ktstofps, pressure);
-  double rhoSL = Atmosphere->GetDensitySL();
-  double mach = MachFromVcalibrated(fabs(vcas)*ktstofps, pressure, pressureSL, rhoSL);
   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);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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, 'Longitudinal', 'Full', 'Ground', 'Pullup', 'Custom', 'Turn')
-   - trim (0 for no trim, 1 for ground trim)
+   - running (-1 for all engines, 0 ... n-1 for specific engines)
 
    <h3>Properties</h3>
    @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. */
+      @return Trim type, if any requested (version 1). */
-  bool NeedTrim(void) const { return needTrim == 0 ? false : true; }
+  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);
 };
 }

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;

src/FDM/JSBSim/input_output/FGInputSocket.cpp

@@ -57,9 +57,8 @@ CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 FGInputSocket::FGInputSocket(FGFDMExec* fdmex) :
-  FGInputType(fdmex),
+  FGInputType(fdmex), socket(0), SockProtocol(FGfdmSocket::ptTCP),
-  socket(0),
+  BlockingInput(false)
-  SockProtocol(FGfdmSocket::ptTCP)
 {
 }
 
@@ -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
@@ -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
 
@@ -199,12 +204,12 @@ void FGInputSocket::Read(bool Holding)
       } 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
 
@@ -220,12 +225,12 @@ void FGInputSocket::Read(bool Holding)
         }
         FDMExec->EnableIncrementThenHold( argumentInt );
         FDMExec->Resume();
-        socket->Reply("");
+        socket->Reply("Iterations performed\n");
 
       } else if (command == "quit") {               // QUIT
 
         // close the socket connection
-        socket->Reply("");
+        socket->Reply("Closing connection\n");
         socket->Close();
 
       } else if (command == "info") {               // INFO

src/FDM/JSBSim/input_output/FGInputSocket.h

@@ -88,6 +88,7 @@ protected:
   FGfdmSocket* socket;
   FGfdmSocket::ProtocolType SockProtocol;
   std::string data;
+  bool BlockingInput;
 };
 }
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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;

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
+  */
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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;

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<LagrangeMultiplier*>& 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 U = multipliers[i]->ForceJacobian;
-    FGColumnVector3 v2 = Jinv * multipliers[i]->MomentJacobian; // Should be J^-T but J is symmetric and so is J^-1
+    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)
+    for (unsigned int j=i; j < n; j++) {
-               + DotProduct(v2, multipliers[j]->MomentJacobian);
+      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;
+    FGColumnVector3 U = multipliers[i]->ForceJacobian;
-    vFrictionMoments += lambda * multipliers[i]->MomentJacobian;
+    FGColumnVector3 r = multipliers[i]->LeverArm;
+
+    FGColumnVector3 F = lambda * U;
+    vFrictionForces += F;
+    vFrictionMoments += r * F;
   }
 
-  FGColumnVector3 accel = invMass * vFrictionForces;
+  FGColumnVector3 accel = vFrictionForces / in.Mass;
-  FGColumnVector3 omegadot = Jinv * vFrictionMoments;
+  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
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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);

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;
+  SLtemperature = Temperature = StdDaySLtemperature;
-  SLpressure = Pressure = 2116.228;
+  SLpressure = Pressure = StdDaySLpressure;
   SLdensity = Density = Pressure/(Reng*Temperature);
-  SLsoundspeed = Soundspeed = sqrt(SHRatio*Reng*Temperature);
+  SLsoundspeed = Soundspeed = StdDaySLsoundspeed;
 
   return true;
 }

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

src/FDM/JSBSim/models/FGAuxiliary.cpp

@@ -59,9 +59,9 @@ CLASS IMPLEMENTATION
 FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex)
 {
   Name = "FGAuxiliary";
-  pt = 1.0;
+  pt = 2116.23; // ISA SL pressure
-  tat = 1.0;
+  tatc = 15.0; // ISA SL temperature
-  tatc = RankineToCelsius(tat);
+  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);

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;

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
+    // The Lagrange multiplier value obtained from the previous iteration is
-    // This is supposed to accelerate the convergence of the projected Gauss-Seidel
+    // kept. This is supposed to accelerate the convergence of the projected
-    // algorithm. The code just below is to make sure that the initial value
+    // Gauss-Seidel algorithm. The code just below is to make sure that the
-    // is consistent with the current friction coefficient and normal reaction.
+    // 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.
+    // Static friction is used for ctSTRUCTURE when the contact point is not
-    // It is always used for ctBOGEY elements because the friction coefficients
+    // moving. It is always used for ctBOGEY elements because the friction
-    // of a tyre depend on the direction of the movement (roll & side directions).
+    // coefficients of a tyre depend on the direction of the movement (roll &
-    // This cannot be handled properly by the so-called "dynamic friction".
+    // 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[ftRoll].LeverArm = vWhlContactVec;
-    LMultiplier[ftSide].MomentJacobian = vWhlContactVec * LMultiplier[ftSide].ForceJacobian;
+    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
+    // The Lagrange multiplier value obtained from the previous iteration is
-    // This is supposed to accelerate the convergence of the projected Gauss-Seidel
+    // kept. This is supposed to accelerate the convergence of the projected
-    // algorithm. The code just below is to make sure that the initial value
+    // Gauss-Seidel algorithm. The code just below is to make sure that the
-    // is consistent with the current friction coefficient and normal reaction.
+    // 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);
 

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

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: