src/FDM/JSBSim/models/flight_control/FGSensor.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3  Module:       FGSensor.cpp
   4  Author:       Jon Berndt
   5  Date started: 9 July 2005
   6
   7  ------------- Copyright (C) 2005 -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26 FUNCTIONAL DESCRIPTION
  27 --------------------------------------------------------------------------------
  28
  29 HISTORY
  30 --------------------------------------------------------------------------------
  31
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  33 COMMENTS, REFERENCES,  and NOTES
  34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGSensor.h"
  41
  42 namespace JSBSim {
  43
  44 static const char *IdSrc = "$Id$";
  45 static const char *IdHdr = ID_SENSOR;
  46
  47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  48 CLASS IMPLEMENTATION
  49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  50
  51
  52 FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
  53 {
  54   double denom;
  55   dt = fcs->GetDt();
  56
  57   // inputs are read from the base class constructor
  58
  59   bits = quantized = divisions = index = delay = 0;
  60   PreviousInput = PreviousOutput = 0.0;
  61   min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
  62   granularity = 0.0;
  63   noise_type = 0;
  64   fail_low = fail_high = fail_stuck = false;
  65
  66   Element* quantization_element = element->FindElement("quantization");
  67   if ( quantization_element) {
  68     if ( quantization_element->FindElement("bits") ) {
  69       bits = (int)quantization_element->FindElementValueAsNumber("bits");
  70     }
  71     divisions = (1<<bits);
  72     if ( quantization_element->FindElement("min") ) {
  73       min = quantization_element->FindElementValueAsNumber("min");
  74     }
  75     if ( quantization_element->FindElement("max") ) {
  76       max = quantization_element->FindElementValueAsNumber("max");
  77     }
  78     quant_property = quantization_element->GetAttributeValue("name");
  79     span = max - min;
  80     granularity = span/divisions;
  81   }
  82   if ( element->FindElement("bias") ) {
  83     bias = element->FindElementValueAsNumber("bias");
  84   }
  85   if ( element->FindElement("gain") ) {
  86     gain = element->FindElementValueAsNumber("gain");
  87   }
  88   if ( element->FindElement("drift_rate") ) {
  89     drift_rate = element->FindElementValueAsNumber("drift_rate");
  90   }
  91   if ( element->FindElement("lag") ) {
  92     lag = element->FindElementValueAsNumber("lag");
  93     denom = 2.00 + dt*lag;
  94     ca = dt*lag / denom;
  95     cb = (2.00 - dt*lag) / denom;
  96   }
  97   if ( element->FindElement("noise") ) {
  98     noise_variance = element->FindElementValueAsNumber("noise");
  99     string variation = element->FindElement("noise")->GetAttributeValue("variation");
 100     if (variation == "PERCENT") {
 101       NoiseType = ePercent;
 102     } else if (variation == "ABSOLUTE") {
 103       NoiseType = eAbsolute;
 104     } else {
 105       NoiseType = ePercent;
 106       cerr << "Unknown noise type in sensor: " << Name << endl;
 107       cerr << "  defaulting to PERCENT." << endl;
 108     }
 109     string distribution = element->FindElement("noise")->GetAttributeValue("distribution");
 110     if (distribution == "UNIFORM") {
 111       DistributionType = eUniform;
 112     } else if (distribution == "GAUSSIAN") {
 113       DistributionType = eGaussian;
 114     } else {
 115       DistributionType = eUniform;
 116       cerr << "Unknown random distribution type in sensor: " << Name << endl;
 117       cerr << "  defaulting to UNIFORM." << endl;
 118     }
 119   }
 120   if ( element->FindElement("delay") ) {
 121     delay = (unsigned int)element->FindElementValueAsNumber("delay");
 122     output_array.resize(delay);
 123     for (unsigned int i=0; i<delay; i++) output_array[i] = 0.0;
 124   }
 125
 126   FGFCSComponent::bind();
 127   bind();
 128
 129   Debug(0);
 130 }
 131
 132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 133
 134 FGSensor::~FGSensor()
 135 {
 136   Debug(1);
 137 }
 138
 139 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 140
 141 bool FGSensor::Run(void )
 142 {
 143   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 144
 145   Output = Input; // perfect sensor
 146
 147   // Degrade signal as specified
 148
 149   if (fail_stuck) {
 150     Output = PreviousOutput;
 151     return true;
 152   }
 153
 154   if (lag != 0.0)            Lag();       // models sensor lag and filter
 155   if (noise_variance != 0.0) Noise();     // models noise
 156   if (drift_rate != 0.0)     Drift();     // models drift over time
 157   if (bias != 0.0)           Bias();      // models a finite bias
 158   if (gain != 0.0)           Gain();      // models a finite gain
 159
 160   if (delay != 0.0)          Delay();     // models system signal transport latencies
 161
 162   if (fail_low)  Output = -HUGE_VAL;
 163   if (fail_high) Output =  HUGE_VAL;
 164
 165   if (bits != 0)             Quantize();  // models quantization degradation
 166
 167   Clip(); // Is it right to clip a sensor?
 168   return true;
 169 }
 170
 171 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 172
 173 void FGSensor::Noise(void)
 174 {
 175   double random_value=0.0;
 176
 177   if (DistributionType == eUniform) {
 178     random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
 179   } else {
 180     random_value = GaussianRandomNumber();
 181   }
 182
 183   switch( NoiseType ) {
 184   case ePercent:
 185     Output *= (1.0 + noise_variance*random_value);
 186     break;
 187
 188   case eAbsolute:
 189     Output += noise_variance*random_value;
 190     break;
 191   }
 192 }
 193
 194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 195
 196 void FGSensor::Bias(void)
 197 {
 198   Output += bias;
 199 }
 200
 201 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 202
 203 void FGSensor::Gain(void)
 204 {
 205   Output *= gain;
 206 }
 207
 208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 209
 210 void FGSensor::Drift(void)
 211 {
 212   drift += drift_rate*dt;
 213   Output += drift;
 214 }
 215
 216 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 217
 218 void FGSensor::Quantize(void)
 219 {
 220   if (Output < min) Output = min;
 221   if (Output > max) Output = max;
 222   double portion = Output - min;
 223   quantized = (int)(portion/granularity);
 224   Output = quantized*granularity + min;
 225 }
 226
 227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 228
 229 void FGSensor::Lag(void)
 230 {
 231   // "Output" on the right side of the "=" is the current input
 232   Output = ca * (Output + PreviousInput) + PreviousOutput * cb;
 233
 234   PreviousOutput = Output;
 235   PreviousInput  = Input;
 236 }
 237
 238 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 239
 240 void FGSensor::Delay(void)
 241 {
 242   output_array[index] = Output;
 243   if (index == delay-1) index = 0;
 244   else index++;
 245   Output = output_array[index];
 246 }
 247
 248 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 249
 250 void FGSensor::bind(void)
 251 {
 252   string tmp = Name;
 253   if (Name.find("/") == string::npos) {
 254     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
 255   }
 256   const string tmp_low = tmp + "/malfunction/fail_low";
 257   const string tmp_high = tmp + "/malfunction/fail_high";
 258   const string tmp_stuck = tmp + "/malfunction/fail_stuck";
 259
 260   PropertyManager->Tie( tmp_low, this, &FGSensor::GetFailLow, &FGSensor::SetFailLow);
 261   PropertyManager->Tie( tmp_high, this, &FGSensor::GetFailHigh, &FGSensor::SetFailHigh);
 262   PropertyManager->Tie( tmp_stuck, this, &FGSensor::GetFailStuck, &FGSensor::SetFailStuck);
 263
 264   if (!quant_property.empty()) {
 265     if (quant_property.find("/") == string::npos) { // not found
 266       string qprop = "fcs/" + PropertyManager->mkPropertyName(quant_property, true);
 267       PropertyManager->Tie(qprop, this, &FGSensor::GetQuantized);
 268     }
 269   }
 270
 271 }
 272
 273 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 274 //    The bitmasked value choices are as follows:
 275 //    unset: In this case (the default) JSBSim would only print
 276 //       out the normally expected messages, essentially echoing
 277 //       the config files as they are read. If the environment
 278 //       variable is not set, debug_lvl is set to 1 internally
 279 //    0: This requests JSBSim not to output any messages
 280 //       whatsoever.
 281 //    1: This value explicity requests the normal JSBSim
 282 //       startup messages
 283 //    2: This value asks for a message to be printed out when
 284 //       a class is instantiated
 285 //    4: When this value is set, a message is displayed when a
 286 //       FGModel object executes its Run() method
 287 //    8: When this value is set, various runtime state variables
 288 //       are printed out periodically
 289 //    16: When set various parameters are sanity checked and
 290 //       a message is printed out when they go out of bounds
 291
 292 void FGSensor::Debug(int from)
 293 {
 294   if (debug_lvl <= 0) return;
 295
 296   if (debug_lvl & 1) { // Standard console startup message output
 297     if (from == 0) { // Constructor
 298       if (InputSigns.size() > 0) {
 299         if (InputSigns[0] < 0)
 300           cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
 301         else
 302           cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 303       }
 304       if (delay > 0) cout <<"      Frame delay: " << delay
 305                                    << " frames (" << delay*dt << " sec)" << endl;
 306       if (bits != 0) {
 307         if (quant_property.empty())
 308           cout << "      Quantized output" << endl;
 309         else
 310           cout << "      Quantized output (property: " << quant_property << ")" << endl;
 311
 312         cout << "        Bits: " << bits << endl;
 313         cout << "        Min value: " << min << endl;
 314         cout << "        Max value: " << max << endl;
 315         cout << "          (span: " << span << ", granularity: " << granularity << ")" << endl;
 316       }
 317       if (bias != 0.0) cout << "      Bias: " << bias << endl;
 318       if (gain != 0.0) cout << "      Gain: " << gain << endl;
 319       if (drift_rate != 0) cout << "      Sensor drift rate: " << drift_rate << endl;
 320       if (lag != 0) cout << "      Sensor lag: " << lag << endl;
 321       if (noise_variance != 0) {
 322         if (NoiseType == eAbsolute) {
 323           cout << "      Noise variance (absolute): " << noise_variance << endl;
 324         } else if (NoiseType == ePercent) {
 325           cout << "      Noise variance (percent): " << noise_variance << endl;
 326         } else {
 327           cout << "      Noise variance type is invalid" << endl;
 328         }
 329         if (DistributionType == eUniform) {
 330           cout << "      Random noise is uniformly distributed." << endl;
 331         } else if (DistributionType == eGaussian) {
 332           cout << "      Random noise is gaussian distributed." << endl;
 333         }
 334       }
 335       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
 336     }
 337   }
 338   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 339     if (from == 0) cout << "Instantiated: FGSensor" << endl;
 340     if (from == 1) cout << "Destroyed:    FGSensor" << endl;
 341   }
 342   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 343   }
 344   if (debug_lvl & 8 ) { // Runtime state variables
 345   }
 346   if (debug_lvl & 16) { // Sanity checking
 347   }
 348   if (debug_lvl & 64) {
 349     if (from == 0) { // Constructor
 350       cout << IdSrc << endl;
 351       cout << IdHdr << endl;
 352     }
 353   }
 354 }
 355 }