%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGSensor.h"
+#include "input_output/FGXMLElement.h"
+#include <iostream>
+#include <cstdlib>
+
+using namespace std;
namespace JSBSim {
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGSensor.cpp,v 1.23 2011/08/18 12:42:17 jberndt Exp $";
static const char *IdHdr = ID_SENSOR;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
{
double denom;
- dt = fcs->GetDt();
// inputs are read from the base class constructor
- bits = quantized = divisions = index = delay = 0;
+ bits = quantized = divisions = 0;
PreviousInput = PreviousOutput = 0.0;
- min = max = bias = noise_variance = lag = drift_rate = drift = span = 0.0;
+ min = max = bias = gain = noise_variance = lag = drift_rate = drift = span = 0.0;
granularity = 0.0;
noise_type = 0;
fail_low = fail_high = fail_stuck = false;
if ( element->FindElement("bias") ) {
bias = element->FindElementValueAsNumber("bias");
}
+ if ( element->FindElement("gain") ) {
+ gain = element->FindElementValueAsNumber("gain");
+ }
if ( element->FindElement("drift_rate") ) {
drift_rate = element->FindElementValueAsNumber("drift_rate");
}
cerr << " defaulting to UNIFORM." << endl;
}
}
- if ( element->FindElement("delay") ) {
- delay = (unsigned int)element->FindElementValueAsNumber("delay");
- output_array.resize(delay);
- for (unsigned int i=0; i<delay; i++) output_array[i] = 0.0;
- }
FGFCSComponent::bind();
bind();
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-bool FGSensor::Run(void )
+bool FGSensor::Run(void)
{
Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
+ ProcessSensorSignal();
+
+ return true;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGSensor::ProcessSensorSignal(void)
+{
Output = Input; // perfect sensor
// Degrade signal as specified
if (fail_stuck) {
Output = PreviousOutput;
- return true;
- }
-
- if (lag != 0.0) Lag(); // models sensor lag and filter
- if (noise_variance != 0.0) Noise(); // models noise
- if (drift_rate != 0.0) Drift(); // models drift over time
- if (bias != 0.0) Bias(); // models a finite bias
+ } else {
+ if (lag != 0.0) Lag(); // models sensor lag and filter
+ if (noise_variance != 0.0) Noise(); // models noise
+ if (drift_rate != 0.0) Drift(); // models drift over time
+ if (gain != 0.0) Gain(); // models a finite gain
+ if (bias != 0.0) Bias(); // models a finite bias
- if (delay != 0.0) Delay(); // models system signal transport latencies
+ if (delay != 0) Delay(); // models system signal transport latencies
- if (fail_low) Output = -HUGE_VAL;
- if (fail_high) Output = HUGE_VAL;
+ if (fail_low) Output = -HUGE_VAL;
+ if (fail_high) Output = HUGE_VAL;
- if (bits != 0) Quantize(); // models quantization degradation
+ if (bits != 0) Quantize(); // models quantization degradation
- Clip(); // Is it right to clip a sensor?
- return true;
+ Clip();
+ }
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
double random_value=0.0;
if (DistributionType == eUniform) {
- random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
+ random_value = 2.0*(((double)rand()/(double)RAND_MAX) - 0.5);
} else {
random_value = GaussianRandomNumber();
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+void FGSensor::Gain(void)
+{
+ Output *= gain;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGSensor::Drift(void)
{
drift += drift_rate*dt;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-void FGSensor::Delay(void)
-{
- output_array[index] = Output;
- if (index == delay-1) index = 0;
- else index++;
- Output = output_array[index];
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
void FGSensor::bind(void)
{
string tmp = Name;
if (from == 0) { // Constructor
if (InputSigns.size() > 0) {
if (InputSigns[0] < 0)
- cout << " INPUT: -" << InputNodes[0]->getName() << endl;
+ cout << " INPUT: -" << InputNodes[0]->GetName() << endl;
else
- cout << " INPUT: " << InputNodes[0]->getName() << endl;
+ cout << " INPUT: " << InputNodes[0]->GetName() << endl;
}
- if (delay > 0) cout <<" Frame delay: " << delay
- << " frames (" << delay*dt << " sec)" << endl;
if (bits != 0) {
if (quant_property.empty())
cout << " Quantized output" << endl;
cout << " (span: " << span << ", granularity: " << granularity << ")" << endl;
}
if (bias != 0.0) cout << " Bias: " << bias << endl;
+ if (gain != 0.0) cout << " Gain: " << gain << endl;
if (drift_rate != 0) cout << " Sensor drift rate: " << drift_rate << endl;
if (lag != 0) cout << " Sensor lag: " << lag << endl;
if (noise_variance != 0) {
cout << " Random noise is gaussian distributed." << endl;
}
}
- if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl;
+ if (IsOutput) {
+ for (unsigned int i=0; i<OutputNodes.size(); i++)
+ cout << " OUTPUT: " << OutputNodes[i]->getName() << endl;
+ }
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification