From 22d91528f87011b1f0f326f27ad4070c36137b96 Mon Sep 17 00:00:00 2001 From: Anders Gidenstam Date: Sun, 22 Sep 2013 17:56:58 +0200 Subject: [PATCH] Synchronized with JSBSim/CVS. --- src/FDM/JSBSim/CMakeLists.txt | 4 + src/FDM/JSBSim/FGJSBBase.h | 11 +- .../JSBSim/input_output/FGOutputTextFile.cpp | 13 +- src/FDM/JSBSim/input_output/FGScript.cpp | 82 ++++-- src/FDM/JSBSim/input_output/FGScript.h | 8 +- src/FDM/JSBSim/input_output/FGXMLElement.cpp | 23 +- src/FDM/JSBSim/math/FGFunction.cpp | 11 +- src/FDM/JSBSim/math/FGFunction.h | 4 +- src/FDM/JSBSim/models/FGAerodynamics.cpp | 55 +++- src/FDM/JSBSim/models/FGAerodynamics.h | 8 +- src/FDM/JSBSim/models/FGAuxiliary.cpp | 4 +- src/FDM/JSBSim/models/FGFCS.cpp | 10 +- src/FDM/JSBSim/models/FGFCS.h | 6 +- src/FDM/JSBSim/models/FGOutput.cpp | 10 +- src/FDM/JSBSim/models/FGPropagate.cpp | 61 +++- src/FDM/JSBSim/models/FGPropagate.h | 11 +- src/FDM/JSBSim/models/FGPropulsion.cpp | 34 ++- .../JSBSim/models/flight_control/FGAngles.cpp | 205 ++++++++++++++ .../JSBSim/models/flight_control/FGAngles.h | 108 ++++++++ .../models/flight_control/FGFCSComponent.cpp | 8 +- .../models/flight_control/FGFCSComponent.h | 8 +- .../models/flight_control/FGWaypoint.cpp | 261 ++++++++++++++++++ .../JSBSim/models/flight_control/FGWaypoint.h | 133 +++++++++ 23 files changed, 1000 insertions(+), 78 deletions(-) create mode 100755 src/FDM/JSBSim/models/flight_control/FGAngles.cpp create mode 100755 src/FDM/JSBSim/models/flight_control/FGAngles.h create mode 100755 src/FDM/JSBSim/models/flight_control/FGWaypoint.cpp create mode 100755 src/FDM/JSBSim/models/flight_control/FGWaypoint.h diff --git a/src/FDM/JSBSim/CMakeLists.txt b/src/FDM/JSBSim/CMakeLists.txt index 85c1fef7c..5449aa1fc 100644 --- a/src/FDM/JSBSim/CMakeLists.txt +++ b/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 diff --git a/src/FDM/JSBSim/FGJSBBase.h b/src/FDM/JSBSim/FGJSBBase.h index 269b550d9..bcc6e3fdf 100644 --- a/src/FDM/JSBSim/FGJSBBase.h +++ b/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 diff --git a/src/FDM/JSBSim/input_output/FGOutputTextFile.cpp b/src/FDM/JSBSim/input_output/FGOutputTextFile.cpp index f0eb0561c..35fe76c01 100644 --- a/src/FDM/JSBSim/input_output/FGOutputTextFile.cpp +++ b/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;iGetName(); + } + } + outstream << endl; outstream.flush(); @@ -382,6 +390,9 @@ void FGOutputTextFile::Print(void) for (unsigned int i=0;igetDoubleValue(); } + for (unsigned int i=0;igetDoubleValue(); + } outstream.precision(10); outstream << endl; diff --git a/src/FDM/JSBSim/input_output/FGScript.cpp b/src/FDM/JSBSim/input_output/FGScript.cpp index 4d2b6ed0a..6ac660c90 100644 --- a/src/FDM/JSBSim/input_output/FGScript.cpp +++ b/src/FDM/JSBSim/input_output/FGScript.cpp @@ -55,7 +55,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGScript.cpp,v 1.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 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 << ")" - << " executed at time: " << currentTime << endl; + if (thisEvent.NotifyKML) { + cout << endl << "" << endl; + cout << " " << currentTime << " seconds" << " " << endl; + cout << " " << endl; + cout << " " << thisEvent.Name << " (Event " << event_ctr << ")" << " executed at time: " << currentTime << "
" << 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; jGetRelativeName() - cout << " " << thisEvent.DisplayString[j] - << " = " << thisEvent.NotifyProperties[j]->getDoubleValue() << endl; + if (thisEvent.NotifyProperties[j] == 0) { + if (PropertyManager->HasNode(thisEvent.NotifyPropertyNames[j])) { + 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 << "
"; + cout << endl; + } + if (thisEvent.NotifyKML) { + cout << " ]]>" << endl; + cout << " " << endl; + cout << " " << endl; + cout << " absolute " << endl; + cout << " 1 " << endl; + cout << " " << FDMExec->GetPropagate()->GetLongitudeDeg() + << "," << FDMExec->GetPropagate()->GetGeodLatitudeDeg() + << "," << FDMExec->GetPropagate()->GetAltitudeASLmeters() << "" << endl; + cout << " " << endl; + cout << "" << endl; } cout << endl; thisEvent.Notified = true; @@ -561,25 +585,37 @@ void FGScript::Debug(int from) for (unsigned j=0; jGetRelativeName("/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; - for (unsigned j=0; jGetRelativeName("/fdm/jsbsim/") + << Events[i].NotifyPropertyNames[j] << endl; } cout << " }" << endl; diff --git a/src/FDM/JSBSim/input_output/FGScript.h b/src/FDM/JSBSim/input_output/FGScript.h index 17abb4b38..aac134c7a 100644 --- a/src/FDM/JSBSim/input_output/FGScript.h +++ b/src/FDM/JSBSim/input_output/FGScript.h @@ -49,7 +49,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FGSCRIPT "$Id: FGScript.h,v 1.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 "event" clauses.

