Synchronized with JSBSim/CVS.

src/FDM/JSBSim/CMakeLists.txt

@@ -64,6 +64,7 @@ set(HEADERS
     models/atmosphere/FGWinds.h
     models/flight_control/FGAccelerometer.h
     models/flight_control/FGActuator.h
+    models/flight_control/FGAngles.h
     models/flight_control/FGDeadBand.h
     models/flight_control/FGFCSComponent.h
     models/flight_control/FGFCSFunction.h
@@ -77,6 +78,7 @@ set(HEADERS
     models/flight_control/FGSensorOrientation.h
     models/flight_control/FGSummer.h
     models/flight_control/FGSwitch.h
+    models/flight_control/FGWaypoint.h
     models/propulsion/FGElectric.h
     models/propulsion/FGEngine.h
     models/propulsion/FGForce.h
@@ -152,6 +154,7 @@ set(SOURCES
     models/atmosphere/FGWinds.cpp
     models/flight_control/FGAccelerometer.cpp
     models/flight_control/FGActuator.cpp
+    models/flight_control/FGAngles.cpp
     models/flight_control/FGDeadBand.cpp
     models/flight_control/FGFCSComponent.cpp
     models/flight_control/FGFCSFunction.cpp
@@ -164,6 +167,7 @@ set(SOURCES
     models/flight_control/FGSensor.cpp
     models/flight_control/FGSummer.cpp
     models/flight_control/FGSwitch.cpp
+    models/flight_control/FGWaypoint.cpp
     models/propulsion/FGElectric.cpp
     models/propulsion/FGEngine.cpp
     models/propulsion/FGForce.cpp

src/FDM/JSBSim/FGJSBBase.h

@@ -56,7 +56,7 @@ using std::max;
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_JSBBASE "$Id: FGJSBBase.h,v 1.34 2011/10/22 14:38:30 bcoconni Exp $"
+#define ID_JSBBASE "$Id: FGJSBBase.h,v 1.38 2013/06/20 13:01:48 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -72,7 +72,7 @@ CLASS DOCUMENTATION
 *   This class provides universal constants, utility functions, messaging
 *   functions, and enumerated constants to JSBSim.
     @author Jon S. Berndt
-    @version $Id: FGJSBBase.h,v 1.34 2011/10/22 14:38:30 bcoconni Exp $
+    @version $Id: FGJSBBase.h,v 1.38 2013/06/20 13:01:48 jberndt Exp $
 */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -256,6 +256,13 @@ public:
     return kelvin - 273.15;
   }
 
+  /** Converts from feet to meters
+  *   @param measure The length in feet.
+  *   @return The length in meters. */
+  static double FeetToMeters (double measure) {
+    return measure*0.3048;
+  }
+
   /** 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

src/FDM/JSBSim/input_output/FGOutputTextFile.cpp

@@ -63,7 +63,7 @@ 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 *IdSrc = "$Id: FGOutputTextFile.cpp,v 1.6 2013/09/11 12:51:13 jberndt Exp $";
 static const char *IdHdr = ID_OUTPUTTEXTFILE;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -75,6 +75,8 @@ bool FGOutputTextFile::Load(Element* el)
   if(!FGOutputFile::Load(el))
     return false;
 
+  PreLoad(el, PropertyManager);
+
   string type = el->GetAttributeValue("type");
   string delim;
   if (type == "TABULAR") {
@@ -238,6 +240,12 @@ bool FGOutputTextFile::OpenFile(void)
     }
   }
 
+  if (PreFunctions.size() > 0) {
+    for (unsigned int i=0;i<PreFunctions.size();i++) {
+      outstream << delimeter << PreFunctions[i]->GetName();
+    }
+  }
+
   outstream << endl;
   outstream.flush();
 
@@ -382,6 +390,9 @@ void FGOutputTextFile::Print(void)
   for (unsigned int i=0;i<OutputProperties.size();i++) {
     outstream << delimeter << OutputProperties[i]->getDoubleValue();
   }
+  for (unsigned int i=0;i<PreFunctions.size();i++) {
+    outstream << delimeter << PreFunctions[i]->getDoubleValue();
+  }
   outstream.precision(10);
 
   outstream << endl;

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.51 2013/06/10 01:50:43 jberndt Exp $";
+static const char *IdSrc = "$Id: FGScript.cpp,v 1.52 2013/09/11 12:46:35 jberndt Exp $";
 static const char *IdHdr = ID_FGSCRIPT;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -275,6 +275,9 @@ bool FGScript::LoadScript(string script, double deltaT, const string initfile)
 
     // Notify about when this event is triggered?
     if ((notify_element = event_element->FindElement("notify")) != 0) {
+      if (notify_element->HasAttribute("format")) {
+        if (notify_element->GetAttributeValue("format") == "kml") newEvent->NotifyKML = true;
+      }
       newEvent->Notify = true;
       // Check here for new <description> tag that gets echoed
       string notify_description = notify_element->FindElementValue("description");
@@ -284,23 +287,16 @@ bool FGScript::LoadScript(string script, double deltaT, const string initfile)
       notify_property_element = notify_element->FindElement("property");
       while (notify_property_element) {
         notifyPropertyName = notify_property_element->GetDataLine();
-        if (PropertyManager->GetNode(notifyPropertyName)) {
+
-          newEvent->NotifyProperties.push_back( PropertyManager->GetNode(notifyPropertyName) );
+        newEvent->NotifyPropertyNames.push_back(notifyPropertyName);
+        newEvent->NotifyProperties.push_back(0);
           string caption_attribute = notify_property_element->GetAttributeValue("caption");
           if (caption_attribute.empty()) {
             newEvent->DisplayString.push_back(notifyPropertyName);
           } else {
             newEvent->DisplayString.push_back(caption_attribute);
           }
-        } else {
+
-          cout << endl << fgred << "  Could not find the property named "
-               << notifyPropertyName << " in script" << endl << "  \""
-               << ScriptName << "\". Execution is aborted. Please recheck "
-               << "your input files and scripts." << reset << endl;
-          delete newEvent->Condition;
-          delete newEvent;
-          return false;
-        }
         notify_property_element = notify_element->FindNextElement("property");
       }
     }
@@ -313,7 +309,6 @@ bool FGScript::LoadScript(string script, double deltaT, const string initfile)
       newEvent->SetParam.push_back( PropertyManager->GetNode(prop_name) );
       } else {
         newEvent->SetParam.push_back( 0L );
-        cerr << "Property " << prop_name << " will be late-bound." << endl;
       }
       newEvent->SetParamName.push_back( prop_name );
 
@@ -477,15 +472,44 @@ bool FGScript::RunScript(void)
 
       // Print notification values after setting them
       if (thisEvent.Notify && !thisEvent.Notified) {
-        cout << endl << "  Event " << event_ctr << " (" << thisEvent.Name << ")"
+        if (thisEvent.NotifyKML) {
-             << " executed at time: " << currentTime << endl;
+          cout << endl << "<Placemark>" << endl;
+          cout << "  <name> " << currentTime << " seconds" << " </name>" << endl;
+          cout << "  <description>" << endl;
+          cout << "  <![CDATA[" << endl;
+          cout << "  <b>" << thisEvent.Name << " (Event " << event_ctr << ")" << " executed at time: " << currentTime << "</b><br/>" << endl;
+        } else  {
+          cout << endl << underon
+               << highint << thisEvent.Name << normint << underoff
+               << " (Event " << event_ctr << ")" 
+               << " executed at time: " << highint << currentTime << normint << endl;
+        }
         if (!thisEvent.Description.empty()) {
           cout << "    " << thisEvent.Description << endl;
         }
         for (j=0; j<thisEvent.NotifyProperties.size();j++) {
-//          cout << "    " << thisEvent.NotifyProperties[j]->GetRelativeName()
+          if (thisEvent.NotifyProperties[j] == 0) {
-          cout << "    " << thisEvent.DisplayString[j]
+            if (PropertyManager->HasNode(thisEvent.NotifyPropertyNames[j])) {
-               << " = " << thisEvent.NotifyProperties[j]->getDoubleValue() << endl;
+              thisEvent.NotifyProperties[j] = PropertyManager->GetNode(thisEvent.NotifyPropertyNames[j]);
+            } else {
+              throw("Could not find property named "+thisEvent.NotifyPropertyNames[j]+" in script.");
+            }
+          }
+          cout << "    " << thisEvent.DisplayString[j] << " = " << thisEvent.NotifyProperties[j]->getDoubleValue();
+          if (thisEvent.NotifyKML) cout << " <br/>";
+          cout << endl;
+        }
+        if (thisEvent.NotifyKML) {
+          cout << "  ]]>" << endl;
+          cout << "  </description>" << endl;
+          cout << "  <Point>" << endl;
+          cout << "    <altitudeMode> absolute </altitudeMode>" << endl;
+          cout << "    <extrude> 1 </extrude>" << endl;
+          cout << "    <coordinates>" << FDMExec->GetPropagate()->GetLongitudeDeg()
+            << "," << FDMExec->GetPropagate()->GetGeodLatitudeDeg()
+            << "," << FDMExec->GetPropagate()->GetAltitudeASLmeters() << "</coordinates>" << endl;
+          cout << "  </Point>" << endl;
+          cout << "</Placemark>" << endl;
         }
         cout << endl;
         thisEvent.Notified = true;
@@ -561,25 +585,37 @@ void FGScript::Debug(int from)
         for (unsigned j=0; j<Events[i].SetValue.size(); j++) {
           if (Events[i].SetValue[j] == 0.0 && Events[i].Functions[j] != 0L) {
             if (Events[i].SetParam[j] == 0) {
+              if (Events[i].SetParamName[j].size() == 0) {
               cerr << fgred << highint << endl
                    << "  An attempt has been made to access a non-existent property" << endl
                    << "  in this event. Please check the property names used, spelling, etc."
                    << reset << endl;
               exit(-1);
+              } else {
+                cout << endl << "      set " << Events[i].SetParamName[j]
+                     << " to function value (Late Bound)";
             }
+            } else {
             cout << endl << "      set " << Events[i].SetParam[j]->GetRelativeName("/fdm/jsbsim/")
                  << " to function value";
+            }
           } else {
             if (Events[i].SetParam[j] == 0) {
+              if (Events[i].SetParamName[j].size() == 0) {
               cerr << fgred << highint << endl
                    << "  An attempt has been made to access a non-existent property" << endl
                    << "  in this event. Please check the property names used, spelling, etc."
                    << reset << endl;
               exit(-1);
+              } else {
+                cout << endl << "      set " << Events[i].SetParamName[j]
+                     << " to function value (Late Bound)";
             }
+            } else {
             cout << endl << "      set " << Events[i].SetParam[j]->GetRelativeName("/fdm/jsbsim/")
                  << " to " << Events[i].SetValue[j];
           }
+          }
 
           switch (Events[i].Type[j]) {
           case FG_VALUE:
@@ -615,10 +651,14 @@ void FGScript::Debug(int from)
         // Print notifications
         if (Events[i].Notify) {
           if (Events[i].NotifyProperties.size() > 0) {
-            cout << "  Notifications" << ":" << endl << "    {" << endl;
+            if (Events[i].NotifyKML) {
-            for (unsigned j=0; j<Events[i].NotifyProperties.size();j++) {
+              cout << "  Notifications (KML Format):" << endl << "    {" << endl;
+            } else {
+              cout << "  Notifications:" << endl << "    {" << endl;
+            }
+            for (unsigned j=0; j<Events[i].NotifyPropertyNames.size();j++) {
               cout << "      "
-                   << Events[i].NotifyProperties[j]->GetRelativeName("/fdm/jsbsim/")
+                   << Events[i].NotifyPropertyNames[j]
                    << endl;
             }
             cout << "    }" << endl;

src/FDM/JSBSim/input_output/FGScript.h

@@ -49,7 +49,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_FGSCRIPT "$Id: FGScript.h,v 1.24 2013/06/10 01:50:43 jberndt Exp $"
+#define ID_FGSCRIPT "$Id: FGScript.h,v 1.25 2013/09/11 12:46:35 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -158,7 +158,7 @@ CLASS DOCUMENTATION
     comes the "run" section, where the conditions are
     described in &quot;event&quot; clauses.</p>
     @author Jon S. Berndt
-    @version "$Id: FGScript.h,v 1.24 2013/06/10 01:50:43 jberndt Exp $"
+    @version "$Id: FGScript.h,v 1.25 2013/09/11 12:46:35 jberndt Exp $"
 */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -214,6 +214,7 @@ private:
     bool             Continuous;
     bool             Triggered;
     bool             Notify;
+    bool             NotifyKML;
     bool             Notified;
     double           Delay;
     double           StartTime;
@@ -223,6 +224,7 @@ private:
     vector <FGPropertyNode_ptr>  SetParam;
     vector <std::string>  SetParamName;
     vector <FGPropertyNode_ptr>  NotifyProperties;
+    vector <string>              NotifyPropertyNames;
     vector <string>              DisplayString;
     vector <eAction> Action;
     vector <eType>   Type;
@@ -239,7 +241,7 @@ private:
       Persistent = false;
       Continuous = false;
       Delay = 0.0;
-      Notify = Notified = false;
+      Notify = Notified = NotifyKML = false;
       Name = "";
       StartTime = 0.0;
       TimeSpan = 0.0;

src/FDM/JSBSim/input_output/FGXMLElement.cpp

@@ -43,7 +43,7 @@ FORWARD DECLARATIONS
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGXMLElement.cpp,v 1.38 2012/12/13 04:41:06 jberndt Exp $";
+static const char *IdSrc = "$Id: FGXMLElement.cpp,v 1.39 2013/06/20 04:37:27 jberndt Exp $";
 static const char *IdHdr = ID_XMLELEMENT;
 
 bool Element::converterIsInitialized = false;
@@ -428,22 +428,27 @@ double Element::FindElementValueAsNumberConvertTo(const string& el, const string
   Element* element = FindElement(el);
 
   if (!element) {
-    cerr << "Attempting to get non-existent element " << el << endl;
+    throw("Attempting to get the value of a non-existent element "+el);
-    exit(0);
+//    cerr << "Attempting to get non-existent element " << el << endl;
+//    exit(0);
   }
 
   string supplied_units = element->GetAttributeValue("unit");
 
   if (!supplied_units.empty()) {
     if (convert.find(supplied_units) == convert.end()) {
-      cerr << endl << "Supplied unit: \"" << supplied_units << "\" does not exist (typo?). Add new unit"
+      throw("Supplied unit: \"" + supplied_units + "\" does not exist (typo?). Add new unit"
-           << " conversion in FGXMLElement.cpp." << endl;
+           + " conversion in FGXMLElement.cpp.");
-      exit(-1);
+//      cerr << endl << "Supplied unit: \"" << supplied_units << "\" does not exist (typo?). Add new unit"
+//           << " conversion in FGXMLElement.cpp." << endl;
+//      exit(-1);
     }
     if (convert[supplied_units].find(target_units) == convert[supplied_units].end()) {
-      cerr << endl << "Supplied unit: \"" << supplied_units << "\" cannot be converted to "
+      throw("Supplied unit: \"" + supplied_units + "\" cannot be converted to "
-                   << target_units << ". Add new unit conversion in FGXMLElement.cpp or fix typo" << endl;
+                   + target_units + ". Add new unit conversion in FGXMLElement.cpp or fix typo");
-      exit(-1);
+//      cerr << endl << "Supplied unit: \"" << supplied_units << "\" cannot be converted to "
+//                   << target_units << ". Add new unit conversion in FGXMLElement.cpp or fix typo" << endl;
+//      exit(-1);
     }
   }
 

src/FDM/JSBSim/math/FGFunction.cpp

@@ -43,7 +43,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGFunction.cpp,v 1.50 2013/06/10 02:05:12 jberndt Exp $";
+static const char *IdSrc = "$Id: FGFunction.cpp,v 1.51 2013/09/11 12:49:36 jberndt Exp $";
 static const char *IdHdr = ID_FUNCTION;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -807,14 +807,11 @@ void FGFunction::bind(void)
     }
 
     if (PropertyManager->HasNode(tmp)) {
-      FGPropertyNode* property = PropertyManager->GetNode(tmp);
-      if (property->isTied()) {
         cout << "Property " << tmp << " has already been successfully bound (late)." << endl;
-        return;
+    } else {
-      }
+    PropertyManager->Tie( tmp, this, &FGFunction::GetValue);
       }
 
-    PropertyManager->Tie( tmp, this, &FGFunction::GetValue);
   }
 }
 

src/FDM/JSBSim/math/FGFunction.h

@@ -43,7 +43,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_FUNCTION "$Id: FGFunction.h,v 1.30 2013/06/10 02:25:18 jberndt Exp $"
+#define ID_FUNCTION "$Id: FGFunction.h,v 1.31 2013/06/20 04:37:27 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS

src/FDM/JSBSim/models/FGAerodynamics.cpp

@@ -48,7 +48,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.47 2013/06/10 01:59:16 jberndt Exp $";
+static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.48 2013/09/11 12:42:14 jberndt Exp $";
 static const char *IdHdr = ID_AERODYNAMICS;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -77,6 +77,7 @@ FGAerodynamics::FGAerodynamics(FGFDMExec* FDMExec) : FGModel(FDMExec)
   axisType = atNone;
 
   AeroFunctions = new AeroFunctionArray[6];
+  AeroFunctionsAtCG = new AeroFunctionArray[6];
 
   impending_stall = stall_hyst = 0.0;
   alphaclmin = alphaclmax = 0.0;
@@ -101,8 +102,12 @@ FGAerodynamics::~FGAerodynamics()
   for (i=0; i<6; i++)
     for (j=0; j<AeroFunctions[i].size(); j++)
       delete AeroFunctions[i][j];
+  for (i=0; i<6; i++)
+    for (j=0; j<AeroFunctionsAtCG[i].size(); j++)
+      delete AeroFunctionsAtCG[i][j];
 
   delete[] AeroFunctions;
+  delete[] AeroFunctionsAtCG;
 
   delete AeroRPShift;
 
@@ -165,7 +170,9 @@ bool FGAerodynamics::Run(bool Holding)
   }
 
   vFw.InitMatrix();
