Sync. with JSBSim (CVS) again

[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 19bf872af46af939348bbf505d6f6ce15259c499..bd6a384179a172b913a4835db9513bfaa9508d51 100755 (executable)
--- a/src/FDM/JSBSim/models/flight_control/FGSensor.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGSensor.cpp
@@ -56,9 +56,9 @@ FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
 
   // inputs are read from the base class constructor
 
-  bits = quantized = divisions = 0;
+  bits = quantized = divisions = index = delay = 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 +82,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 +106,21 @@ 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;
+    }
+  }
+  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();
@@ -133,16 +151,18 @@ bool FGSensor::Run(void )
     return true;
   }
 
-  if (lag != 0.0)            Lag();       // models sensor lag
+  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
+
+  if (delay != 0.0)          Delay();     // models system signal transport latencies
 
   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
 
   Clip(); // Is it right to clip a sensor?
   return true;
@@ -152,7 +172,13 @@ bool FGSensor::Run(void )
 
 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 +200,13 @@ void FGSensor::Bias(void)
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+void FGSensor::Gain(void)
+{
+  Output *= gain;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGSensor::Drift(void)
 {
   drift += drift_rate*dt;
@@ -195,7 +228,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;
@@ -204,9 +237,22 @@ 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 = "fcs/" + PropertyManager->mkPropertyName(Name, true);
+  string tmp = Name;
+  if (Name.find("/") == string::npos) {
+    tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
+  }
   const string tmp_low = tmp + "/malfunction/fail_low";
   const string tmp_high = tmp + "/malfunction/fail_high";
   const string tmp_stuck = tmp + "/malfunction/fail_stuck";
@@ -249,12 +295,14 @@ void FGSensor::Debug(int from)
 
   if (debug_lvl & 1) { // Standard console startup message output
     if (from == 0) { // Constructor
-      if (InputSigns[0] < 0)
-        cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
-      else
-        cout << "      INPUT: " << InputNodes[0]->getName() << endl;
-
-      if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
+      if (InputSigns.size() > 0) {
+        if (InputSigns[0] < 0)
+          cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
+        else
+          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;
@@ -267,6 +315,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) {
@@ -277,7 +326,13 @@ 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) cout << "      OUTPUT: " << OutputNode->getName() << endl;
     }
   }
   if (debug_lvl & 2 ) { // Instantiation/Destruction notification