Sync. with 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
index 5e4ede578291c1b3f31f395a2ba8bd7a10035562..1f46dc9540a9679da8acb35a3c1b3c072ffde525 100755 (executable)
--- 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.20 2009/10/24 22:59:30 jberndt Exp $";
 static const char *IdHdr = ID_SENSOR;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -52,13 +57,12 @@ 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 = 0;
   PreviousInput = PreviousOutput = 0.0;
-  min = max = bias = noise_variance = lag = drift_rate = drift = span = 0.0;
+  min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
   granularity = 0.0;
   noise_type = 0;
   fail_low = fail_high = fail_stuck = false;
@@ -82,6 +86,9 @@ FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
   if ( element->FindElement("bias") ) {
     bias = element->FindElementValueAsNumber("bias");
   }
+  if ( element->FindElement("gain") ) {
+    gain = element->FindElementValueAsNumber("gain");
+  }
   if ( element->FindElement("drift_rate") ) {
     drift_rate = element->FindElementValueAsNumber("drift_rate");
   }
@@ -103,6 +110,16 @@ FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
       cerr << "Unknown noise type in sensor: " << Name << endl;
       cerr << "  defaulting to PERCENT." << endl;
     }
+    string distribution = element->FindElement("noise")->GetAttributeValue("distribution");
+    if (distribution == "UNIFORM") {
+      DistributionType = eUniform;
+    } else if (distribution == "GAUSSIAN") {
+      DistributionType = eGaussian;
+    } else {
+      DistributionType = eUniform;
+      cerr << "Unknown random distribution type in sensor: " << Name << endl;
+      cerr << "  defaulting to UNIFORM." << endl;
+    }
   }
 
   FGFCSComponent::bind();
@@ -120,39 +137,54 @@ 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;
-  }
+  } 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 (lag != 0.0)            Lag();       // models sensor lag
-  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 (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 (delay != 0.0)          Delay();     // models system signal transport latencies
+    if (bits != 0)             Quantize();  // models quantization degradation
 
-  Clip(); // Is it right to clip a sensor?
-  return true;
+    Clip();
+  }
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGSensor::Noise(void)
 {
-  double random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
+  double random_value=0.0;
+
+  if (DistributionType == eUniform) {
+    random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
+  } else {
+    random_value = GaussianRandomNumber();
+  }
 
   switch( NoiseType ) {
   case ePercent:
@@ -174,6 +206,13 @@ void FGSensor::Bias(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+void FGSensor::Gain(void)
+{
+  Output *= gain;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGSensor::Drift(void)
 {
   drift += drift_rate*dt;
@@ -195,7 +234,7 @@ void FGSensor::Quantize(void)
 
 void FGSensor::Lag(void)
 {
-  // "Output" on the right side of the "=" is the current frame input
+  // "Output" on the right side of the "=" is the current input
   Output = ca * (Output + PreviousInput) + PreviousOutput * cb;
 
   PreviousOutput = Output;
@@ -258,7 +297,6 @@ void FGSensor::Debug(int from)
         else
           cout << "      INPUT: " << InputNodes[0]->getName() << endl;
       }
-      if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
       if (bits != 0) {
         if (quant_property.empty())
           cout << "      Quantized output" << endl;
@@ -271,6 +309,7 @@ void FGSensor::Debug(int from)
         cout << "          (span: " << span << ", granularity: " << granularity << ")" << endl;
       }
       if (bias != 0.0) cout << "      Bias: " << bias << endl;
+      if (gain != 0.0) cout << "      Gain: " << gain << endl;
       if (drift_rate != 0) cout << "      Sensor drift rate: " << drift_rate << endl;
       if (lag != 0) cout << "      Sensor lag: " << lag << endl;
       if (noise_variance != 0) {
@@ -281,6 +320,15 @@ void FGSensor::Debug(int from)
         } else {
           cout << "      Noise variance type is invalid" << endl;
         }
+        if (DistributionType == eUniform) {
+          cout << "      Random noise is uniformly distributed." << endl;
+        } else if (DistributionType == eGaussian) {
+          cout << "      Random noise is gaussian distributed." << endl;
+        }
+      }
+      if (IsOutput) {
+        for (unsigned int i=0; i<OutputNodes.size(); i++)
+          cout << "      OUTPUT: " << OutputNodes[i]->getName() << endl;
       }
     }
   }