+  vFwAtCG.InitMatrix();
   vFnative.InitMatrix();
+  vFnativeAtCG.InitMatrix();
 
   for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
     for (ctr=0; ctr < AeroFunctions[axis_ctr].size(); ctr++) {
@@ -173,6 +180,12 @@ bool FGAerodynamics::Run(bool Holding)
     }
   }
 
+  for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
+    for (ctr=0; ctr < AeroFunctionsAtCG[axis_ctr].size(); ctr++) {
+      vFnativeAtCG(axis_ctr+1) += AeroFunctionsAtCG[axis_ctr][ctr]->GetValue();
+    }
+  }
+
   // Note that we still need to convert to wind axes here, because it is
   // used in the L/D calculation, and we still may want to look at Lift
   // and Drag.
@@ -189,19 +202,32 @@ bool FGAerodynamics::Run(bool Holding)
   switch (axisType) {
     case atBodyXYZ:       // Forces already in body axes; no manipulation needed
       vFw = in.Tb2w*vFnative;
-      vFw(eDrag)*=-1; vFw(eLift)*=-1;
       vForces = vFnative;
+      vFw(eDrag)*=-1; vFw(eLift)*=-1;
+
+      vFwAtCG = in.Tb2w*vFnativeAtCG;
+      vForcesAtCG = vFnativeAtCG;
+      vFwAtCG(eDrag)*=-1; vFwAtCG(eLift)*=-1;
       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;
+
+      vFwAtCG = vFnativeAtCG;
+      vFwAtCG(eDrag)*=-1; vFwAtCG(eLift)*=-1;
+      vForcesAtCG = in.Tw2b*vFwAtCG;
+      vFwAtCG(eDrag)*=-1; vFwAtCG(eLift)*=-1;
       break;
     case atAxialNormal:   // Convert native forces into Axial|Normal|Side system
       vFw = in.Tb2w*vFnative;
       vFnative(eX)*=-1; vFnative(eZ)*=-1;
       vForces = vFnative;
+
+      vFwAtCG = in.Tb2w*vFnativeAtCG;
+      vFnativeAtCG(eX)*=-1; vFnativeAtCG(eZ)*=-1;
+      vForcesAtCG = vFnativeAtCG;
       break;
     default:
       cerr << endl << "  A proper axis type has NOT been selected. Check "
@@ -211,12 +237,13 @@ bool FGAerodynamics::Run(bool Holding)
 
   // Calculate lift coefficient squared
   if ( in.Qbar > 0) {
-    clsq = vFw(eLift) / (in.Wingarea*in.Qbar);
+    clsq = (vFw(eLift) + vFwAtCG(eLift))/ (in.Wingarea*in.Qbar);
     clsq *= clsq;
   }
 
   // Calculate lift Lift over Drag
-  if ( fabs(vFw(eDrag)) > 0.0) lod = fabs( vFw(eLift) / vFw(eDrag) );
+  if ( fabs(vFw(eDrag) + vFwAtCG(eDrag)) > 0.0)
+    lod = fabs( (vFw(eLift) + vFwAtCG(eLift))/ (vFw(eDrag) + vFwAtCG(eDrag)));
 
   // Calculate aerodynamic reference point shift, if any. The shift
   // takes place in the structual axis. That is, if the shift is positive,
@@ -237,6 +264,10 @@ bool FGAerodynamics::Run(bool Holding)
     }
   }
   vMoments = vMomentsMRC + vDXYZcg*vForces; // M = r X F
