sync with JSB JSBSim CVS

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

diff --git a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp
old mode 100755 (executable)
new mode 100644 (file)
index bd6a384..bccf518
--- a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp
@@ -38,10 +38,15 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGSensor.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: FGSensor.cpp,v 1.23 2011/08/18 12:42:17 jberndt Exp $";
 static const char *IdHdr = ID_SENSOR;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -52,11 +57,10 @@ CLASS IMPLEMENTATION
 FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
 {
   double denom;
-  dt = fcs->GetDt();
 
   // inputs are read from the base class constructor
 
-  bits = quantized = divisions = index = delay = 0;
+  bits = quantized = divisions = 0;
   PreviousInput = PreviousOutput = 0.0;
   min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
   granularity = 0.0;
@@ -117,11 +121,6 @@ FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
       cerr << "  defaulting to UNIFORM." << endl;
     }
   }
-  if ( element->FindElement("delay") ) {
-    delay = (unsigned int)element->FindElementValueAsNumber("delay");
-    output_array.resize(delay);
-    for (unsigned int i=0; i<delay; i++) output_array[i] = 0.0;
-  }
 
   FGFCSComponent::bind();
   bind();
@@ -138,34 +137,41 @@ FGSensor::~FGSensor()
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-bool FGSensor::Run(void )
+bool FGSensor::Run(void)
 {
   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 
+  ProcessSensorSignal();
+
+  return true;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGSensor::ProcessSensorSignal(void)
+{
   Output = Input; // perfect sensor
 
   // Degrade signal as specified
 
   if (fail_stuck) {
     Output = PreviousOutput;
-    return true;
-  }
-
-  if (lag != 0.0)            Lag();       // models sensor lag and filter
-  if (noise_variance != 0.0) Noise();     // models noise
-  if (drift_rate != 0.0)     Drift();     // models drift over time
-  if (bias != 0.0)           Bias();      // models a finite bias
-  if (gain != 0.0)           Gain();      // models a finite gain
+  } else {
+    if (lag != 0.0)            Lag();       // models sensor lag and filter
+    if (noise_variance != 0.0) Noise();     // models noise
+    if (drift_rate != 0.0)     Drift();     // models drift over time
+    if (gain != 0.0)           Gain();      // models a finite gain
+    if (bias != 0.0)           Bias();      // models a finite bias
 
-  if (delay != 0.0)          Delay();     // models system signal transport latencies
+    if (delay != 0)            Delay();     // models system signal transport latencies
 
-  if (fail_low)  Output = -HUGE_VAL;
-  if (fail_high) Output =  HUGE_VAL;
+    if (fail_low)  Output = -HUGE_VAL;
+    if (fail_high) Output =  HUGE_VAL;
 
-  if (bits != 0)             Quantize();  // models quantization degradation
+    if (bits != 0)             Quantize();  // models quantization degradation
 
-  Clip(); // Is it right to clip a sensor?
-  return true;
+    Clip();
+  }
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -175,7 +181,7 @@ void FGSensor::Noise(void)
   double random_value=0.0;
 
   if (DistributionType == eUniform) {
-    random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
+    random_value = 2.0*(((double)rand()/(double)RAND_MAX) - 0.5);
   } else {
     random_value = GaussianRandomNumber();
   }
@@ -237,16 +243,6 @@ void FGSensor::Lag(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGSensor::Delay(void)
-{
-  output_array[index] = Output;
-  if (index == delay-1) index = 0;
-  else index++;
-  Output = output_array[index];
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
 void FGSensor::bind(void)
 {
   string tmp = Name;
@@ -297,12 +293,10 @@ void FGSensor::Debug(int from)
     if (from == 0) { // Constructor
       if (InputSigns.size() > 0) {
         if (InputSigns[0] < 0)
-          cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
+          cout << "      INPUT: -" << InputNodes[0]->GetName() << endl;
         else
-          cout << "      INPUT: " << InputNodes[0]->getName() << endl;
+          cout << "      INPUT: " << InputNodes[0]->GetName() << endl;
       }
-      if (delay > 0) cout <<"      Frame delay: " << delay
-                                   << " frames (" << delay*dt << " sec)" << endl;
       if (bits != 0) {
         if (quant_property.empty())
           cout << "      Quantized output" << endl;
@@ -332,7 +326,10 @@ void FGSensor::Debug(int from)
           cout << "      Random noise is gaussian distributed." << endl;
         }
       }
-      if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
+      if (IsOutput) {
+        for (unsigned int i=0; i<OutputNodes.size(); i++)
+          cout << "      OUTPUT: " << OutputNodes[i]->getName() << endl;
+      }
     }
   }
   if (debug_lvl & 2 ) { // Instantiation/Destruction notification