/**
* Attempt to compute airspeed when it is not yet available from:
* 1) dynamic pressure and air density derived from some altitude.
* 2) pitot pressure and static pressure.
* 3) ground speed and wind.
*/
static void
ComputeAirspeed(NMEAInfo &basic, const DerivedInfo &calculated)
{
if (basic.airspeed_available && basic.airspeed_real)
/* got it already */
return;
const auto any_altitude = basic.GetAnyAltitude();
if (!basic.airspeed_available && any_altitude.first) {
fixed dyn; bool available = false;
if (basic.dyn_pressure_available) {
dyn = basic.dyn_pressure.GetHectoPascal();
available = true;
} else if (basic.pitot_pressure_available && basic.static_pressure_available) {
dyn = basic.pitot_pressure.GetHectoPascal() - basic.static_pressure.GetHectoPascal();
available = true;
}
if (available) {
basic.indicated_airspeed = sqrt(fixed(163.2653061) * dyn);
basic.true_airspeed = basic.indicated_airspeed *
AirDensityRatio(any_altitude.second);
basic.airspeed_available.Update(basic.clock);
basic.airspeed_real = true; // Anyway not less real then any other method.
return;
}
}
if (!basic.ground_speed_available || !calculated.wind_available ||
!calculated.flight.flying) {
/* impossible to calculate */
basic.airspeed_available.Clear();
return;
}
fixed TrueAirspeedEstimated = fixed(0);
const SpeedVector wind = calculated.wind;
if (positive(basic.ground_speed) || wind.IsNonZero()) {
fixed x0 = basic.track.fastsine() * basic.ground_speed;
fixed y0 = basic.track.fastcosine() * basic.ground_speed;
x0 += wind.bearing.fastsine() * wind.norm;
y0 += wind.bearing.fastcosine() * wind.norm;
TrueAirspeedEstimated = SmallHypot(x0, y0);
}
basic.true_airspeed = TrueAirspeedEstimated;
basic.indicated_airspeed = TrueAirspeedEstimated;
if (any_altitude.first)
basic.indicated_airspeed /= AirDensityRatio(any_altitude.second);
basic.airspeed_available.Update(basic.clock);
basic.airspeed_real = false;
}
/**
* Update the measurements if new level reached
* @param basic NMEA_INFO for temperature and humidity
*/
void
CuSonde::UpdateMeasurements(const NMEAInfo &basic,
const DerivedInfo &calculated)
{
// if (not flying) nothing to update...
if (!calculated.flight.flying)
return;
// if (no temperature or humidity available) nothing to update...
if (!basic.temperature_available || !basic.humidity_available)
return;
// find appropriate level
const auto any_altitude = basic.GetAnyAltitude();
if (!any_altitude.first)
return;
unsigned short level = (unsigned short)((int)max(any_altitude.second,
fixed(0.0))
/ HEIGHT_STEP);
// if (level out of range) cancel update
if (level >= NUM_LEVELS)
return;
// if (level skipped) cancel update
if (abs(level - last_level) > 1) {
last_level = level;
return;
}
// if (no level transition yet) wait for transition
if (abs(level - last_level) == 0)
return;
// calculate ground height
hGround = calculated.altitude_agl;
// if (going up)
if (level > last_level) {
// we round down (level) because of potential lag of temp sensor
cslevels[level].UpdateTemps(basic.humidity,
KelvinToCelsius(basic.temperature));
fixed h_agl = fixed(level * HEIGHT_STEP) - hGround;
cslevels[level].UpdateThermalIndex(h_agl, maxGroundTemperature);
if (level > 0) {
FindThermalHeight((unsigned short)(level - 1));
FindCloudBase((unsigned short)(level - 1));
}
// if (going down)
} else {
// we round up (level+1) because of potential lag of temp sensor
cslevels[level + 1].UpdateTemps(basic.humidity,
KelvinToCelsius(basic.temperature));
fixed h_agl = fixed((level + 1) * HEIGHT_STEP) - hGround;
cslevels[level + 1].UpdateThermalIndex(h_agl, maxGroundTemperature);
if (level < NUM_LEVELS - 1) {
FindThermalHeight(level);
FindCloudBase(level);
}
}
last_level = level;
}
void
FlyingComputer::Compute(fixed takeoff_speed,
const NMEAInfo &basic,
const DerivedInfo &calculated,
FlyingState &flying)
{
if (!basic.time_available || !basic.location_available)
return;
const fixed dt = delta_time.Update(basic.time, fixed(0.5), fixed(20));
if (negative(dt)) {
Reset();
flying.Reset();
}
if (!positive(dt))
return;
const auto any_altitude = basic.GetAnyAltitude();
if (!basic.airspeed_available && !calculated.altitude_agl_valid &&
any_altitude.first && !negative(last_ground_altitude) &&
any_altitude.second > last_ground_altitude + fixed(250)) {
/* lower the threshold for "not moving" when the aircraft is high
above the take-off airfield and there's no airspeed probe; this
shall reduce the risk of false landing detection when flying in
strong head wind (e.g. ridge or wave) */
fixed dh = any_altitude.second - last_ground_altitude;
if (dh > fixed(1000))
takeoff_speed /= 4;
else if (dh > fixed(500))
takeoff_speed /= 2;
else
takeoff_speed = takeoff_speed * 2 / 3;
}
if (CheckTakeOffSpeed(takeoff_speed, basic) ||
CheckAltitudeAGL(calculated))
Moving(flying, basic.time, dt, basic.location);
else if (!flying.flying ||
(CheckLandingSpeed(takeoff_speed, basic) &&
(!any_altitude.first || !CheckClimbing(dt, any_altitude.second))))
Stationary(flying, basic.time, dt, basic.location);
if (basic.engine_noise_level_available)
CheckPowered(dt, basic, flying);
if (any_altitude.first) {
if (flying.on_ground)
last_ground_altitude = any_altitude.second;
CheckRelease(flying, basic.time, basic.location, any_altitude.second);
} else
sinking_since = fixed(-1);
if (flying.flying && flying.release_location.IsValid()) {
fixed distance = basic.location.Distance(flying.release_location);
if (distance > flying.far_distance) {
flying.far_location = basic.location;
flying.far_distance = distance;
}
}
}
