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->GetState()->Getdt();
  56
  57   // inputs are read from the base class constructor
  58
  59   dt = fcs->GetState()->Getdt();
  60
  61   bits = quantized = divisions = 0;
  62   PreviousInput = PreviousOutput = 0.0;
  63   min = max = bias = noise_variance = lag = drift_rate = drift = span = 0.0;
  64   granularity = 0.0;
  65   noise_type = 0;
  66   fail_low = fail_high = fail_stuck = false;
  67
  68   Element* quantization_element = element->FindElement("quantization");
  69   if ( quantization_element) {
  70     if ( quantization_element->FindElement("bits") ) {
  71       bits = (int)quantization_element->FindElementValueAsNumber("bits");
  72     }
  73     divisions = (1<<bits);
  74     if ( quantization_element->FindElement("min") ) {
  75       min = quantization_element->FindElementValueAsNumber("min");
  76     }
  77     if ( quantization_element->FindElement("max") ) {
  78       max = quantization_element->FindElementValueAsNumber("max");
  79     }
  80     span = max - min;
  81     granularity = span/divisions;
  82   }
  83   if ( element->FindElement("bias") ) {
  84     bias = element->FindElementValueAsNumber("bias");
  85   }
  86   if ( element->FindElement("drift_rate") ) {
  87     drift_rate = element->FindElementValueAsNumber("drift_rate");
  88   }
  89   if ( element->FindElement("lag") ) {
  90     lag = element->FindElementValueAsNumber("lag");
  91     denom = 2.00 + dt*lag;
  92     ca = dt*lag / denom;
  93     cb = (2.00 - dt*lag) / denom;
  94   }
  95   if ( element->FindElement("noise") ) {
  96     noise_variance = element->FindElementValueAsNumber("noise");
  97     string variation = element->FindElement("noise")->GetAttributeValue("variation");
  98     if (variation == "PERCENT") {
  99       NoiseType = ePercent;
 100     } else if (variation == "ABSOLUTE") {
 101       NoiseType = eAbsolute;
 102     } else {
 103       NoiseType = ePercent;
 104       cerr << "Unknown noise type in sensor: " << Name << endl;
 105       cerr << "  defaulting to PERCENT." << endl;
 106     }
 107   }
 108
 109   FGFCSComponent::bind();
 110   bind();
 111
 112   Debug(0);
 113 }
 114
 115 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 116
 117 FGSensor::~FGSensor()
 118 {
 119   Debug(1);
 120 }
 121
 122 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 123
 124 bool FGSensor::Run(void )
 125 {
 126   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 127
 128   Output = Input; // perfect sensor
 129
 130   // Degrade signal as specified
 131
 132   if (fail_stuck) {
 133     Output = PreviousOutput;
 134     return true;
 135   }
 136
 137   if (lag != 0.0)            Lag();       // models sensor lag
 138   if (noise_variance != 0.0) Noise();     // models noise
 139   if (drift_rate != 0.0)     Drift();     // models drift over time
 140   if (bias != 0.0)           Bias();      // models a finite bias
 141
 142   if (fail_low)  Output = -HUGE_VAL;
 143   if (fail_high) Output =  HUGE_VAL;
 144
 145   if (bits != 0)             Quantize();  // models quantization degradation
 146 //  if (delay != 0.0)          Delay();     // models system signal transport latencies
 147
 148   return true;
 149 }
 150
 151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 152
 153 void FGSensor::Noise(void)
 154 {
 155   double random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
 156
 157   switch( NoiseType ) {
 158   case ePercent:
 159     Output *= (1.0 + noise_variance*random_value);
 160     break;
 161
 162   case eAbsolute:
 163     Output += noise_variance*random_value;
 164     break;
 165   }
 166 }
 167
 168 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 169
 170 void FGSensor::Bias(void)
 171 {
 172   Output += bias;
 173 }
 174
 175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 176
 177 void FGSensor::Drift(void)
 178 {
 179   drift += drift_rate*dt;
 180   Output += drift;
 181 }
 182
 183 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 184
 185 void FGSensor::Quantize(void)
 186 {
 187   if (Output < min) Output = min;
 188   if (Output > max) Output = max;
 189   double portion = Output - min;
 190   quantized = (int)(portion/granularity);
 191   Output = quantized*granularity + min;
 192 }
 193
 194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 195
 196 void FGSensor::Lag(void)
 197 {
 198   // "Output" on the right side of the "=" is the current frame input
 199   Output = ca * (Output + PreviousInput) + PreviousOutput * cb;
 200
 201   PreviousOutput = Output;
 202   PreviousInput  = Input;
 203 }
 204
 205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 206
 207 void FGSensor::bind(void)
 208 {
 209   string tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
 210   const string tmp_low = tmp + "/malfunction/fail_low";
 211   const string tmp_high = tmp + "/malfunction/fail_high";
 212   const string tmp_stuck = tmp + "/malfunction/fail_stuck";
 213
 214   PropertyManager->Tie( tmp_low, this, &FGSensor::GetFailLow, &FGSensor::SetFailLow);
 215   PropertyManager->Tie( tmp_high, this, &FGSensor::GetFailHigh, &FGSensor::SetFailHigh);
 216   PropertyManager->Tie( tmp_stuck, this, &FGSensor::GetFailStuck, &FGSensor::SetFailStuck);
 217 }
 218
 219 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 220 //    The bitmasked value choices are as follows:
 221 //    unset: In this case (the default) JSBSim would only print
 222 //       out the normally expected messages, essentially echoing
 223 //       the config files as they are read. If the environment
 224 //       variable is not set, debug_lvl is set to 1 internally
 225 //    0: This requests JSBSim not to output any messages
 226 //       whatsoever.
 227 //    1: This value explicity requests the normal JSBSim
 228 //       startup messages
 229 //    2: This value asks for a message to be printed out when
 230 //       a class is instantiated
 231 //    4: When this value is set, a message is displayed when a
 232 //       FGModel object executes its Run() method
 233 //    8: When this value is set, various runtime state variables
 234 //       are printed out periodically
 235 //    16: When set various parameters are sanity checked and
 236 //       a message is printed out when they go out of bounds
 237
 238 void FGSensor::Debug(int from)
 239 {
 240   if (debug_lvl <= 0) return;
 241
 242   if (debug_lvl & 1) { // Standard console startup message output
 243     if (from == 0) { // Constructor
 244
 245     }
 246   }
 247   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 248     if (from == 0) cout << "Instantiated: FGSensor" << endl;
 249     if (from == 1) cout << "Destroyed:    FGSensor" << endl;
 250   }
 251   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 252   }
 253   if (debug_lvl & 8 ) { // Runtime state variables
 254   }
 255   if (debug_lvl & 16) { // Sanity checking
 256   }
 257   if (debug_lvl & 64) {
 258     if (from == 0) { // Constructor
 259       cout << IdSrc << endl;
 260       cout << IdHdr << endl;
 261     }
 262   }
 263 }
 264 }