Exemple #1
0
void
MapItemListBuilder::AddArrivalAltitudes(
    const ProtectedRoutePlanner &route_planner,
    const RasterTerrain *terrain, fixed safety_height)
{
  if (list.full())
    return;

  // Calculate terrain elevation if possible
  short elevation;
  if (terrain != NULL)
    elevation = terrain->GetTerrainHeight(location);
  else
    elevation = RasterBuffer::TERRAIN_INVALID;

  // Calculate target altitude
  RoughAltitude safety_elevation(safety_height);
  if (!RasterBuffer::IsInvalid(elevation))
    safety_elevation += RoughAltitude(elevation);

  // Save destination point incl. elevation and safety height
  const AGeoPoint destination(location, safety_elevation);

  // Calculate arrival altitudes
  ReachResult reach;

  ProtectedRoutePlanner::Lease leased_route_planner(route_planner);
  if (!leased_route_planner->FindPositiveArrival(destination, reach))
    return;

  reach.Subtract(RoughAltitude(safety_height));

  list.append(new ArrivalAltitudeMapItem(RoughAltitude(elevation), reach));
}
Exemple #2
0
int main(int argc, char** argv) {

  const char hc_path[] = "tmp/terrain";
  const char *map_path;
  if ((argc<2) || !strlen(argv[0])) {
    map_path = hc_path;
  } else {
    map_path = argv[0];
  }

  TCHAR jp2_path[4096];
  _tcscpy(jp2_path, PathName(map_path));
  _tcscat(jp2_path, _T(DIR_SEPARATOR_S) _T("terrain.jp2"));

  TCHAR j2w_path[4096];
  _tcscpy(j2w_path, PathName(map_path));
  _tcscat(j2w_path, _T(DIR_SEPARATOR_S) _T("terrain.j2w"));

  NullOperationEnvironment operation;
  RasterMap map(jp2_path, j2w_path, NULL, operation);
  do {
    map.SetViewCenter(map.GetMapCenter(), fixed(100000));
  } while (map.IsDirty());

  plan_tests(16*3);
  test_troute(map, fixed(0), fixed(0.1), RoughAltitude(10000));
  test_troute(map, fixed(0), fixed(0), RoughAltitude(10000));
  test_troute(map, fixed(5.0), fixed(1), RoughAltitude(10000));

  return exit_status();
}
Exemple #3
0
static void test_reach(const RasterMap& map, fixed mwind, fixed mc)
{
  GlideSettings settings;
  settings.SetDefaults();
  GlidePolar polar(mc);
  SpeedVector wind(Angle::Degrees(0), mwind);
  TerrainRoute route;
  route.UpdatePolar(settings, polar, polar, wind);
  route.SetTerrain(&map);

  GeoPoint origin(map.GetMapCenter());

  fixed pd = map.PixelDistance(origin, 1);
  printf("# pixel size %g\n", (double)pd);

  bool retval= true;

  short horigin = map.GetHeight(origin)+1000;
  AGeoPoint aorigin(origin, RoughAltitude(horigin));

  RoutePlannerConfig config;
  config.SetDefaults();
  retval = route.SolveReach(aorigin, config, RoughAltitude::Max());

  ok(retval, "reach solve", 0);

  PrintHelper::print_reach_tree(route);

  GeoPoint dest(origin.longitude-Angle::Degrees(0.02),
                origin.latitude-Angle::Degrees(0.02));

  {
    Directory::Create(_T("output/results"));
    std::ofstream fout("output/results/terrain.txt");
    unsigned nx = 100;
    unsigned ny = 100;
    for (unsigned i=0; i< nx; ++i) {
      for (unsigned j=0; j< ny; ++j) {
        fixed fx = (fixed)i / (nx - 1) * 2 - fixed(1);
        fixed fy = (fixed)j / (ny - 1) * 2 - fixed(1);
        GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.6) * fx),
                   origin.latitude + Angle::Degrees(fixed(0.6) * fy));
        short h = map.GetInterpolatedHeight(x);
        AGeoPoint adest(x, RoughAltitude(h));
        ReachResult reach;
        route.FindPositiveArrival(adest, reach);
        if ((i % 5 == 0) && (j % 5 == 0)) {
          AGeoPoint ao2(x, RoughAltitude(h + 1000));
          route.SolveReach(ao2, config, RoughAltitude::Max());
        }
        fout << x.longitude.Degrees() << " "
             << x.latitude.Degrees() << " "
             << h << " " << (int)reach.terrain << "\n";
      }
      fout << "\n";
    }
    fout << "\n";
  }
}
Exemple #4
0
bool
ReachFan::FindPositiveArrival(const AGeoPoint dest, const RoutePolars &rpolars,
                              ReachResult &result_r) const
{
  if (root.IsEmpty())
    return false;

  const FlatGeoPoint d(task_proj.ProjectInteger(dest));
  const ReachFanParms parms(rpolars, task_proj, (int)terrain_base);

  result_r.Clear();

  // first calculate direct (terrain-independent height)
  result_r.direct = root.DirectArrival(d, parms);

  if (root.IsDummy())
    /* terrain reach is not available, stop here */
    return true;

  // if can't reach even with no terrain, exit early
  if (std::min(root.GetHeight(), result_r.direct) < dest.altitude) {
    result_r.terrain = result_r.direct;
    result_r.terrain_valid = ReachResult::Validity::UNREACHABLE;
    return true;
  }

  // now calculate turning solution
  result_r.terrain = dest.altitude - RoughAltitude(1);
  result_r.terrain_valid = root.FindPositiveArrival(d, parms, result_r.terrain)
    ? ReachResult::Validity::VALID
    : ReachResult::Validity::UNREACHABLE;

  return true;
}
bool
FlatTriangleFanTree::CheckGap(const AFlatGeoPoint &n, const RouteLink &e_1,
                              const RouteLink &e_2, ReachFanParms &parms)
{
  const bool side = (e_1.d > e_2.d);
  const RouteLink &e_long = (side ? e_1 : e_2);
  const RouteLink &e_short = (side ? e_2 : e_1);
  if (e_short.d >= e_long.d)
    return false;

  const FlatGeoPoint &p_long = (side ? e_1.first : e_2.first);

  // return true if this gap was caught (applicable) whether or not it generated
  // a change

  const fixed f0 = e_short.d * e_long.inv_d;
  const RoughAltitude h_loss =
      parms.rpolars.CalcGlideArrival(n, p_long, parms.task_proj) - n.altitude;

  const FlatGeoPoint dp(p_long - n);
  // scan from n-p_long to perpendicular to n-p_long

  int index_left, index_right;
  if (!side) {
    index_left = e_long.polar_index - REACH_SWEEP;
    index_right = e_long.polar_index - REACH_BUFFER;
  } else {
    index_left = e_long.polar_index + REACH_BUFFER;
    index_right = e_long.polar_index + REACH_SWEEP;
  }

  children.emplace_back(depth + 1);
  FlatTriangleFanTree &child = children.back();

  for (fixed f = f0; f < fixed(0.9); f += fixed(0.1)) {
    // find corner point
    const FlatGeoPoint px = (dp * f + n);
    // position x is length (n to p_short) along (n to p_long)
    const RoughAltitude h = n.altitude + RoughAltitude(f * h_loss);

    // altitude calculated from pure glide from n to x
    const AFlatGeoPoint x(px, h);

    child.FillReach(x, index_left, index_right, parms);

    // prune child if empty or single spike
    if (child.vs.size() > 3) {
      parms.vertex_counter += child.vs.size();
      parms.fan_counter++;
      return true;
    }

    child.vs.clear();
  }

  // don't need the child
  children.pop_back();

  return false;
}
Exemple #6
0
RoughAltitude
RoutePolars::CalcGlideArrival(const AFlatGeoPoint& origin,
                                const FlatGeoPoint& dest,
                                const TaskProjection& proj) const
{
  const RouteLink e(RoutePoint(dest, RoughAltitude(0)), origin, proj);
  return origin.altitude - CalcVHeight(e);
}
Exemple #7
0
  void visit_abstract(const AbstractAirspace &as) {
    assert(!m_intersections.empty());

    GeoPoint point = m_intersections[0].first;

    RouteLink l = rpolar.GenerateIntermediate(link.first,
                                               RoutePoint(proj.project(point), link.second.altitude),
                                               proj);

    if (l.second.altitude < RoughAltitude(as.GetBase().altitude) ||
        l.second.altitude > RoughAltitude(as.GetTop().altitude))
      return;

    if (negative(min_distance) || (l.d < min_distance)) {
      min_distance = l.d;
      nearest = std::make_pair(&as, l.second);
    }
  }
