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