Andreas Gaeb: fix #222 (JSBSIm reset problems)

[flightgear.git] / src / FDM / JSBSim / models / flight_control / FGFCSComponent.cpp

diff --git a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
index 26f73facf2f4cb33560ef4d047be6ef251503ac3..6cdf9ab8334ab381cb00c91f759369817637b4bb 100644 (file)
--- a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
@@ -38,10 +38,16 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGFCSComponent.h"
+#include "input_output/FGPropertyManager.h"
+#include "input_output/FGXMLElement.h"
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.29 2010/09/07 00:40:03 jberndt Exp $";
 static const char *IdHdr = ID_FCSCOMPONENT;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -52,11 +58,13 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
 {
   Element *input_element, *clip_el;
   Input = Output = clipmin = clipmax = 0.0;
-  OutputNode = treenode = 0;
+  treenode = 0;
+  delay = index = 0;
   ClipMinPropertyNode = ClipMaxPropertyNode = 0;
   clipMinSign = clipMaxSign = 1.0;
   IsOutput   = clip = false;
   string input, clip_string;
+  dt = fcs->GetDt();
 
   PropertyManager = fcs->GetPropertyManager();
   if        (element->GetName() == string("lag_filter")) {
@@ -89,6 +97,12 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
     Type = "PID";
   } else if (element->GetName() == string("sensor")) {
     Type = "SENSOR";
+  } else if (element->GetName() == string("accelerometer")) {
+    Type = "ACCELEROMETER";
+  } else if (element->GetName() == string("magnetometer")) {
+    Type = "MAGNETOMETER";
+  } else if (element->GetName() == string("gyro")) {
+    Type = "GYRO";
   } else if (element->GetName() == string("actuator")) {
     Type = "ACTUATOR";
   } else { // illegal component in this channel
@@ -108,30 +122,55 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
     } else {
       InputSigns.push_back( 1.0);
     }
-    tmp = PropertyManager->GetNode(input);
-    if (tmp) {
-      InputNodes.push_back( tmp );
+    if (PropertyManager->HasNode(input)) {
+      tmp = PropertyManager->GetNode(input);
     } else {
-      cerr << fgred << "  In component: " << Name << " unknown property "
-           << input << " referenced. Aborting" << reset << endl;
-      exit(-1);
+      tmp = 0L;
+      // cerr << fgcyan << "In component: " + Name + " property "
+      //      + input + " is initially undefined." << reset << endl;
     }
+    InputNodes.push_back( tmp );
+    InputNames.push_back( input );
+
     input_element = element->FindNextElement("input");
   }
 
-  if (element->FindElement("output")) {
+  Element *out_elem = element->FindElement("output");
+  while (out_elem) {
     IsOutput = true;
-    OutputNode = PropertyManager->GetNode( element->FindElementValue("output"), true );
+    string output_node_name = out_elem->GetDataLine();
+    FGPropertyManager* OutputNode = PropertyManager->GetNode( output_node_name, true );
+    OutputNodes.push_back(OutputNode);
     if (!OutputNode) {
-      cerr << endl << "  Unable to process property: " << element->FindElementValue("output") << endl;
+      cerr << endl << "  Unable to process property: " << output_node_name << endl;
       throw(string("Invalid output property name in flight control definition"));
     }
+    out_elem = element->FindNextElement("output");
+  }
+
+  Element* delay_elem = element->FindElement("delay");
+  if ( delay_elem ) {
+    delay = (unsigned int)delay_elem->GetDataAsNumber();
+    string delayType = delay_elem->GetAttributeValue("type");
+    if (delayType.length() > 0) {
+      if (delayType == "time") {
+        delay = (int)(delay / dt);
+      } else if (delayType == "frames") {
+        // no op. the delay type of "frames" is assumed and is the default.
+      } else {
+        cerr << "Unallowed delay type" << endl;
+      }
+    } else {
+      delay = (int)(delay / dt);
+    }
+    output_array.resize(delay);
+    for (int i=0; i<delay; i++) output_array[i] = 0.0;
   }
 
   clip_el = element->FindElement("clipto");
   if (clip_el) {
     clip_string = clip_el->FindElementValue("min");
-    if (clip_string.find_first_not_of("+-.0123456789") != string::npos) { // it's a property
+    if (!is_number(clip_string)) { // it's a property
       if (clip_string[0] == '-') {
         clipMinSign = -1.0;
         clip_string.erase(0,1);
@@ -141,7 +180,7 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
       clipmin = clip_el->FindElementValueAsNumber("min");
     }
     clip_string = clip_el->FindElementValue("max");
-    if (clip_string.find_first_not_of("+-.0123456789") != string::npos) { // it's a property
+    if (!is_number(clip_string)) { // it's a property
       if (clip_string[0] == '-') {
         clipMaxSign = -1.0;
         clip_string.erase(0,1);
@@ -167,7 +206,7 @@ FGFCSComponent::~FGFCSComponent()
 
 void FGFCSComponent::SetOutput(void)
 {
-  OutputNode->setDoubleValue(Output);
+  for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -179,6 +218,16 @@ bool FGFCSComponent::Run(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+void FGFCSComponent::Delay(void)
+{
+  output_array[index] = Output;
+  if (index == delay-1) index = 0;
+  else index++;
+  Output = output_array[index];
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGFCSComponent::Clip(void)
 {
   if (clip) {
@@ -189,6 +238,24 @@ void FGFCSComponent::Clip(void)
   }
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGFCSComponent::LateBind(void)
+{
+  FGPropertyManager* node = 0L;
+
+  for (unsigned int i=0; i<InputNodes.size(); i++) {
+    if (!InputNodes[i]) {
+      if (PropertyManager->HasNode(InputNames[i])) {
+        node = PropertyManager->GetNode(InputNames[i]);
+        InputNodes[i] = node;
+      } else {
+        throw(InputNames[i]);
+      }
+    }
+  }
+}
+
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 //
 // The old way of naming FCS components allowed upper or lower case, spaces, etc.
@@ -254,6 +321,8 @@ void FGFCSComponent::Debug(int from)
           cout << "      Maximum limit: " << clipmax << endl;
         }
       }  
+      if (delay > 0) cout <<"      Frame delay: " << delay
+                                   << " frames (" << delay*dt << " sec)" << endl;
     }
   }
   if (debug_lvl & 2 ) { // Instantiation/Destruction notification