Fix for bug 1304 - crash loading XML route

[flightgear.git] / src / FDM / JSBSim / FGJSBBase.cpp

diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp
index f07a4f08158fecbd18c6718e3926a2a995bc9be3..b509fd1b8d6ef19672e9a8fc56174a3c64e31a5e 100644 (file)
--- a/src/FDM/JSBSim/FGJSBBase.cpp
+++ b/src/FDM/JSBSim/FGJSBBase.cpp
@@ -44,8 +44,8 @@ INCLUDES
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
-static const char *IdHdr = ID_JSBBASE;
+IDENT(IdSrc,"$Id: FGJSBBase.cpp,v 1.38 2014/01/13 10:45:59 ehofman Exp $");
+IDENT(IdHdr,ID_JSBBASE);
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
@@ -77,8 +77,8 @@ CLASS IMPLEMENTATION
     char FGJSBBase::fgdef[6]    = {'\0' };
 #endif
 
-const double FGJSBBase::radtodeg = 57.29578;
-const double FGJSBBase::degtorad = 1.745329E-2;
+const double FGJSBBase::radtodeg = 57.295779513082320876798154814105;
+const double FGJSBBase::degtorad = 0.017453292519943295769236907684886;
 const double FGJSBBase::hptoftlbssec = 550.0;
 const double FGJSBBase::psftoinhg = 0.014138;
 const double FGJSBBase::psftopa = 47.88;
@@ -89,7 +89,9 @@ const double FGJSBBase::in3tom3 = 1.638706E-5;
 const double FGJSBBase::m3toft3 = 1.0/(fttom*fttom*fttom);
 const double FGJSBBase::inhgtopa = 3386.38;
 const double FGJSBBase::fttom = 0.3048;
-double FGJSBBase::Reng = 1716.0;
+double FGJSBBase::Reng = 1716.56;   // Gas constant for Air (ft-lb/slug-R)
+double FGJSBBase::Rstar = 1545.348; // Universal gas constant
+double FGJSBBase::Mair = 28.9645;   //
 const double FGJSBBase::SHRatio = 1.40;
 
 // Note that definition of lbtoslug by the inverse of slugtolb and not
@@ -104,7 +106,7 @@ const double FGJSBBase::kgtolb = 2.20462;
 const double FGJSBBase::kgtoslug = 0.06852168;
 
 const string FGJSBBase::needed_cfg_version = "2.0";
-const string FGJSBBase::JSBSim_version = "1.0 "__DATE__" "__TIME__;
+const string FGJSBBase::JSBSim_version = "1.0 " __DATE__ " " __TIME__ ;
 
 std::queue <FGJSBBase::Message> FGJSBBase::Messages;
 FGJSBBase::Message FGJSBBase::localMsg;
@@ -157,7 +159,7 @@ void FGJSBBase::PutMessage(const string& text, int iVal)
   msg.messageId = messageId++;
   msg.subsystem = "FDM";
   msg.type = Message::eInteger;
-  msg.bVal = (iVal != 0);
+  msg.iVal = iVal;
   Messages.push(msg);
 }
 
@@ -170,7 +172,7 @@ void FGJSBBase::PutMessage(const string& text, double dVal)
   msg.messageId = messageId++;
   msg.subsystem = "FDM";
   msg.type = Message::eDouble;
-  msg.bVal = (dVal != 0.0);
+  msg.dVal = dVal;
   Messages.push(msg);
 }
 
@@ -226,7 +228,8 @@ FGJSBBase::Message* FGJSBBase::ProcessNextMessage(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGJSBBase::disableHighLighting(void) {
+void FGJSBBase::disableHighLighting(void)
+{
   highint[0]='\0';
   halfint[0]='\0';
   normint[0]='\0';
@@ -280,5 +283,67 @@ double FGJSBBase::GaussianRandomNumber(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+double FGJSBBase::VcalibratedFromMach(double mach, double p, double psl, double rhosl)
+{
+  double pt,A;
+
+  if (mach < 0) mach=0;
+  if (mach < 1)    //calculate total pressure assuming isentropic flow
+    pt=p*pow((1 + 0.2*mach*mach),3.5);
+  else {
+    // shock in front of pitot tube, we'll assume its normal and use
+    // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
+    // pressure behind the shock to the static pressure in front of
+    // the normal shock assumption should not be a bad one -- most supersonic
+    // aircraft place the pitot probe out front so that it is the forward
+    // most point on the aircraft.  The real shock would, of course, take
+    // on something like the shape of a rounded-off cone but, here again,
+    // the assumption should be good since the opening of the pitot probe
+    // is very small and, therefore, the effects of the shock curvature
+    // should be small as well. AFAIK, this approach is fairly well accepted
+    // within the aerospace community
+
+    // The denominator below is zero for Mach ~ 0.38, for which
+    // we'll never be here, so we're safe
+
+    pt = p*166.92158*pow(mach,7.0)/pow(7*mach*mach-1,2.5);
+  }
+
+  A = pow(((pt-p)/psl+1),0.28571);
+  return sqrt(7*psl/rhosl*(A-1));
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGJSBBase::MachFromVcalibrated(double vcas, double p, double psl, double rhosl)
+{
+  double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1);
+
+  if (pt/p < 1.89293)
+    return sqrt(5.0*(pow(pt/p, 0.2857143) -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;
+  }
+}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 } // namespace JSBSim