FGSensor::FGSensor(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
{
double denom;
- dt = fcs->GetState()->Getdt();
+ dt = fcs->GetDt();
// inputs are read from the base class constructor
- dt = fcs->GetState()->Getdt();
-
- bits = quantized = divisions = 0;
+ bits = quantized = divisions = index = delay = 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 ( quantization_element->FindElement("max") ) {
max = quantization_element->FindElementValueAsNumber("max");
}
+ quant_property = quantization_element->GetAttributeValue("name");
span = max - min;
granularity = span/divisions;
}
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 << "Unknown noise type in sensor: " << Name << endl;
cerr << " defaulting to PERCENT." << endl;
}
+ string distribution = element->FindElement("noise")->GetAttributeValue("distribution");
+ if (distribution == "UNIFORM") {
+ DistributionType = eUniform;
+ } else if (distribution == "GAUSSIAN") {
+ DistributionType = eGaussian;
+ } else {
+ DistributionType = eUniform;
+ cerr << "Unknown random distribution type in sensor: " << Name << endl;
+ 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();
return true;
}
- if (lag != 0.0) Lag(); // models sensor lag
+ 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
+ if (gain != 0.0) Gain(); // models a finite gain
+
+ if (delay != 0.0) Delay(); // models system signal transport latencies
if (fail_low) Output = -HUGE_VAL;
if (fail_high) Output = HUGE_VAL;
if (bits != 0) Quantize(); // models quantization degradation
-// if (delay != 0.0) Delay(); // models system signal transport latencies
+ Clip(); // Is it right to clip a sensor?
return true;
}
void FGSensor::Noise(void)
{
- double random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
+ double random_value=0.0;
+
+ if (DistributionType == eUniform) {
+ random_value = ((double)rand()/(double)RAND_MAX) - 0.5;
+ } else {
+ random_value = GaussianRandomNumber();
+ }
switch( NoiseType ) {
case ePercent:
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+void FGSensor::Gain(void)
+{
+ Output *= gain;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGSensor::Drift(void)
{
drift += drift_rate*dt;
void FGSensor::Lag(void)
{
- // "Output" on the right side of the "=" is the current frame input
+ // "Output" on the right side of the "=" is the current input
Output = ca * (Output + PreviousInput) + PreviousOutput * cb;
PreviousOutput = Output;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+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 = "fcs/" + PropertyManager->mkPropertyName(Name, true);
+ string tmp = Name;
+ if (Name.find("/") == string::npos) {
+ tmp = "fcs/" + PropertyManager->mkPropertyName(Name, true);
+ }
const string tmp_low = tmp + "/malfunction/fail_low";
const string tmp_high = tmp + "/malfunction/fail_high";
const string tmp_stuck = tmp + "/malfunction/fail_stuck";
PropertyManager->Tie( tmp_low, this, &FGSensor::GetFailLow, &FGSensor::SetFailLow);
PropertyManager->Tie( tmp_high, this, &FGSensor::GetFailHigh, &FGSensor::SetFailHigh);
PropertyManager->Tie( tmp_stuck, this, &FGSensor::GetFailStuck, &FGSensor::SetFailStuck);
+
+ if (!quant_property.empty()) {
+ if (quant_property.find("/") == string::npos) { // not found
+ string qprop = "fcs/" + PropertyManager->mkPropertyName(quant_property, true);
+ PropertyManager->Tie(qprop, this, &FGSensor::GetQuantized);
+ }
+ }
+
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (debug_lvl & 1) { // Standard console startup message output
if (from == 0) { // Constructor
-
+ if (InputSigns.size() > 0) {
+ if (InputSigns[0] < 0)
+ cout << " INPUT: -" << InputNodes[0]->getName() << endl;
+ else
+ 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;
+ else
+ cout << " Quantized output (property: " << quant_property << ")" << endl;
+
+ cout << " Bits: " << bits << endl;
+ cout << " Min value: " << min << endl;
+ cout << " Max value: " << max << 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) {
+ if (NoiseType == eAbsolute) {
+ cout << " Noise variance (absolute): " << noise_variance << endl;
+ } else if (NoiseType == ePercent) {
+ cout << " Noise variance (percent): " << noise_variance << endl;
+ } else {
+ cout << " Noise variance type is invalid" << endl;
+ }
+ if (DistributionType == eUniform) {
+ cout << " Random noise is uniformly distributed." << endl;
+ } else if (DistributionType == eGaussian) {
+ cout << " Random noise is gaussian distributed." << endl;
+ }
+ }
+ if (IsOutput) cout << " OUTPUT: " << OutputNode->getName() << endl;
}
}
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
projects / flightgear.git / blobdiff