Exemple #8
0
int main(int argc, char** argv) {
  static const char hc_path[] = "tmp/map.xcm";
  const char *map_path;
  if ((argc<2) || !strlen(argv[0])) {
    map_path = hc_path;
  } else {
    map_path = argv[0];
  }

  ZZIP_DIR *dir = zzip_dir_open(map_path, nullptr);
  if (dir == nullptr) {
    fprintf(stderr, "Failed to open %s\n", map_path);
    return EXIT_FAILURE;
  }

  RasterMap map;

  NullOperationEnvironment operation;
  if (!LoadTerrainOverview(dir, map.GetTileCache(),
                           operation)) {
    fprintf(stderr, "failed to load map\n");
    zzip_dir_close(dir);
    return EXIT_FAILURE;
  }

  SharedMutex mutex;
  do {
    UpdateTerrainTiles(dir, map.GetTileCache(), mutex,
                       map.GetProjection(),
                       map.GetMapCenter(), fixed(100000));
  } while (map.IsDirty());
  zzip_dir_close(dir);

  plan_tests(16*3);
  test_troute(map, fixed(0), fixed(0.1), RoughAltitude(10000));
  test_troute(map, fixed(0), fixed(0), RoughAltitude(10000));
  test_troute(map, fixed(5.0), fixed(1), RoughAltitude(10000));

  return exit_status();
}
void
FlatTriangleFanTree::FillGaps(const AFlatGeoPoint &origin, ReachFanParms &parms)
{
  // worth checking for gaps?
  if (vs.size() > 2 && parms.rpolars.IsTurningReachEnabled()) {

    // now check gaps
    const RoutePoint o(origin, RoughAltitude(0));
    RouteLink e_last(RoutePoint(*vs.begin(), RoughAltitude(0)),
                     o, parms.task_proj);
    for (auto x_last = vs.cbegin(), end = vs.cend(),
         x = x_last + 1; x != end; x_last = x++) {
      if (TooClose(*x, origin) || TooClose(*x_last, origin))
        continue;

      const RouteLink e(RoutePoint(*x, RoughAltitude(0)), o, parms.task_proj);
      // check if children need to be added
      CheckGap(origin, e_last, e, parms);

      e_last = e;
    }
  }
}
Exemple #10
0
gcc_pure
NearestAirspace
NearestAirspace::FindHorizontal(const MoreData &basic,
                                const ProtectedAirspaceWarningManager &airspace_warnings,
                                const Airspaces &airspace_database)
{
  if (!basic.location_available)
    /* can't check for airspaces without a GPS fix */
    return NearestAirspace();

  /* find the nearest airspace */
  //consider only active airspaces
  const WrapAirspacePredicate<ActiveAirspacePredicate> active_predicate(&airspace_warnings);
  const WrapAirspacePredicate<OutsideAirspacePredicate> outside_predicate(AGeoPoint(basic.location, RoughAltitude(0)));
  const AndAirspacePredicate outside_and_active_predicate(active_predicate, outside_predicate);
  const Airspace *airspace;

  //if altitude is available, filter airspaces in same height as airplane
  if (basic.NavAltitudeAvailable()) {
    /* check altitude; hard-coded margin of 50m (for now) */
    WrapAirspacePredicate<AirspacePredicateHeightRange> height_range_predicate(RoughAltitude(basic.nav_altitude-fixed(50)),
                                                                               RoughAltitude(basic.nav_altitude+fixed(50)));
     AndAirspacePredicate and_predicate(outside_and_active_predicate, height_range_predicate);
     airspace = airspace_database.FindNearest(basic.location, and_predicate);
  } else //only filter outside and active
    airspace = airspace_database.FindNearest(basic.location, outside_and_active_predicate);

  if (airspace == nullptr)
    return NearestAirspace();

  const AbstractAirspace &as = airspace->GetAirspace();

  /* calculate distance to the nearest point */
  const GeoPoint closest = as.ClosestPoint(basic.location,
                                           airspace_database.GetProjection());
  return NearestAirspace(as, basic.location.Distance(closest));
}
Exemple #11
0
bool
RoutePolars::CheckClearance(const RouteLink &e, const RasterMap* map,
                             const TaskProjection &proj, RoutePoint& inp) const
{
  if (!config.IsTerrainEnabled())
    return true;

  GeoPoint int_x;
  int int_h;
  GeoPoint start = proj.Unproject(e.first);
  GeoPoint dest = proj.Unproject(e.second);

  assert(map);

  if (!map->FirstIntersection(start, (int)e.first.altitude, dest,
                              (int)e.second.altitude, (int)CalcVHeight(e),
                              (int)climb_ceiling, (int)GetSafetyHeight(),
                              int_x, int_h))
    return true;

  inp = RoutePoint(proj.ProjectInteger(int_x), RoughAltitude(int_h));
  return false;
}
Exemple #12
0
unsigned
RoutePolars::CalcTime(const RouteLink& link) const
{
  const RoughAltitude dh = link.second.altitude - link.first.altitude;
  if (dh.IsNegative() && !positive(inv_mc))
    // impossible, can't climb
    return UINT_MAX;

  // dh/d = gradient
  const fixed rho = dh.IsPositive() ?
    std::min(fixed(1), (dh * link.inv_d *
                         polar_glide.GetPoint(link.polar_index).inv_gradient)) :
    fixed(0);

  if ((rho < fixed(1)) && !polar_cruise.GetPoint(link.polar_index).valid)
    // impossible, can't cruise
    return UINT_MAX;

  if (positive(rho) && !polar_glide.GetPoint(link.polar_index).valid)
    // impossible, can't glide
    return UINT_MAX;

  const int t_cruise = (int)(
    link.d * (rho * polar_glide.GetPoint(link.polar_index).slowness +
    (fixed(1) - rho) * polar_cruise.GetPoint(link.polar_index).slowness));

  if (link.second.altitude > cruise_altitude) {
    // penalise any climbs required above cruise altitude
    const RoughAltitude h_penalty = std::max(
        RoughAltitude(0),
        link.second.altitude - std::max(cruise_altitude, link.first.altitude));
    return t_cruise + (int)(h_penalty * inv_mc);
  }

  return t_cruise;
}
Exemple #13
0
static bool
test_route(const unsigned n_airspaces, const RasterMap& map)
{
  Airspaces airspaces;
  setup_airspaces(airspaces, map.GetMapCenter(), n_airspaces);

  {
    std::ofstream fout("results/terrain.txt");

    unsigned nx = 100;
    unsigned ny = 100;
    GeoPoint origin(map.GetMapCenter());

    for (unsigned i = 0; i < nx; ++i) {
      for (unsigned j = 0; j < ny; ++j) {
        fixed fx = (fixed)i / (nx - 1) * fixed(2.0) - fixed_one;
        fixed fy = (fixed)j / (ny - 1) * fixed(2.0) - fixed_one;
        GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.2) + fixed(0.7) * fx),
                   origin.latitude + Angle::Degrees(fixed(0.9) * fy));
        short h = map.GetInterpolatedHeight(x);
        fout << x.longitude.Degrees() << " " << x.latitude.Degrees()
             << " " << h << "\n";
      }

      fout << "\n";
    }

    fout << "\n";
  }

  {
    // local scope, see what happens when we go out of scope
    GeoPoint p_start(Angle::Degrees(fixed(-0.3)), Angle::Degrees(fixed(0.0)));
    p_start += map.GetMapCenter();

    GeoPoint p_dest(Angle::Degrees(fixed(0.8)), Angle::Degrees(fixed(-0.7)));
    p_dest += map.GetMapCenter();

    AGeoPoint loc_start(p_start, RoughAltitude(map.GetHeight(p_start) + 100));
    AGeoPoint loc_end(p_dest, RoughAltitude(map.GetHeight(p_dest) + 100));

    AircraftState state;
    GlidePolar glide_polar(fixed(0.1));
    AirspaceAircraftPerformanceGlide perf(glide_polar);

    GeoVector vec(loc_start, loc_end);
    fixed range = fixed(10000) + vec.distance / 2;

    state.location = loc_start;
    state.altitude = loc_start.altitude;

    {
      Airspaces as_route(airspaces, false);
      // dummy

      // real one, see if items changed
      as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range);
      int size_1 = as_route.size();
      if (verbose)
        printf("# route airspace size %d\n", size_1);

      as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), fixed_one);
      int size_2 = as_route.size();
      if (verbose)
        printf("# route airspace size %d\n", size_2);

      ok(size_2 < size_1, "shrink as", 0);

      // go back
      as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_end), range);
      int size_3 = as_route.size();
      if (verbose)
        printf("# route airspace size %d\n", size_3);

      ok(size_3 >= size_2, "grow as", 0);

      // and again
      as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range);
      int size_4 = as_route.size();
      if (verbose)
        printf("# route airspace size %d\n", size_4);

      ok(size_4 >= size_3, "grow as", 0);

      scan_airspaces(state, as_route, perf, true, loc_end);
    }

    // try the solver
    SpeedVector wind(Angle::Degrees(fixed(0)), fixed(0.0));
    GlidePolar polar(fixed_one);

    GlideSettings settings;
    settings.SetDefaults();
    AirspaceRoute route(airspaces);
    route.UpdatePolar(settings, polar, polar, wind);
    route.SetTerrain(&map);
    RoutePlannerConfig config;
    config.mode = RoutePlannerConfig::Mode::BOTH;

    bool sol = false;
    for (int i = 0; i < NUM_SOL; i++) {
      loc_end.latitude += Angle::Degrees(fixed(0.1));
      loc_end.altitude = map.GetHeight(loc_end) + 100;
      route.Synchronise(airspaces, loc_start, loc_end);
      if (route.Solve(loc_start, loc_end, config)) {
        sol = true;
        if (verbose) {
          PrintHelper::print_route(route);
        }
      } else {
        if (verbose) {
          printf("# fail\n");
        }
        sol = false;
      }
      char buffer[80];
      sprintf(buffer, "route %d solution", i);
      ok(sol, buffer, 0);
    }
  }

  return true;
}
Exemple #14
0
 gcc_constexpr_method
 RoughAltitude operator+(const RoughAltitude other) const {
   return RoughAltitude(value + other.value);
 }