+  // Now add the "at CG" values to base forces - after the moments have been transferred
+  vForces += vForcesAtCG;
+  vFnative += vFnativeAtCG;
+  vFw += vFwAtCG;
 
   RunPostFunctions();
 
@@ -291,10 +322,16 @@ bool FGAerodynamics::Load(Element *element)
   axis_element = document->FindElement("axis");
   while (axis_element) {
     AeroFunctionArray ca;
+    AeroFunctionArray ca_atCG;
     axis = axis_element->GetAttributeValue("name");
     function_element = axis_element->FindElement("function");
     while (function_element) {
       string current_func_name = function_element->GetAttributeValue("name");
+      bool apply_at_cg = false;
+      if (function_element->HasAttribute("apply_at_cg")) {
+        if (function_element->GetAttributeValue("apply_at_cg") == "true") apply_at_cg = true;
+      }
+      if (!apply_at_cg) {
       try {
         ca.push_back( new FGFunction(PropertyManager, function_element) );
       } catch (string const str) {
@@ -302,9 +339,19 @@ bool FGAerodynamics::Load(Element *element)
              << current_func_name << ":" << str << " Aborting." << reset << endl;
         return false;
       }
+      } else {
+        try {
+          ca_atCG.push_back( new FGFunction(PropertyManager, function_element) );
+        } catch (string const str) {
+          cerr << endl << fgred << "Error loading aerodynamic function in " 
+               << current_func_name << ":" << str << " Aborting." << reset << endl;
+          return false;
+        }
+      }
       function_element = axis_element->FindNextElement("function");
     }
     AeroFunctions[AxisIdx[axis]] = ca;
+    AeroFunctionsAtCG[AxisIdx[axis]] = ca_atCG;
     axis_element = document->FindNextElement("axis");
   }
 

