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 #include "input_output/FGXMLElement.h"
  42 #include <iostream>
  43 #include <cstdlib>
  44
  45 using namespace std;
  46
  47 namespace JSBSim {
  48
  49 static const char *IdSrc = "$Id: FGSensor.cpp,v 1.20 2009/10/24 22:59:30 jberndt Exp $";
  50 static const char *IdHdr = ID_SENSOR;
  51
  52 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  53 CLASS IMPLEMENTATION
  54 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  55
  56
  57 FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
  58 {
  59   double denom;
  60
  61   // inputs are read from the base class constructor
  62
  63   bits = quantized = divisions = 0;
  64   PreviousInput = PreviousOutput = 0.0;
  65   min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
  66   granularity = 0.0;
  67   noise_type = 0;
  68   fail_low = fail_high = fail_stuck = false;
  69
  70   Element* quantization_element = element->FindElement("quantization");
  71   if ( quantization_element) {
  72     if ( quantization_element->FindElement("bits") ) {
  73       bits = (int)quantization_element->FindElementValueAsNumber("bits");
  74     }
  75     divisions = (1<<bits);
  76     if ( quantization_element->FindElement("min") ) {
  77       min = quantization_element->FindElementValueAsNumber("min");
  78     }
  79     if ( quantization_element->FindElement("max") ) {
  80       max = quantization_element->FindElementValueAsNumber("max");
  81     }
  82     quant_property = quantization_element->GetAttributeValue("name");
  83     span = max - min;
  84     granularity = span/divisions;
  85   }
  86   if ( element->FindElement("bias") ) {
  87     bias = element->FindElementValueAsNumber("bias");
  88   }
  89   if ( element->FindElement("gain") ) {
  90     gain = element->FindElementValueAsNumber("gain");
  91   }
  92   if ( element->FindElement("drift_rate") ) {
  93     drift_rate = element->FindElementValueAsNumber("drift_rate");
  94   }
  95   if ( element->FindElement("lag") ) {
  96     lag = element->FindElementValueAsNumber("lag");
  97     denom = 2.00 + dt*lag;
  98     ca = dt*lag / denom;
  99     cb = (2.00 - dt*lag) / denom;
 100   }
 101   if ( element->FindElement("noise") ) {
 102     noise_variance = element->FindElementValueAsNumber("noise");
 103     string variation = element->FindElement("noise")->GetAttributeValue("variation");
 104     if (variation == "PERCENT") {
 105       NoiseType = ePercent;
 106     } else if (variation == "ABSOLUTE") {
 107       NoiseType = eAbsolute;
 108     } else {
 109       NoiseType = ePercent;
 110       cerr << "Unknown noise type in sensor: " << Name << endl;
 111       cerr << "  defaulting to PERCENT." << endl;
 112     }
 113     string distribution = element->FindElement("noise")->GetAttributeValue("distribution");
 114     if (distribution == "UNIFORM") {
 115       DistributionType = eUniform;
 116     } else if (distribution == "GAUSSIAN") {
 117       DistributionType = eGaussian;
 118     } else {
 119       DistributionType = eUniform;
 120       cerr << "Unknown random distribution type in sensor: " << Name << endl;
 121       cerr << "  defaulting to UNIFORM." << endl;
 122     }
 123   }
 124
 125   FGFCSComponent::bind();
 126   bind();
 127
 128   Debug(0);
 129 }
 130
 131 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 132
 133 FGSensor::~FGSensor()
 134 {
 135   Debug(1);
 136 }
 137
 138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 139
 140 bool FGSensor::Run(void)
 141 {
 142   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 143
 144   ProcessSensorSignal();
 145
 146   return true;
 147 }
 148
 149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 150
 151 void FGSensor::ProcessSensorSignal(void)
 152 {
 153   Output = Input; // perfect sensor
 154
 155   // Degrade signal as specified
 156
 157   if (fail_stuck) {
 158     Output = PreviousOutput;
 159   } else {
 160     if (lag != 0.0)            Lag();       // models sensor lag and filter
 161     if (noise_variance != 0.0) Noise();     // models noise
 162     if (drift_rate != 0.0)     Drift();     // models drift over time
 163     if (gain != 0.0)           Gain();      // models a finite gain
 164     if (bias != 0.0)           Bias();      // models a finite bias
 165
 166     if (delay != 0)            Delay();     // models system signal transport latencies
 167
 168     if (fail_low)  Output = -HUGE_VAL;
 169     if (fail_high) Output =  HUGE_VAL;
 170
 171     if (bits != 0)             Quantize();  // models quantization degradation
 172
 173     Clip();
 174   }
 175 }
 176
 177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 178
 179 void FGSensor::Noise(void)
 180 {
 181   double random_value=0.0;
 182
 183   if (DistributionType == eUniform) {
 184     random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
 185   } else {
 186     random_value = GaussianRandomNumber();
 187   }
 188
 189   switch( NoiseType ) {
 190   case ePercent:
 191     Output *= (1.0 + noise_variance*random_value);
 192     break;
 193
 194   case eAbsolute:
 195     Output += noise_variance*random_value;
 196     break;
 197   }
 198 }
 199
 200 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 201
 202 void FGSensor::Bias(void)
 203 {
 204   Output += bias;
 205 }
 206
 207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 208
 209 void FGSensor::Gain(void)
 210 {
 211   Output *= gain;
 212 }
 213
 214 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 215
 216 void FGSensor::Drift(void)
 217 {
 218   drift += drift_rate*dt;
 219   Output += drift;
 220 }
 221
 222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 223
 224 void FGSensor::Quantize(void)
 225 {
 226   if (Output < min) Output = min;
 227   if (Output > max) Output = max;
 228   double portion = Output - min;
 229   quantized = (int)(portion/granularity);
 230   Output = quantized*granularity + min;
 231 }
 232
 233 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 234
 235 void FGSensor::Lag(void)
 236 {
 237   // "Output" on the right side of the "=" is the current input
 238   Output = ca * (Output + PreviousInput) + PreviousOutput * cb;
 239
 240   PreviousOutput = Output;
 241   PreviousInput  = Input;
 242 }
 243
 244 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 245
 246 void FGSensor::bind(void)
 247 {
 248   string tmp = Name;
 249   if (Name.find("/") == string::npos) {
 250     tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
 251   }
 252   const string tmp_low = tmp + "/malfunction/fail_low";
 253   const string tmp_high = tmp + "/malfunction/fail_high";
 254   const string tmp_stuck = tmp + "/malfunction/fail_stuck";
 255
 256   PropertyManager->Tie( tmp_low, this, &FGSensor::GetFailLow, &FGSensor::SetFailLow);
 257   PropertyManager->Tie( tmp_high, this, &FGSensor::GetFailHigh, &FGSensor::SetFailHigh);
 258   PropertyManager->Tie( tmp_stuck, this, &FGSensor::GetFailStuck, &FGSensor::SetFailStuck);
 259
 260   if (!quant_property.empty()) {
 261     if (quant_property.find("/") == string::npos) { // not found
 262       string qprop = "fcs/" + PropertyManager->mkPropertyName(quant_property, true);
 263       PropertyManager->Tie(qprop, this, &FGSensor::GetQuantized);
 264     }
 265   }
 266
 267 }
 268
 269 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 270 //    The bitmasked value choices are as follows:
 271 //    unset: In this case (the default) JSBSim would only print
 272 //       out the normally expected messages, essentially echoing
 273 //       the config files as they are read. If the environment
 274 //       variable is not set, debug_lvl is set to 1 internally
 275 //    0: This requests JSBSim not to output any messages
 276 //       whatsoever.
 277 //    1: This value explicity requests the normal JSBSim
 278 //       startup messages
 279 //    2: This value asks for a message to be printed out when
 280 //       a class is instantiated
 281 //    4: When this value is set, a message is displayed when a
 282 //       FGModel object executes its Run() method
 283 //    8: When this value is set, various runtime state variables
 284 //       are printed out periodically
 285 //    16: When set various parameters are sanity checked and
 286 //       a message is printed out when they go out of bounds
 287
 288 void FGSensor::Debug(int from)
 289 {
 290   if (debug_lvl <= 0) return;
 291
 292   if (debug_lvl & 1) { // Standard console startup message output
 293     if (from == 0) { // Constructor
 294       if (InputSigns.size() > 0) {
 295         if (InputSigns[0] < 0)
 296           cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
 297         else
 298           cout << "      INPUT: " << InputNodes[0]->getName() << endl;
 299       }
 300       if (bits != 0) {
 301         if (quant_property.empty())
 302           cout << "      Quantized output" << endl;
 303         else
 304           cout << "      Quantized output (property: " << quant_property << ")" << endl;
 305
 306         cout << "        Bits: " << bits << endl;
 307         cout << "        Min value: " << min << endl;
 308         cout << "        Max value: " << max << endl;
 309         cout << "          (span: " << span << ", granularity: " << granularity << ")" << endl;
 310       }
 311       if (bias != 0.0) cout << "      Bias: " << bias << endl;
 312       if (gain != 0.0) cout << "      Gain: " << gain << endl;
 313       if (drift_rate != 0) cout << "      Sensor drift rate: " << drift_rate << endl;
 314       if (lag != 0) cout << "      Sensor lag: " << lag << endl;
 315       if (noise_variance != 0) {
 316         if (NoiseType == eAbsolute) {
 317           cout << "      Noise variance (absolute): " << noise_variance << endl;
 318         } else if (NoiseType == ePercent) {
 319           cout << "      Noise variance (percent): " << noise_variance << endl;
 320         } else {
 321           cout << "      Noise variance type is invalid" << endl;
 322         }
 323         if (DistributionType == eUniform) {
 324           cout << "      Random noise is uniformly distributed." << endl;
 325         } else if (DistributionType == eGaussian) {
 326           cout << "      Random noise is gaussian distributed." << endl;
 327         }
 328       }
 329       if (IsOutput) {
 330         for (unsigned int i=0; i<OutputNodes.size(); i++)
 331           cout << "      OUTPUT: " << OutputNodes[i]->getName() << endl;
 332       }
 333     }
 334   }
 335   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 336     if (from == 0) cout << "Instantiated: FGSensor" << endl;
 337     if (from == 1) cout << "Destroyed:    FGSensor" << endl;
 338   }
 339   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 340   }
 341   if (debug_lvl & 8 ) { // Runtime state variables
 342   }
 343   if (debug_lvl & 16) { // Sanity checking
 344   }
 345   if (debug_lvl & 64) {
 346     if (from == 0) { // Constructor
 347       cout << IdSrc << endl;
 348       cout << IdHdr << endl;
 349     }
 350   }
 351 }
 352 }