]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/models/flight_control/FGSensor.cpp
Hide some user generated files.
[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 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 General Public License for more
17  details.
18
19  You should have received a copy of the GNU 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 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 }