src/FDM/JSBSim/models/FGAerodynamics.h

@@ -52,7 +52,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_AERODYNAMICS "$Id: FGAerodynamics.h,v 1.26 2012/07/26 04:33:46 jberndt Exp $"
+#define ID_AERODYNAMICS "$Id: FGAerodynamics.h,v 1.27 2013/09/11 12:42:14 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.26 $
+    @version $Revision: 1.27 $
 */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -237,6 +237,10 @@ private:
   FGColumnVector3 vFnative;
   FGColumnVector3 vFw;
   FGColumnVector3 vForces;
+  AeroFunctionArray* AeroFunctionsAtCG;
+  FGColumnVector3 vFwAtCG;
+  FGColumnVector3 vFnativeAtCG;
+  FGColumnVector3 vForcesAtCG;
   FGColumnVector3 vMoments;
   FGColumnVector3 vMomentsMRC;
   FGColumnVector3 vDXYZcg;

src/FDM/JSBSim/models/FGAuxiliary.cpp

@@ -50,7 +50,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.61 2013/06/10 01:56:14 jberndt Exp $";
+static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.62 2013/09/11 12:43:20 jberndt Exp $";
 static const char *IdHdr = ID_AUXILIARY;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -216,7 +216,7 @@ bool FGAuxiliary::Run(bool Holding)
   }
 
   A = pow(((pt-in.Pressure)/in.PressureSL + 1),0.28571);
