]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGSensor.cpp
Sync. with JSBSim CVS (header cleanups).
[flightgear.git] / 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 = 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   }
107
108   FGFCSComponent::bind();
109   bind();
110
111   Debug(0);
112 }
113
114 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115
116 FGSensor::~FGSensor()
117 {
118   Debug(1);
119 }
120
121 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122
123 bool FGSensor::Run(void )
124 {
125   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
126
127   Output = Input; // perfect sensor
128
129   // Degrade signal as specified
130
131   if (fail_stuck) {
132     Output = PreviousOutput;
133     return true;
134   }
135
136   if (lag != 0.0)            Lag();       // models sensor lag
137   if (noise_variance != 0.0) Noise();     // models noise
138   if (drift_rate != 0.0)     Drift();     // models drift over time
139   if (bias != 0.0)           Bias();      // models a finite bias
140
141   if (fail_low)  Output = -HUGE_VAL;
142   if (fail_high) Output =  HUGE_VAL;
143
144   if (bits != 0)             Quantize();  // models quantization degradation
145 //  if (delay != 0.0)          Delay();     // models system signal transport latencies
146
147   Clip(); // Is it right to clip a sensor?
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   if (!quant_property.empty()) {
219     if (quant_property.find("/") == string::npos) { // not found
220       string qprop = "fcs/" + PropertyManager->mkPropertyName(quant_property, true);
221       PropertyManager->Tie(qprop, this, &FGSensor::GetQuantized);
222     }
223   }
224
225 }
226
227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
228 //    The bitmasked value choices are as follows:
229 //    unset: In this case (the default) JSBSim would only print
230 //       out the normally expected messages, essentially echoing
231 //       the config files as they are read. If the environment
232 //       variable is not set, debug_lvl is set to 1 internally
233 //    0: This requests JSBSim not to output any messages
234 //       whatsoever.
235 //    1: This value explicity requests the normal JSBSim
236 //       startup messages
237 //    2: This value asks for a message to be printed out when
238 //       a class is instantiated
239 //    4: When this value is set, a message is displayed when a
240 //       FGModel object executes its Run() method
241 //    8: When this value is set, various runtime state variables
242 //       are printed out periodically
243 //    16: When set various parameters are sanity checked and
244 //       a message is printed out when they go out of bounds
245
246 void FGSensor::Debug(int from)
247 {
248   if (debug_lvl <= 0) return;
249
250   if (debug_lvl & 1) { // Standard console startup message output
251     if (from == 0) { // Constructor
252       if (InputSigns[0] < 0)
253         cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
254       else
255         cout << "      INPUT: " << InputNodes[0]->getName() << endl;
256
257       if (IsOutput) cout << "      OUTPUT: " << OutputNode->getName() << endl;
258       if (bits != 0) {
259         if (quant_property.empty())
260           cout << "      Quantized output" << endl;
261         else
262           cout << "      Quantized output (property: " << quant_property << ")" << endl;
263
264         cout << "        Bits: " << bits << endl;
265         cout << "        Min value: " << min << endl;
266         cout << "        Max value: " << max << endl;
267         cout << "          (span: " << span << ", granularity: " << granularity << ")" << endl;
268       }
269       if (bias != 0.0) cout << "      Bias: " << bias << endl;
270       if (drift_rate != 0) cout << "      Sensor drift rate: " << drift_rate << endl;
271       if (lag != 0) cout << "      Sensor lag: " << lag << endl;
272       if (noise_variance != 0) {
273         if (NoiseType == eAbsolute) {
274           cout << "      Noise variance (absolute): " << noise_variance << endl;
275         } else if (NoiseType == ePercent) {
276           cout << "      Noise variance (percent): " << noise_variance << endl;
277         } else {
278           cout << "      Noise variance type is invalid" << endl;
279         }
280       }
281     }
282   }
283   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
284     if (from == 0) cout << "Instantiated: FGSensor" << endl;
285     if (from == 1) cout << "Destroyed:    FGSensor" << endl;
286   }
287   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
288   }
289   if (debug_lvl & 8 ) { // Runtime state variables
290   }
291   if (debug_lvl & 16) { // Sanity checking
292   }
293   if (debug_lvl & 64) {
294     if (from == 0) { // Constructor
295       cout << IdSrc << endl;
296       cout << IdHdr << endl;
297     }
298   }
299 }
300 }