1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5 Date started: 9 July 2005
7 ------------- Copyright (C) 2005 -------------
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
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
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.
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.
26 FUNCTIONAL DESCRIPTION
27 --------------------------------------------------------------------------------
30 --------------------------------------------------------------------------------
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 COMMENTS, REFERENCES, and NOTES
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
44 static const char *IdSrc = "$Id$";
45 static const char *IdHdr = ID_SENSOR;
47 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
57 // inputs are read from the base class constructor
59 bits = quantized = divisions = index = delay = 0;
60 PreviousInput = PreviousOutput = 0.0;
61 min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
64 fail_low = fail_high = fail_stuck = false;
66 Element* quantization_element = element->FindElement("quantization");
67 if ( quantization_element) {
68 if ( quantization_element->FindElement("bits") ) {
69 bits = (int)quantization_element->FindElementValueAsNumber("bits");
71 divisions = (1<<bits);
72 if ( quantization_element->FindElement("min") ) {
73 min = quantization_element->FindElementValueAsNumber("min");
75 if ( quantization_element->FindElement("max") ) {
76 max = quantization_element->FindElementValueAsNumber("max");
78 quant_property = quantization_element->GetAttributeValue("name");
80 granularity = span/divisions;
82 if ( element->FindElement("bias") ) {
83 bias = element->FindElementValueAsNumber("bias");
85 if ( element->FindElement("gain") ) {
86 gain = element->FindElementValueAsNumber("gain");
88 if ( element->FindElement("drift_rate") ) {
89 drift_rate = element->FindElementValueAsNumber("drift_rate");
91 if ( element->FindElement("lag") ) {
92 lag = element->FindElementValueAsNumber("lag");
93 denom = 2.00 + dt*lag;
95 cb = (2.00 - dt*lag) / denom;
97 if ( element->FindElement("noise") ) {
98 noise_variance = element->FindElementValueAsNumber("noise");
99 string variation = element->FindElement("noise")->GetAttributeValue("variation");
100 if (variation == "PERCENT") {
101 NoiseType = ePercent;
102 } else if (variation == "ABSOLUTE") {
103 NoiseType = eAbsolute;
105 NoiseType = ePercent;
106 cerr << "Unknown noise type in sensor: " << Name << endl;
107 cerr << " defaulting to PERCENT." << endl;
109 string distribution = element->FindElement("noise")->GetAttributeValue("distribution");
110 if (distribution == "UNIFORM") {
111 DistributionType = eUniform;
112 } else if (distribution == "GAUSSIAN") {
113 DistributionType = eGaussian;
115 DistributionType = eUniform;
116 cerr << "Unknown random distribution type in sensor: " << Name << endl;
117 cerr << " defaulting to UNIFORM." << endl;
120 if ( element->FindElement("delay") ) {
121 delay = (unsigned int)element->FindElementValueAsNumber("delay");
122 output_array.resize(delay);
123 for (unsigned int i=0; i<delay; i++) output_array[i] = 0.0;
126 FGFCSComponent::bind();
132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
134 FGSensor::~FGSensor()
139 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
141 bool FGSensor::Run(void )
143 Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
145 Output = Input; // perfect sensor
147 // Degrade signal as specified
150 Output = PreviousOutput;
154 if (lag != 0.0) Lag(); // models sensor lag and filter
155 if (noise_variance != 0.0) Noise(); // models noise
156 if (drift_rate != 0.0) Drift(); // models drift over time
157 if (bias != 0.0) Bias(); // models a finite bias
158 if (gain != 0.0) Gain(); // models a finite gain
160 if (delay != 0.0) Delay(); // models system signal transport latencies
162 if (fail_low) Output = -HUGE_VAL;
163 if (fail_high) Output = HUGE_VAL;
165 if (bits != 0) Quantize(); // models quantization degradation
167 Clip(); // Is it right to clip a sensor?
171 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
173 void FGSensor::Noise(void)
175 double random_value=0.0;
177 if (DistributionType == eUniform) {
178 random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
180 random_value = GaussianRandomNumber();
183 switch( NoiseType ) {
185 Output *= (1.0 + noise_variance*random_value);
189 Output += noise_variance*random_value;
194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
196 void FGSensor::Bias(void)
201 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
203 void FGSensor::Gain(void)
208 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
210 void FGSensor::Drift(void)
212 drift += drift_rate*dt;
216 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
218 void FGSensor::Quantize(void)
220 if (Output < min) Output = min;
221 if (Output > max) Output = max;
222 double portion = Output - min;
223 quantized = (int)(portion/granularity);
224 Output = quantized*granularity + min;
227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
229 void FGSensor::Lag(void)
231 // "Output" on the right side of the "=" is the current input
232 Output = ca * (Output + PreviousInput) + PreviousOutput * cb;
234 PreviousOutput = Output;
235 PreviousInput = Input;
238 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
240 void FGSensor::Delay(void)
242 output_array[index] = Output;
243 if (index == delay-1) index = 0;
245 Output = output_array[index];
248 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
250 void FGSensor::bind(void)
253 if (Name.find("/") == string::npos) {
254 tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
256 const string tmp_low = tmp + "/malfunction/fail_low";
257 const string tmp_high = tmp + "/malfunction/fail_high";
258 const string tmp_stuck = tmp + "/malfunction/fail_stuck";
260 PropertyManager->Tie( tmp_low, this, &FGSensor::GetFailLow, &FGSensor::SetFailLow);
261 PropertyManager->Tie( tmp_high, this, &FGSensor::GetFailHigh, &FGSensor::SetFailHigh);
262 PropertyManager->Tie( tmp_stuck, this, &FGSensor::GetFailStuck, &FGSensor::SetFailStuck);
264 if (!quant_property.empty()) {
265 if (quant_property.find("/") == string::npos) { // not found
266 string qprop = "fcs/" + PropertyManager->mkPropertyName(quant_property, true);
267 PropertyManager->Tie(qprop, this, &FGSensor::GetQuantized);
273 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
274 // The bitmasked value choices are as follows:
275 // unset: In this case (the default) JSBSim would only print
276 // out the normally expected messages, essentially echoing
277 // the config files as they are read. If the environment
278 // variable is not set, debug_lvl is set to 1 internally
279 // 0: This requests JSBSim not to output any messages
281 // 1: This value explicity requests the normal JSBSim
283 // 2: This value asks for a message to be printed out when
284 // a class is instantiated
285 // 4: When this value is set, a message is displayed when a
286 // FGModel object executes its Run() method
287 // 8: When this value is set, various runtime state variables
288 // are printed out periodically
289 // 16: When set various parameters are sanity checked and
290 // a message is printed out when they go out of bounds
292 void FGSensor::Debug(int from)
294 if (debug_lvl <= 0) return;
296 if (debug_lvl & 1) { // Standard console startup message output
297 if (from == 0) { // Constructor
298 if (InputSigns.size() > 0) {
299 if (InputSigns[0] < 0)
300 cout << " INPUT: -" << InputNodes[0]->getName() << endl;
302 cout << " INPUT: " << InputNodes[0]->getName() << endl;
304 if (delay > 0) cout <<" Frame delay: " << delay
305 << " frames (" << delay*dt << " sec)" << endl;
307 if (quant_property.empty())
308 cout << " Quantized output" << endl;
310 cout << " Quantized output (property: " << quant_property << ")" << endl;
312 cout << " Bits: " << bits << endl;
313 cout << " Min value: " << min << endl;
314 cout << " Max value: " << max << endl;
315 cout << " (span: " << span << ", granularity: " << granularity << ")" << endl;
317 if (bias != 0.0) cout << " Bias: " << bias << endl;
318 if (gain != 0.0) cout << " Gain: " << gain << endl;
319 if (drift_rate != 0) cout << " Sensor drift rate: " << drift_rate << endl;
320 if (lag != 0) cout << " Sensor lag: " << lag << endl;
321 if (noise_variance != 0) {
322 if (NoiseType == eAbsolute) {
323 cout << " Noise variance (absolute): " << noise_variance << endl;
324 } else if (NoiseType == ePercent) {
325 cout << " Noise variance (percent): " << noise_variance << endl;
327 cout << " Noise variance type is invalid" << endl;
329 if (DistributionType == eUniform) {
330 cout << " Random noise is uniformly distributed." << endl;
331 } else if (DistributionType == eGaussian) {
332 cout << " Random noise is gaussian distributed." << endl;
335 if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl;
338 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
339 if (from == 0) cout << "Instantiated: FGSensor" << endl;
340 if (from == 1) cout << "Destroyed: FGSensor" << endl;
342 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
344 if (debug_lvl & 8 ) { // Runtime state variables
346 if (debug_lvl & 16) { // Sanity checking
348 if (debug_lvl & 64) {
349 if (from == 0) { // Constructor
350 cout << IdSrc << endl;
351 cout << IdHdr << endl;