-  if (MachU > 0.0) {
+  if (abs(MachU) > 0.0) {
     vcas = sqrt(7 * in.PressureSL / in.DensitySL * (A-1));
     veas = sqrt(2 * qbar / in.DensitySL);
     vtrue = 1116.43559 * MachU * sqrt(in.Temperature / 518.67);

src/FDM/JSBSim/models/FGFCS.cpp

@@ -59,6 +59,8 @@ INCLUDES
 #include "models/flight_control/FGAccelerometer.h"
 #include "models/flight_control/FGMagnetometer.h"
 #include "models/flight_control/FGGyro.h"
+#include "models/flight_control/FGWaypoint.h"
+#include "models/flight_control/FGAngles.h"
 
 #include "FGFCSChannel.h"
 
@@ -66,7 +68,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGFCS.cpp,v 1.80 2013/01/26 17:06:49 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGFCS.cpp,v 1.81 2013/06/20 04:37:27 jberndt Exp $";
 static const char *IdHdr = ID_FCS;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -623,6 +625,12 @@ bool FGFCS::Load(Element* el, SystemType systype)
           newChannel->Add(new FGMagnetometer(this, component_element));
         } else if (component_element->GetName() == string("gyro")) {
           newChannel->Add(new FGGyro(this, component_element));
+        } else if ((component_element->GetName() == string("waypoint_heading")) ||
+                   (component_element->GetName() == string("waypoint_distance")))
+        {
+          newChannel->Add(new FGWaypoint(this, component_element));
+        } else if (component_element->GetName() == string("angle")) {
+          newChannel->Add(new FGAngles(this, component_element));
         } else {
           cerr << "Unknown FCS component: " << component_element->GetName() << endl;
         }

src/FDM/JSBSim/models/FGFCS.h

@@ -51,7 +51,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_FCS "$Id: FGFCS.h,v 1.41 2012/10/15 05:02:29 jberndt Exp $"
+#define ID_FCS "$Id: FGFCS.h,v 1.42 2013/06/20 04:37:27 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -169,7 +169,7 @@ CLASS DOCUMENTATION
     @property gear/tailhook-pos-norm
 
     @author Jon S. Berndt
-    @version $Revision: 1.41 $
+    @version $Revision: 1.42 $
     @see FGActuator
     @see FGDeadBand
     @see FGFCSFunction
@@ -180,6 +180,8 @@ CLASS DOCUMENTATION
     @see FGSensor
     @see FGSummer
     @see FGSwitch
+    @see FGWaypoint
+    @see FGAngles
     @see FGFCSComponent
     @see Element
 */

src/FDM/JSBSim/models/FGOutput.cpp

@@ -49,7 +49,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGOutput.cpp,v 1.72 2013/01/26 17:06:50 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGOutput.cpp,v 1.73 2013/09/11 12:44:02 jberndt Exp $";
 static const char *IdHdr = ID_OUTPUT;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -212,7 +212,9 @@ bool FGOutput::Load(int subSystems, std::string protocol, std::string type,
   unsigned int idx = OutputTypes.size();
   FGOutputType* Output = 0;
 
-  if (debug_lvl > 0) cout << endl << "  Output data set: " << idx << "  ";
+  if (debug_lvl > 0) cout << endl << "  Output data set: " << idx << endl;
+
+  type = to_upper(type);
 
   if (type == "CSV") {
     FGOutputTextFile* OutputTextFile = new FGOutputTextFile(FDMExec);
@@ -258,7 +260,9 @@ bool FGOutput::Load(Element* document)
   string type = document->GetAttributeValue("type");
   FGOutputType* Output = 0;
 
-  if (debug_lvl > 0) cout << endl << "  Output data set: " << idx << "  ";
+  if (debug_lvl > 0) cout << endl << "  Output data set: " << idx << "  " << endl;
+
+  type = to_upper(type);
 
   if (type == "CSV") {
     Output = new FGOutputTextFile(FDMExec);

src/FDM/JSBSim/models/FGPropagate.cpp

@@ -77,7 +77,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.112 2013/06/10 01:57:52 jberndt Exp $";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.115 2013/09/14 11:26:02 bcoconni Exp $";
 static const char *IdHdr = ID_PROPAGATE;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -509,6 +509,13 @@ double FGPropagate::GetDistanceAGL(void) const
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+double FGPropagate::GetDistanceAGLKm(void) const
+{
+  return VState.vLocation.GetAltitudeAGL(FDMExec->GetSimTime())*0.0003048;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGPropagate::SetDistanceAGL(double tt)
 {
   VState.vLocation.SetAltitudeAGL(tt, FDMExec->GetSimTime());
@@ -517,6 +524,14 @@ void FGPropagate::SetDistanceAGL(double tt)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+void FGPropagate::SetDistanceAGLKm(double tt)
+{
+  VState.vLocation.SetAltitudeAGL(tt*3280.8399, FDMExec->GetSimTime());
+  UpdateVehicleState();
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGPropagate::SetVState(const VehicleState& vstate)
 {
   //ToDo: Shouldn't all of these be set from the vstate vector passed in?
@@ -577,9 +592,9 @@ void FGPropagate::DumpState(void)
        <<   "Position" << underoff << endl;
   cout << "    ECI:   " << VState.vInertialPosition.Dump(", ") << " (x,y,z, in ft)" << endl;
   cout << "    ECEF:  " << VState.vLocation << " (x,y,z, in ft)"  << endl;
-  cout << "    Local: " << VState.vLocation.GetLatitudeDeg()
+  cout << "    Local: " << VState.vLocation.GetGeodLatitudeDeg()
                         << ", " << VState.vLocation.GetLongitudeDeg()
-                        << ", " << GetAltitudeASL() << " (lat, lon, alt in deg and ft)" << endl;
+                        << ", " << GetAltitudeASL() << " (geodetic lat, lon, alt ASL in deg and ft)" << endl;
 
   cout << endl << "  " << underon
        <<   "Orientation" << underoff << endl;
@@ -613,6 +628,8 @@ void FGPropagate::WriteStateFile(int num)
   ofstream outfile(filename.c_str());
 
   if (outfile.is_open()) {
+    switch(num) {
+    case 1:
     outfile << "<?xml version=\"1.0\"?>" << endl;
     outfile << "<initialize name=\"reset00\">" << endl;
     outfile << "  <ubody unit=\"FT/SEC\"> " << VState.vUVW(eU) << " </ubody> " << endl;
@@ -626,6 +643,41 @@ void FGPropagate::WriteStateFile(int num)
     outfile << "  <altitude unit=\"FT\"> " << GetDistanceAGL() << " </altitude>" << endl;
     outfile << "</initialize>" << endl;
     outfile.close();
+    break;
+    case 2:
+      outfile << "<?xml version=\"1.0\"?>" << endl;
+      outfile << "<initialize name=\"IC File\" version=\"2.0\">" << endl;
+      outfile << "" << endl;
+      outfile << "  <position frame=\"ECEF\">" << endl;
+      outfile << "    <latitude unit=\"DEG\" type=\"geodetic\"> " << VState.vLocation.GetGeodLatitudeDeg() << " </latitude>" << endl;
+      outfile << "    <longitude unit=\"DEG\"> " << VState.vLocation.GetLongitudeDeg() << " </longitude>" << endl;
+      outfile << "    <altitudeMSL unit=\"FT\"> " << GetAltitudeASL() << " </altitudeMSL>" << endl;
+      outfile << "  </position>" << endl;
+      outfile << "" << endl;
+      outfile << "  <orientation unit=\"DEG\" frame=\"LOCAL\">" << endl;
+      outfile << "    <yaw> " << VState.qAttitudeLocal.GetEulerDeg(eYaw) << " </yaw>" << endl;
+      outfile << "    <pitch> " << VState.qAttitudeLocal.GetEulerDeg(ePitch) << " </pitch>" << endl;
+      outfile << "    <roll> " << VState.qAttitudeLocal.GetEulerDeg(eRoll) << " </roll>" << endl;
+      outfile << "  </orientation>" << endl;
+      outfile << "" << endl;
+      outfile << "  <velocity unit=\"FT/SEC\" frame=\"LOCAL\">" << endl;
+      outfile << "    <x> " << GetVel(eNorth) << " </x>" << endl;
+      outfile << "    <y> " << GetVel(eEast) << " </y>" << endl;
+      outfile << "    <z> " << GetVel(eDown) << " </z>" << endl;
+      outfile << "  </velocity>" << endl;
+      outfile << "" << endl;
+      outfile << "  <attitude_rate unit=\"DEG/SEC\" frame=\"BODY\">" << endl;
+      outfile << "    <roll> " << (VState.vPQR*radtodeg)(eRoll) << " </roll>" << endl;
+      outfile << "    <pitch> " << (VState.vPQR*radtodeg)(ePitch) << " </pitch>" << endl;
+      outfile << "    <yaw> " << (VState.vPQR*radtodeg)(eYaw) << " </yaw>" << endl;
+      outfile << "  </attitude_rate>" << endl;
+      outfile << "" << endl;
+      outfile << "</initialize>" << endl;
+      outfile.close();
+    break;
+    default:
+      throw("When writing a state file, the supplied value must be 1 or 2 for the version number of teh resulting IC file");
+    }
   } else {
     cerr << "Could not open and/or write the state to the initial conditions file: " << filename << endl;
   }
@@ -657,6 +709,7 @@ void FGPropagate::bind(void)
   PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 
   PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
+  PropertyManager->Tie("velocities/ned-velocity-mag-fps", this, &FGPropagate::GetNEDVelocityMagnitude);
 
   PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
   PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
@@ -668,6 +721,8 @@ void FGPropagate::bind(void)
   PropertyManager->Tie("position/lat-geod-deg", this, &FGPropagate::GetGeodLatitudeDeg);
   PropertyManager->Tie("position/geod-alt-ft", this, &FGPropagate::GetGeodeticAltitude);
   PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
+  PropertyManager->Tie("position/geod-alt-km", this, &FGPropagate::GetGeodeticAltitudeKm);
+  PropertyManager->Tie("position/h-agl-km", this,  &FGPropagate::GetDistanceAGLKm, &FGPropagate::SetDistanceAGLKm);
   PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
   PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
                           &FGPropagate::GetTerrainElevation,

src/FDM/JSBSim/models/FGPropagate.h

@@ -49,7 +49,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.75 2013/06/10 01:58:01 jberndt Exp $"
+#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.78 2013/09/14 11:26:04 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.75 2013/06/10 01:58:01 jberndt Exp $
+    @version $Id: FGPropagate.h,v 1.78 2013/09/14 11:26:04 bcoconni Exp $
   */
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -294,6 +294,10 @@ public:
   */
   double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); }
 
+  /** Retrieves the total local NED velocity in ft/sec.
+  */
+  double GetNEDVelocityMagnitude(void) const { return VState.vUVW.Magnitude(); }
+
   /** Retrieves the inertial velocity vector in ft/sec.
   */
   const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; }
@@ -419,6 +423,7 @@ public:
 
   double GetTerrainElevation(void) const { return GetLocalTerrainRadius() - VState.vLocation.GetSeaLevelRadius(); }
   double GetDistanceAGL(void)  const;
+  double GetDistanceAGLKm(void)  const;
   double GetRadius(void) const {
       if (VState.vLocation.GetRadius() == 0) return 1.0;
       else return VState.vLocation.GetRadius();
@@ -430,6 +435,7 @@ public:
   double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); }
 
   double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); }
+  double GetGeodeticAltitudeKm(void) const { return VState.vLocation.GetGeodAltitude()*0.0003048; }
 
   double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); }
   double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); }
