bool FGSensor::Run(void )
{
Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
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
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
Clip(); // Is it right to clip a sensor?
return true;
}
void FGSensor::ProcessSensorSignal(void)
{
Output = Input; // perfect sensor
// Degrade signal as specified
if (fail_stuck) {
Output = PreviousOutput;
} else if (fcs->GetTrimStatus()) {
if (lag != 0.0) {PreviousOutput = Output; PreviousInput = Input;}
if (drift_rate != 0.0) drift = 0;
if (gain != 0.0) Gain(); // models a finite gain
if (bias != 0.0) Bias(); // models a finite bias
if (delay != 0) for (int i=0; i<delay; i++) output_array[i] = Output;
Clip();
} 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) 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
Clip();
}
if (IsOutput) SetOutput();
}
void FGSensor::ProcessSensorSignal(void)
{
Output = Input; // perfect sensor
// Degrade signal as specified
if (fail_stuck) {
Output = PreviousOutput;
} 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) 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
Clip();
}
}
void
ThermalLocator::Update(const fixed t_0,
const GeoPoint &location_0,
const SpeedVector wind,
ThermalLocatorInfo &therm)
{
if (n_points < TLOCATOR_NMIN) {
therm.estimate_valid = false;
return; // nothing to do.
}
GeoPoint dloc = FindLatitudeLongitude(location_0, wind.bearing, wind.norm);
TaskProjection projection;
projection.reset(location_0);
projection.update_fast();
// drift points
Drift(t_0, projection, location_0 - dloc);
FlatPoint av = glider_average();
// find thermal center relative to glider's average position
FlatPoint f0(fixed_zero, fixed_zero);
fixed acc = fixed_zero;
for (unsigned i = 0; i < n_points; ++i) {
f0 += (points[i].loc_drift-av)*points[i].lift_weight;
acc += points[i].lift_weight;
}
// if sufficient data, estimate location
if (!positive(acc)) {
therm.estimate_valid = false;
return;
}
f0 = f0 * (fixed_one/acc) + av;
therm.estimate_location = projection.funproject(f0);
therm.estimate_valid = true;
}