@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 SetParam; vector SetParamName; vector NotifyProperties; + vector NotifyPropertyNames; vector DisplayString; vector Action; vector 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; diff --git a/src/FDM/JSBSim/input_output/FGXMLElement.cpp b/src/FDM/JSBSim/input_output/FGXMLElement.cpp index 45132a2e0..72c4f1758 100644 --- a/src/FDM/JSBSim/input_output/FGXMLElement.cpp +++ b/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; - exit(0); + throw("Attempting to get the value of a non-existent element "+el); +// 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" - << " conversion in FGXMLElement.cpp." << endl; - exit(-1); + throw("Supplied unit: \"" + supplied_units + "\" does not exist (typo?). Add new unit" + + " conversion in FGXMLElement.cpp."); +// 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 " - << target_units << ". Add new unit conversion in FGXMLElement.cpp or fix typo" << endl; - exit(-1); + throw("Supplied unit: \"" + supplied_units + "\" cannot be converted to " + + target_units + ". Add new unit conversion in FGXMLElement.cpp or fix typo"); +// 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); } } diff --git a/src/FDM/JSBSim/math/FGFunction.cpp b/src/FDM/JSBSim/math/FGFunction.cpp index c64de4b50..328ca06b8 100644 --- a/src/FDM/JSBSim/math/FGFunction.cpp +++ b/src/FDM/JSBSim/math/FGFunction.cpp @@ -43,7 +43,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGFunction.cpp,v 1.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); + } + } } diff --git a/src/FDM/JSBSim/math/FGFunction.h b/src/FDM/JSBSim/math/FGFunction.h index 43fe4212d..49d3e1516 100644 --- a/src/FDM/JSBSim/math/FGFunction.h +++ b/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 @@ -685,7 +685,7 @@ of the three words refers to one or more instances of a property, value, or tabl 0.00 0.25 0.80 0.50 0.90 0.60 - + @endcode @author Jon Berndt */ diff --git a/src/FDM/JSBSim/models/FGAerodynamics.cpp b/src/FDM/JSBSim/models/FGAerodynamics.cpp index cb6152991..02921c230 100644 --- a/src/FDM/JSBSim/models/FGAerodynamics.cpp +++ b/src/FDM/JSBSim/models/FGAerodynamics.cpp @@ -48,7 +48,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.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; jGetValue(); + } + } + // 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"); } diff --git a/src/FDM/JSBSim/models/FGAerodynamics.h b/src/FDM/JSBSim/models/FGAerodynamics.h index 1ab848ba1..82e766323 100644 --- a/src/FDM/JSBSim/models/FGAerodynamics.h +++ b/src/FDM/JSBSim/models/FGAerodynamics.h @@ -52,7 +52,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_AERODYNAMICS "$Id: FGAerodynamics.h,v 1.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; diff --git a/src/FDM/JSBSim/models/FGAuxiliary.cpp b/src/FDM/JSBSim/models/FGAuxiliary.cpp index 557b7e366..492458d91 100644 --- a/src/FDM/JSBSim/models/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/models/FGAuxiliary.cpp @@ -50,7 +50,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAuxiliary.cpp,v 1.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); diff --git a/src/FDM/JSBSim/models/FGFCS.cpp b/src/FDM/JSBSim/models/FGFCS.cpp index bc9d703c9..855925e1c 100644 --- a/src/FDM/JSBSim/models/FGFCS.cpp +++ b/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; } diff --git a/src/FDM/JSBSim/models/FGFCS.h b/src/FDM/JSBSim/models/FGFCS.h index 5297f9344..316cd630a 100644 --- a/src/FDM/JSBSim/models/FGFCS.h +++ b/src/FDM/JSBSim/models/FGFCS.h @@ -51,7 +51,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_FCS "$Id: FGFCS.h,v 1.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 */ diff --git a/src/FDM/JSBSim/models/FGOutput.cpp b/src/FDM/JSBSim/models/FGOutput.cpp index b77ba859e..221c9b7d3 100644 --- a/src/FDM/JSBSim/models/FGOutput.cpp +++ b/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); diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 86c9893bf..83c3e7f22 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -77,7 +77,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.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 << "" << endl; outfile << "" << endl; outfile << " " << VState.vUVW(eU) << " " << endl; @@ -626,6 +643,41 @@ void FGPropagate::WriteStateFile(int num) outfile << " " << GetDistanceAGL() << " " << endl; outfile << "" << endl; outfile.close(); + break; + case 2: + outfile << "" << endl; + outfile << "" << endl; + outfile << "" << endl; + outfile << " " << endl; + outfile << " " << VState.vLocation.GetGeodLatitudeDeg() << " " << endl; + outfile << " " << VState.vLocation.GetLongitudeDeg() << " " << endl; + outfile << " " << GetAltitudeASL() << " " << endl; + outfile << " " << endl; + outfile << "" << endl; + outfile << " " << endl; + outfile << " " << VState.qAttitudeLocal.GetEulerDeg(eYaw) << " " << endl; + outfile << " " << VState.qAttitudeLocal.GetEulerDeg(ePitch) << " " << endl; + outfile << " " << VState.qAttitudeLocal.GetEulerDeg(eRoll) << " " << endl; + outfile << " " << endl; + outfile << "" << endl; + outfile << " " << endl; + outfile << " " << GetVel(eNorth) << " " << endl; + outfile << " " << GetVel(eEast) << " " << endl; + outfile << " " << GetVel(eDown) << " " << endl; + outfile << " " << endl; + outfile << "" << endl; + outfile << " " << endl; + outfile << " " << (VState.vPQR*radtodeg)(eRoll) << " " << endl; + outfile << " " << (VState.vPQR*radtodeg)(ePitch) << " " << endl; + outfile << " " << (VState.vPQR*radtodeg)(eYaw) << " " << endl; + outfile << " " << endl; + outfile << "" << endl; + outfile << "" << 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, diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index 4fd23ddc4..5d4acf868 100644 --- a/src/FDM/JSBSim/models/FGPropagate.h +++ b/src/FDM/JSBSim/models/FGPropagate.h @@ -49,7 +49,7 @@ INCLUDES DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.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); diff --git a/src/FDM/JSBSim/models/FGPropulsion.cpp b/src/FDM/JSBSim/models/FGPropulsion.cpp index 731e06812..372795790 100644 --- a/src/FDM/JSBSim/models/FGPropulsion.cpp +++ b/src/FDM/JSBSim/models/FGPropulsion.cpp @@ -66,7 +66,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropulsion.cpp,v 1.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; iSetCutoff(false); - else - ((FGTurbine*)Engines[i])->SetCutoff(true); + switch (Engines[i]->GetType()) { + case FGEngine::etTurbine: + ((FGTurbine*)Engines[i])->SetCutoff(bsetting); + break; + case FGEngine::etTurboprop: + ((FGTurboProp*)Engines[i])->SetCutoff(bsetting); + break; + default: + break; + } } } else { - if (setting == 0) - ((FGTurbine*)Engines[ActiveEngine])->SetCutoff(false); - else - ((FGTurbine*)Engines[ActiveEngine])->SetCutoff(true); + switch (Engines[ActiveEngine]->GetType()) { + case FGEngine::etTurbine: + ((FGTurbine*)Engines[ActiveEngine])->SetCutoff(bsetting); + 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); } diff --git a/src/FDM/JSBSim/models/flight_control/FGAngles.cpp b/src/FDM/JSBSim/models/flight_control/FGAngles.cpp new file mode 100755 index 000000000..d75daf963 --- /dev/null +++ b/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; + } + } +} +} diff --git a/src/FDM/JSBSim/models/flight_control/FGAngles.h b/src/FDM/JSBSim/models/flight_control/FGAngles.h new file mode 100755 index 000000000..4a75bfecc --- /dev/null +++ b/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 +#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 + + property_name + property_name + [ + {[-]property name | value} + {[-]property name | value} + ] + [ {property} ] + + @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 diff --git a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp index 0de618984..198ad689f 100644 --- a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp +++ b/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"; } diff --git a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.h b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.h index d050a2833..1c0c03ef9 100644 --- a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.h +++ b/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 */ diff --git a/src/FDM/JSBSim/models/flight_control/FGWaypoint.cpp b/src/FDM/JSBSim/models/flight_control/FGWaypoint.cpp new file mode 100755 index 000000000..f95554aee --- /dev/null +++ b/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; + } + } +} +} diff --git a/src/FDM/JSBSim/models/flight_control/FGWaypoint.h b/src/FDM/JSBSim/models/flight_control/FGWaypoint.h new file mode 100755 index 000000000..272d1a384 --- /dev/null +++ b/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 +#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 + + property_name + property_name + property_name + property_name + [ + {[-]property name | value} + {[-]property name | value} + ] + [ {property} ] + + + + property_name + property_name + property_name + property_name + [ {value} ] + [ + {[-]property name | value} + {[-]property name | value} + ] + [ {property} ] + + @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