@@ -551,6 +557,7 @@ public:
   void SetSeaLevelRadius(double tt);
   void SetTerrainElevation(double tt);
   void SetDistanceAGL(double tt);
+  void SetDistanceAGLKm(double tt);
 
   void SetInitialState(const FGInitialCondition *);
   void SetLocation(const FGLocation& l);

src/FDM/JSBSim/models/FGPropulsion.cpp

@@ -66,7 +66,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGPropulsion.cpp,v 1.69 2012/12/12 06:19:57 jberndt Exp $";
+static const char *IdSrc = "$Id: FGPropulsion.cpp,v 1.70 2013/09/11 23:24:49 jentron Exp $";
 static const char *IdHdr = ID_PROPULSION;
 
 extern short debug_lvl;
@@ -693,18 +693,32 @@ void FGPropulsion::SetStarter(int setting)
 
 void FGPropulsion::SetCutoff(int setting)
 {
+  bool bsetting = setting == 0 ? false : true;
+
   if (ActiveEngine < 0) {
     for (unsigned i=0; i<Engines.size(); i++) {
-      if (setting == 0)
+      switch (Engines[i]->GetType()) { 
-        ((FGTurbine*)Engines[i])->SetCutoff(false);
+        case FGEngine::etTurbine:
-      else
+          ((FGTurbine*)Engines[i])->SetCutoff(bsetting);
-        ((FGTurbine*)Engines[i])->SetCutoff(true);
+          break;
+        case FGEngine::etTurboprop:
+          ((FGTurboProp*)Engines[i])->SetCutoff(bsetting);
+          break;
+        default:
+          break;
+      }
     }
   } else {
-    if (setting == 0)
+    switch (Engines[ActiveEngine]->GetType()) { 
-      ((FGTurbine*)Engines[ActiveEngine])->SetCutoff(false);
+      case FGEngine::etTurbine:
-    else
+        ((FGTurbine*)Engines[ActiveEngine])->SetCutoff(bsetting);
-      ((FGTurbine*)Engines[ActiveEngine])->SetCutoff(true);
+        break;
+      case FGEngine::etTurboprop:
+        ((FGTurboProp*)Engines[ActiveEngine])->SetCutoff(bsetting);
+        break;
+      default:
+        break;
+    }
   }
 }
 
@@ -799,7 +813,7 @@ void FGPropulsion::bind(void)
 
   IsBound = true;
   PropertyManager->Tie("propulsion/set-running", this, (iPMF)0, &FGPropulsion::InitRunning, false);
