C++ NMEAInfo::GetAnyAltitude方法代码示例

/**
 * 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;
    }
  }
}