Merge branch 'next' of git://gitorious.org/fg/flightgear into next

[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..2fc74bd9639ded2471024fc7067c209cf3c5800b 100644 (file)
--- a/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGFCSComponent.cpp
@@ -38,10 +38,17 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGFCSComponent.h"
+#include "input_output/FGPropertyManager.h"
+#include "input_output/FGXMLElement.h"
+#include "math/FGPropertyValue.h"
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGFCSComponent.cpp,v 1.32 2011/06/16 03:39:38 jberndt Exp $";
 static const char *IdHdr = ID_FCSCOMPONENT;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -51,12 +58,14 @@ CLASS IMPLEMENTATION
 FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
 {
   Element *input_element, *clip_el;
-  Input = Output = clipmin = clipmax = 0.0;
-  OutputNode = treenode = 0;
+  Input = Output = clipmin = clipmax = delay_time = 0.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 +98,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
@@ -97,8 +112,6 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
 
   Name = element->GetAttributeValue("name");
 
-  FGPropertyManager *tmp=0;
-
   input_element = element->FindElement("input");
   while (input_element) {
     input = input_element->GetDataLine();
@@ -108,30 +121,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 );
+    FGPropertyManager* node = 0L;
+    if (PropertyManager->HasNode(input)) {
+      node = PropertyManager->GetNode(input);
+      InputNodes.push_back(new FGPropertyValue( node ));
     } else {
-      cerr << fgred << "  In component: " << Name << " unknown property "
-           << input << " referenced. Aborting" << reset << endl;
-      exit(-1);
+      InputNodes.push_back(new FGPropertyValue( input,
+                                                PropertyManager ));
     }
+    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_time = delay_elem->GetDataAsNumber();
+    string delayType = delay_elem->GetAttributeValue("type");
+    if (delayType.length() > 0) {
+      if (delayType == "time") {
+        delay = (unsigned int)(delay_time / dt);
+      } else if (delayType == "frames") {
+        delay = (unsigned int)delay_time;
+      } else {
+        cerr << "Unallowed delay type" << endl;
+      }
+    } else {
+      delay = (unsigned int)(delay_time / 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 +179,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);
@@ -161,13 +199,16 @@ FGFCSComponent::FGFCSComponent(FGFCS* _fcs, Element* element) : fcs(_fcs)
 FGFCSComponent::~FGFCSComponent()
 {
   Debug(1);
+  for (unsigned int i=0; i<InputNodes.size(); i++) {
+    delete InputNodes[i];
+  }
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGFCSComponent::SetOutput(void)
 {
-  OutputNode->setDoubleValue(Output);
+  for (unsigned int i=0; i<OutputNodes.size(); i++) OutputNodes[i]->setDoubleValue(Output);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -179,6 +220,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) {
@@ -254,6 +305,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