-  if (HaveTurbineEngine) {
+  if (HaveTurbineEngine || HaveTurboPropEngine) {
     PropertyManager->Tie("propulsion/starter_cmd", this, (iPMF)0, &FGPropulsion::SetStarter,  false);
     PropertyManager->Tie("propulsion/cutoff_cmd", this,  (iPMF)0, &FGPropulsion::SetCutoff,   false);
   }

src/FDM/JSBSim/models/flight_control/FGAngles.cpp

@@ -0,0 +1,205 @@
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ Module:       FGAngles.cpp
+ Author:       Jon S. Berndt
+ Date started: 6/2013
+
+ ------------- Copyright (C) 2013 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.
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+Created: 6/2013 Jon S. Berndt
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+COMMENTS, REFERENCES,  and NOTES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+  The Included Angle to Heading algorithm is used to find the smallest included angle
+  (the angle less than or equal to 180 degrees) to a specified heading from
+  the current heading. The sense of the rotation to get to that angle is also
+  calculated (positive 1 for a clockwise rotation, negative 1 for counter-
+  clockwise).
+  
+  The angle to the heading is calculated as follows:
+
+  Given an angle phi:
+
+  V = cos(phi)i + sin(phi)j     (this is a unit vector)
+
+  The dot product for two, 2D vectors is written:
+
+  V1*V2 = |V1||V2|cos(phi)
+
+  Since the magnitude of a unit vector is 1, we can write the equation as follows:
+
+  V1*V2 = cos(phi)
+
+  or,
+
+  phi  = acos(V1*V2)
+
+  or,
+
+  phi  = acos[ cos(phi1)cos(phi2) + sin(phi1)sin(phi2) ]
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGAngles.h"
+#include "input_output/FGXMLElement.h"
+#include "input_output/FGPropertyManager.h"
+
+using namespace std;
+
+namespace JSBSim {
+
+static const char *IdSrc = "$Id: FGAngles.cpp,v 1.1 2013/06/20 04:37:28 jberndt Exp $";
+static const char *IdHdr = ID_ANGLES;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGAngles::FGAngles(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
+{
+  source_angle = 0.0;
+  target_angle = 0.0;
+  source_angle_unit = 1.0;
+  target_angle_unit = 1.0;
+  output_unit = 1.0;
+
+  if (element->FindElement("target_angle") ) {
+    target_angle_pNode = PropertyManager->GetNode(element->FindElementValue("target_angle"));
+    if (element->FindElement("target_angle")->HasAttribute("unit")) {
+      if (element->FindElement("target_angle")->GetAttributeValue("unit") == "DEG") {
+        target_angle_unit = 0.017453293;
+      }
+    }
+  } else {
+    throw("Target angle is required for component: "+Name);
+  }
+
+  if (element->FindElement("source_angle") ) {
+    source_angle_pNode = PropertyManager->GetNode(element->FindElementValue("source_angle"));
+    if (element->FindElement("source_angle")->HasAttribute("unit")) {
+      if (element->FindElement("source_angle")->GetAttributeValue("unit") == "DEG") {
+        source_angle_unit = 0.017453293;
+      }
+    }
+  } else {
+    throw("Source latitude is required for Angles component: "+Name);
+  }
+
+  unit = element->GetAttributeValue("unit");
+  if (!unit.empty()) {
+    if      (unit == "DEG") output_unit = 180.0/M_PI;
+    else if (unit == "RAD") output_unit = 1.0;
+    else throw("Unknown unit "+unit+" in angle component, "+Name);
+  } else {
+    output_unit = 1.0; // Default is radians (1.0) if unspecified
+  }
+
+  FGFCSComponent::bind();
+  Debug(0);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGAngles::~FGAngles()
+{
+  Debug(1);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+bool FGAngles::Run(void )
+{
+  source_angle = source_angle_pNode->getDoubleValue() * source_angle_unit;
+  target_angle = target_angle_pNode->getDoubleValue() * target_angle_unit;
+
+  double x1 = cos(source_angle);
+  double y1 = sin(source_angle);
+  double x2 = cos(target_angle);
+  double y2 = sin(target_angle);
+
+  double angle_to_heading_rad = acos(x1*x2 + y1*y2);
+  double x1y2 = x1*y2;
+  double x2y1 = x2*y1;
+
+  if (x1y2 >= x2y1) Output =  angle_to_heading_rad * output_unit;
+  else              Output = -angle_to_heading_rad * output_unit;
+
+  Clip();
+  if (IsOutput) SetOutput();
+
+  return true;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+//    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 FGAngles::Debug(int from)
+{
+  if (debug_lvl <= 0) return;
+
+  if (debug_lvl & 1) { // Standard console startup message output
+    if (from == 0) { // Constructor
+    }
+  }
+  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
+    if (from == 0) cout << "Instantiated: FGAngles" << endl;
+    if (from == 1) cout << "Destroyed:    FGAngles" << 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;
+    }
+  }
+}
+}

src/FDM/JSBSim/models/flight_control/FGAngles.h

@@ -0,0 +1,108 @@
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ Header:       FGAngles.h
+ Author:       Jon Berndt
+ Date started: 6/2013
+
+ ------------- Copyright (C) 2013 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
+--------------------------------------------------------------------------------
+Created: 6/2013 Jon S. Berndt
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGANGLES_H
+#define FGANGLES_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include <string>
+#include "FGFCSComponent.h"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_ANGLES "$Id: FGAngles.h,v 1.1 2013/06/20 04:37:28 jberndt Exp $"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+FORWARD DECLARATIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+namespace JSBSim {
+
+class FGFCS;
+class Element;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DOCUMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+/** Provides a way to determine the smallest included angle.
+
+    @code
+    <angle name="component_name" unit="DEG|RAD">
+      <source_angle unit="DEG|RAD">  property_name </source_angle>
+      <target_angle unit="DEG|RAD">  property_name </target_angle>
+      [<clipto>
+        <min> {[-]property name | value} </min>
+        <max> {[-]property name | value} </max>
+      </clipto>]
+      [<output> {property} </output>]
+    </angle>
+    @endcode
+
+    @author Jon S. Berndt
+    @version $Id: FGAngles.h,v 1.1 2013/06/20 04:37:28 jberndt Exp $
+*/
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DECLARATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGAngles  : public FGFCSComponent
+{
+public:
+  FGAngles(FGFCS* fcs, Element* element);
+  ~FGAngles();
+
+  bool Run(void);
+
+private:
+  FGPropertyNode_ptr target_angle_pNode;
+  FGPropertyNode_ptr source_angle_pNode;
+  double target_angle;
+  double source_angle;
+  double target_angle_unit;
+  double source_angle_unit;
+  double output_unit;
+  string unit;
+
+  void Debug(int from);
+};
+}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+#endif

src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp

@@ -48,7 +48,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.35 2013/01/26 17:06:50 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.36 2013/06/20 04:37:28 jberndt Exp $";
 static const char *IdHdr = ID_FCSCOMPONENT;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -106,6 +106,12 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
     Type = "GYRO";
   } else if (element->GetName() == string("actuator")) {
     Type = "ACTUATOR";
+  } else if (element->GetName() == string("waypoint_heading")) {
+    Type = "WAYPOINT_HEADING";
+  } else if (element->GetName() == string("waypoint_distance")) {
+    Type = "WAYPOINT_DISTANCE";
+  } else if (element->GetName() == string("angle")) {
+    Type = "ANGLE";
   } else { // illegal component in this channel
     Type = "UNKNOWN";
   }

src/FDM/JSBSim/models/flight_control/FGFCSComponent.h

@@ -4,7 +4,7 @@
  Author:       Jon S. Berndt
  Date started: 05/01/2000
 
- ------------- Copyright (C)  -------------
+ ------------- Copyright (C) 2000 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
@@ -46,7 +46,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_FCSCOMPONENT "$Id: FGFCSComponent.h,v 1.21 2013/01/26 17:06:50 bcoconni Exp $"
+#define ID_FCSCOMPONENT "$Id: FGFCSComponent.h,v 1.22 2013/06/20 04:37:28 jberndt Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -78,9 +78,11 @@ CLASS DOCUMENTATION
     - FGAccelerometer
     - FGGyro
     - FGActuator
+    - FGWaypoint
+    - FGAngle
 
     @author Jon S. Berndt
-    @version $Id: FGFCSComponent.h,v 1.21 2013/01/26 17:06:50 bcoconni Exp $
+    @version $Id: FGFCSComponent.h,v 1.22 2013/06/20 04:37:28 jberndt Exp $
     @see Documentation for the FGFCS class, and for the configuration file class
 */
 

src/FDM/JSBSim/models/flight_control/FGWaypoint.cpp

@@ -0,0 +1,261 @@
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ Module:       FGWaypoint.cpp
+ Author:       Jon S. Berndt
+ Date started: 6/2013
+
+ ------------- Copyright (C) 2013 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.
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+COMMENTS, REFERENCES,  and NOTES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGWaypoint.h"
+#include "input_output/FGXMLElement.h"
+#include "input_output/FGPropertyManager.h"
+
+using namespace std;
+
+namespace JSBSim {
+
+static const char *IdSrc = "$Id: FGWaypoint.cpp,v 1.2 2013/08/30 04:44:59 jberndt Exp $";
+static const char *IdHdr = ID_WAYPOINT;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGWaypoint::FGWaypoint(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
+{
+  if      (Type == "WAYPOINT_HEADING")  WaypointType = eHeading;
+  else if (Type == "WAYPOINT_DISTANCE") WaypointType = eDistance;
+
+  target_latitude_unit = 1.0;
+  target_longitude_unit = 1.0;
+  source_latitude_unit = 1.0;
+  source_longitude_unit = 1.0;
+
+  if (element->FindElement("target_latitude") ) {
+    target_latitude_pNode = PropertyManager->GetNode(element->FindElementValue("target_latitude"));
+    if (element->FindElement("target_latitude")->HasAttribute("unit")) {
+      if (element->FindElement("target_latitude")->GetAttributeValue("unit") == "DEG") {
+        target_latitude_unit = 0.017453293;
+      }
+    }
+  } else {
+    throw("Target latitude is required for waypoint component: "+Name);
+  }
+
+  if (element->FindElement("target_longitude") ) {
+    target_longitude_pNode = PropertyManager->GetNode(element->FindElementValue("target_longitude"));
+    if (element->FindElement("target_longitude")->HasAttribute("unit")) {
+      if (element->FindElement("target_longitude")->GetAttributeValue("unit") == "DEG") {
+        target_longitude_unit = 0.017453293;
+      }
+    }
+  } else {
+    throw("Target longitude is required for waypoint component: "+Name);
+  }
+
+  if (element->FindElement("source_latitude") ) {
+    source_latitude_pNode = PropertyManager->GetNode(element->FindElementValue("source_latitude"));
+    if (element->FindElement("source_latitude")->HasAttribute("unit")) {
+      if (element->FindElement("source_latitude")->GetAttributeValue("unit") == "DEG") {
+        source_latitude_unit = 0.017453293;
+      }
+    }
+  } else {
+    throw("Source latitude is required for waypoint component: "+Name);
+  }
+
+  if (element->FindElement("source_longitude") ) {
+    source_longitude_pNode = PropertyManager->GetNode(element->FindElementValue("source_longitude"));
+    if (element->FindElement("source_longitude")->HasAttribute("unit")) {
+      if (element->FindElement("source_longitude")->GetAttributeValue("unit") == "DEG") {
+        source_longitude_unit = 0.017453293;
+      }
+    }
+  } else {
+    throw("Source longitude is required for waypoint component: "+Name);
+  }
+
+  if (element->FindElement("radius")) {
+    radius = element->FindElementValueAsNumberConvertTo("radius", "FT");
+  } else {
+    radius = 21144000; // Radius of Earth in feet.
+  }
+
+  unit = element->GetAttributeValue("unit");
+  if (WaypointType == eHeading) {
+    if (!unit.empty()) {
+    if      (unit == "DEG") eUnit = eDeg;
+    else if (unit == "RAD") eUnit = eRad;
+    else throw("Unknown unit "+unit+" in HEADING waypoint component, "+Name);
+  } else {
+      eUnit = eRad; // Default is radians if unspecified
+    }
+  } else {
+    if (!unit.empty()) {
+    if      (unit == "FT") eUnit = eFeet;
+    else if (unit == "M")  eUnit = eMeters;
+    else throw("Unknown unit "+unit+" in DISTANCE waypoint component, "+Name);
+    } else {
+      eUnit = eFeet; // Default is feet if unspecified
+    }
+  }
+
+  FGFCSComponent::bind();
+  Debug(0);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGWaypoint::~FGWaypoint()
+{
+  Debug(1);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+//  The calculations, below, implement the Haversine formulas to calculate
+//  heading and distance to a set of lat/long coordinates from the current
+//  position. The latitude and longitude are expected to be in radian units
+//  and are measured from the 0 meridian and the equator, with positive 
+//  longitude being east from there, and positive latitude being north.
+//
+//  The basic equations are (lat1, long1 are source positions; lat2
+//  long2 are target positions):
+//
+//  R = earth’s radius
+//  Δlat = lat2 − lat1
+//  Δlong = long2 − long1
+//
+//  For the heading angle calculation:
+//
+//  θ = atan2(sin(Δlong)∙cos(lat2), cos(lat1)∙sin(lat2) − sin(lat1) ∙cos(lat2)∙cos(Δlong) )
+//
+//  For the waypoint distance calculation:
+//
+//  a = sin²(Δlat/2) + cos(lat1)∙cos(lat2)∙sin²(Δlong/2)
+//  c = 2∙atan2(√a, √(1−a))
+//  d = R∙c
+
+bool FGWaypoint::Run(void )
+{
+  target_latitude = target_latitude_pNode->getDoubleValue() * target_latitude_unit;
+  target_longitude = target_longitude_pNode->getDoubleValue() * target_longitude_unit;
+  source_latitude = source_latitude_pNode->getDoubleValue() * source_latitude_unit;
+  source_longitude = source_longitude_pNode->getDoubleValue() * source_longitude_unit;
+
+  double delta_lat_rad = target_latitude  - source_latitude;
+  double delta_lon_rad = target_longitude - source_longitude;
+
+  if (WaypointType == eHeading) {     // Calculate Heading
+
+    double Y = sin(delta_lon_rad) * cos(target_latitude);
+    double X = (cos(source_latitude) * sin(target_latitude))
+               - (sin(source_latitude) * cos(target_latitude) * cos(delta_lon_rad));
+
+    double heading_to_waypoint_rad = atan2(Y, X);
+    if (heading_to_waypoint_rad < 0) heading_to_waypoint_rad += 2.0*M_PI;
+
+    double heading_to_waypoint = 0;
+    if (eUnit == eDeg) heading_to_waypoint = heading_to_waypoint_rad * radtodeg;
+    else               heading_to_waypoint = heading_to_waypoint_rad;
+
+    Output = heading_to_waypoint;
+
+  } else {                            // Calculate Distance
+
+    double distance_a = pow(sin(delta_lat_rad/2.0), 2.0)
+                        + (cos(source_latitude) * cos(target_latitude)
+                          * (pow(sin(delta_lon_rad/2.0), 2.0)));
+
+    double wp_distance = 2.0 * radius * atan2(pow(distance_a, 0.5), pow((1.0 - distance_a), 0.5));
+
+    if (eUnit == eMeters) {
+      Output = FeetToMeters(wp_distance);
+    } else {
+      Output = wp_distance;
+    }
+  }
+
+  Clip();
+  if (IsOutput) SetOutput();
+
+  return true;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+//    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 FGWaypoint::Debug(int from)
+{
+  if (debug_lvl <= 0) return;
+
+  if (debug_lvl & 1) { // Standard console startup message output
+    if (from == 0) { // Constructor
+    }
+  }
+  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
+    if (from == 0) cout << "Instantiated: FGWaypoint" << endl;
+    if (from == 1) cout << "Destroyed:    FGWaypoint" << 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;
+    }
+  }
+}
+}

src/FDM/JSBSim/models/flight_control/FGWaypoint.h

@@ -0,0 +1,133 @@
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ Header:       FGWaypoint.h
+ Author:       Jon Berndt
+ Date started: 6/2013
+
+ ------------- Copyright (C) 2013 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
+--------------------------------------------------------------------------------
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGWAYPOINT_H
+#define FGWAYPOINT_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include <string>
+#include "FGFCSComponent.h"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_WAYPOINT "$Id: FGWaypoint.h,v 1.1 2013/06/20 04:37:28 jberndt Exp $"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+FORWARD DECLARATIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+namespace JSBSim {
+
+class FGFCS;
+class Element;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DOCUMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+/** Models a Waypoint object. 
+    The waypoint_heading component returns the heading to a specified waypoint
+    lat/long from another specified point.
+    The waypoint_distance component returns the distance between
+
+    @code
+    <waypoint_heading name="component_name" unit="DEG|RAD">
+      <target_latitude unit="DEG|RAD">  property_name </target_latitude>
+      <target_longitude unit="DEG|RAD"> property_name </target_longitude>
+      <source_latitude unit="DEG|RAD">  property_name </source_latitude>
+      <source_longitude unit="DEG|RAD"> property_name </source_longitude>
+      [<clipto>
+        <min> {[-]property name | value} </min>
+        <max> {[-]property name | value} </max>
+      </clipto>]
+      [<output> {property} </output>]
+    </waypoint_heading>
+
+    <waypoint_distance name="component_name" unit="FT|M">
+      <target_latitude unit="DEG|RAD">  property_name </target_latitude>
+      <target_longitude unit="DEG|RAD"> property_name </target_longitude>
+      <source_latitude unit="DEG|RAD">  property_name </source_latitude>
+      <source_longitude unit="DEG|RAD"> property_name </source_longitude>
+      [<radius> {value} </radius>]
+      [<clipto>
+        <min> {[-]property name | value} </min>
+        <max> {[-]property name | value} </max>
+      </clipto>]
+      [<output> {property} </output>]
+    </waypoint_distance>
+    @endcode
+
+    @author Jon S. Berndt
+    @version $Id: FGWaypoint.h,v 1.1 2013/06/20 04:37:28 jberndt Exp $
+*/
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DECLARATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGWaypoint  : public FGFCSComponent
+{
+public:
+  FGWaypoint(FGFCS* fcs, Element* element);
+  ~FGWaypoint();
+
+  bool Run(void);
+
+private:
+  FGPropertyNode_ptr target_latitude_pNode;
+  FGPropertyNode_ptr target_longitude_pNode;
+  FGPropertyNode_ptr source_latitude_pNode;
+  FGPropertyNode_ptr source_longitude_pNode;
+  double target_latitude;
+  double target_longitude;
+  double source_latitude;
+  double source_longitude;
+  double target_latitude_unit;
+  double target_longitude_unit;
+  double source_latitude_unit;
+  double source_longitude_unit;
+  double radius;
+  string unit;
+  enum {eNone=0, eDeg, eRad, eFeet, eMeters} eUnit;
+  enum {eNoType=0, eHeading, eDistance} WaypointType;
+
+  void Debug(int from);
+};
+}
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+#endif