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