Exemple #15
0
 /**
  * Create a representation of the largest possible value.
  */
 gcc_constexpr_function
 static RoughAltitude Max() {
   return RoughAltitude((short)SHRT_MAX);
 }
 /**
  * Create a representation of the largest possible value.
  */
 constexpr
 static RoughAltitude Max() {
   return RoughAltitude((short)SHRT_MAX);
 }
 constexpr
 RoughAltitude operator-(const RoughAltitude other) const {
   return RoughAltitude(value - other.value);
 }
Exemple #18
0
 RoughAltitude GetSafetyHeight() const {
   return RoughAltitude(config.safety_height_terrain);
 }
Exemple #19
0
static void
test_troute(const RasterMap& map, fixed mwind, fixed mc, RoughAltitude ceiling)
{
  GlideSettings settings;
  settings.SetDefaults();
  GlidePolar polar(mc);
  SpeedVector wind(Angle::Degrees(0), mwind);
  TerrainRoute route;
  route.UpdatePolar(settings, polar, polar, wind);
  route.SetTerrain(&map);

  GeoPoint origin(map.GetMapCenter());

  fixed pd = map.PixelDistance(origin, 1);
  printf("# pixel size %g\n", (double)pd);

  bool retval= true;

  {
    Directory::Create(_T("output/results"));
    std::ofstream fout ("output/results/terrain.txt");
    unsigned nx = 100;
    unsigned ny = 100;
    for (unsigned i=0; i< nx; ++i) {
      for (unsigned j=0; j< ny; ++j) {
        fixed fx = (fixed)i / (nx - 1) * 2 - fixed(1);
        fixed fy = (fixed)j / (ny - 1) * 2 - fixed(1);
        GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.6) * fx),
                   origin.latitude + Angle::Degrees(fixed(0.4) * fy));
        short h = map.GetInterpolatedHeight(x);
        fout << x.longitude.Degrees() << " " << x.latitude.Degrees() << " " << h << "\n";
      }
      fout << "\n";
    }
    fout << "\n";
  }

  RoutePlannerConfig config;
  config.mode = RoutePlannerConfig::Mode::BOTH;

  unsigned i=0;
  for (fixed ang = fixed(0); ang < fixed_two_pi; ang += fixed(M_PI / 8)) {
    GeoPoint dest = GeoVector(fixed(40000.0), Angle::Radians(ang)).EndPoint(origin);

    short hdest = map.GetHeight(dest)+100;

    retval = route.Solve(AGeoPoint(origin,
                                   RoughAltitude(map.GetHeight(origin) + 100)),
                         AGeoPoint(dest,
                                   RoughAltitude(positive(mc)
                                                 ? hdest
                                                 : std::max(hdest, (short)3200))),
                         config, ceiling);
    char buffer[80];
    sprintf(buffer,"terrain route solve, dir=%g, wind=%g, mc=%g ceiling=%d",
            (double)ang, (double)mwind, (double)mc, (int)ceiling);
    ok(retval, buffer, 0);
    PrintHelper::print_route(route);
    i++;
  }

  // polar.SetMC(fixed(0));
  // route.UpdatePolar(polar, wind);
}
bool
AirspacePredicateHeightRange::check_height(const AbstractAirspace& t) const
{
  return RoughAltitude(t.GetTop().altitude) >= h_min &&
    RoughAltitude(t.GetBase().altitude) <= h_max;
}
Exemple #21
0
void
FlarmComputer::Process(FlarmData &flarm, const FlarmData &last_flarm,
                       const NMEAInfo &basic)
{
  // Cleanup old calculation instances
  if (basic.time_available)
    flarm_calculations.CleanUp(basic.time);

  // if (FLARM data is available)
  if (!flarm.IsDetected())
    return;

  double north_to_latitude(0);
  double east_to_longitude(0);

  if (basic.location_available) {
    // Precalculate relative east and north projection to lat/lon
    // for Location calculations of each target
    constexpr Angle delta_lat = Angle::Degrees(0.01);
    constexpr Angle delta_lon = Angle::Degrees(0.01);

    GeoPoint plat = basic.location;
    plat.latitude += delta_lat;
    GeoPoint plon = basic.location;
    plon.longitude += delta_lon;

    double dlat = basic.location.DistanceS(plat);
    double dlon = basic.location.DistanceS(plon);

    if (fabs(dlat) > 0 && fabs(dlon) > 0) {
      north_to_latitude = delta_lat.Degrees() / dlat;
      east_to_longitude = delta_lon.Degrees() / dlon;
    }
  }

  // for each item in traffic
  for (auto &traffic : flarm.traffic.list) {
    // if we don't know the target's name yet
    if (!traffic.HasName()) {
      // lookup the name of this target's id
      const TCHAR *fname = FlarmDetails::LookupCallsign(traffic.id);
      if (fname != NULL)
        traffic.name = fname;
    }

    // Calculate distance
    traffic.distance = hypot(traffic.relative_north, traffic.relative_east);

    // Calculate Location
    traffic.location_available = basic.location_available;
    if (traffic.location_available) {
      traffic.location.latitude =
          Angle::Degrees(traffic.relative_north * north_to_latitude) +
          basic.location.latitude;

      traffic.location.longitude =
          Angle::Degrees(traffic.relative_east * east_to_longitude) +
          basic.location.longitude;
    }

    // Calculate absolute altitude
    traffic.altitude_available = basic.gps_altitude_available;
    if (traffic.altitude_available)
      traffic.altitude = traffic.relative_altitude + RoughAltitude(basic.gps_altitude);

    // Calculate average climb rate
    traffic.climb_rate_avg30s_available = traffic.altitude_available;
    if (traffic.climb_rate_avg30s_available)
      traffic.climb_rate_avg30s =
        flarm_calculations.Average30s(traffic.id, basic.time, traffic.altitude);

    // The following calculations are only relevant for targets
    // where information is missing
    if (traffic.track_received && traffic.turn_rate_received &&
        traffic.speed_received && traffic.climb_rate_received)
      continue;

    // Check if the target has been seen before in the last seconds
    const FlarmTraffic *last_traffic =
      last_flarm.traffic.FindTraffic(traffic.id);
    if (last_traffic == NULL || !last_traffic->valid)
      continue;

    // Calculate the time difference between now and the last contact
    double dt = traffic.valid.GetTimeDifference(last_traffic->valid);
    if (dt > 0) {
      // Calculate the immediate climb rate
      if (!traffic.climb_rate_received)
        traffic.climb_rate =
          (traffic.relative_altitude - last_traffic->relative_altitude) / dt;
    } else {
      // Since the time difference is zero (or negative)
      // we can just copy the old values
      if (!traffic.climb_rate_received)
        traffic.climb_rate = last_traffic->climb_rate;
    }

    if (dt > 0 &&
        traffic.location_available &&
        last_traffic->location_available) {
      // Calculate the GeoVector between now and the last contact
      GeoVector vec = last_traffic->location.DistanceBearing(traffic.location);

      if (!traffic.track_received)
        traffic.track = vec.bearing;

      // Calculate the turn rate
      if (!traffic.turn_rate_received) {
        Angle turn_rate = traffic.track - last_traffic->track;
        traffic.turn_rate =
          turn_rate.AsDelta().Degrees() / dt;
      }

      // Calculate the speed [m/s]
      if (!traffic.speed_received)
        traffic.speed = vec.distance / dt;
    } else {
      // Since the time difference is zero (or negative)
      // we can just copy the old values
      if (!traffic.track_received)
        traffic.track = last_traffic->track;

      if (!traffic.turn_rate_received)
        traffic.turn_rate = last_traffic->turn_rate;

      if (!traffic.speed_received)
        traffic.speed = last_traffic->speed;
    }
  }
}
Exemple #22
0
RoughAltitude
RoutePolars::CalcVHeight(const RouteLink &link) const
{
  return RoughAltitude(polar_glide.GetPoint(link.polar_index).gradient * link.d);
}
Exemple #23
0
TerrainRoute::TerrainRoute():
  m_inx_terrain(0, 0, RoughAltitude(0))
{

}