void
CaiLNavDevice::OnCalculatedUpdate(const MoreData &basic,
const DerivedInfo &calculated)
{
NullOperationEnvironment env;
char buffer[100];
const GeoPoint here = basic.location_available
? basic.location
: GeoPoint::Invalid();
const ElementStat ¤t_leg = calculated.task_stats.current_leg;
AGeoPoint destination =
AGeoPoint(current_leg.location_remaining,
current_leg.solution_planned.min_arrival_altitude);
if (FormatGPRMC(buffer, sizeof(buffer), basic))
PortWriteNMEA(port, buffer, env);
if (FormatGPRMB(buffer, sizeof(buffer), here, destination))
PortWriteNMEA(port, buffer, env);
if (FormatPCAIB(buffer, sizeof(buffer), destination))
PortWriteNMEANoChecksum(port, buffer, env);
}
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 auto outside_and_active =
MakeAndPredicate(ActiveAirspacePredicate(&airspace_warnings),
OutsideAirspacePredicate(AGeoPoint(basic.location, 0)));
//if altitude is available, filter airspaces in same height as airplane
if (basic.NavAltitudeAvailable()) {
/* check altitude; hard-coded margin of 50m (for now) */
const auto outside_and_active_and_height =
MakeAndPredicate(outside_and_active,
AirspacePredicateHeightRange(basic.nav_altitude - 50,
basic.nav_altitude + 50));
const auto predicate = WrapAirspacePredicate(outside_and_active_and_height);
return ::FindHorizontal(basic.location, airspace_database, predicate);
} else {
/* only filter outside and active */
const auto predicate = WrapAirspacePredicate(outside_and_active);
return ::FindHorizontal(basic.location, airspace_database, predicate);
}
}
inline void
SkyLinesTracking::Client::OnThermalReceived(const ThermalResponsePacket &packet,
size_t length)
{
if (length < sizeof(packet))
return;
const unsigned n = packet.thermal_count;
ConstBuffer<Thermal> thermals((const Thermal *)(&packet + 1), n);
if (length != sizeof(packet) + thermals.size * sizeof(thermals.front()))
return;
for (const auto &thermal : thermals)
handler->OnThermal(FromBE32(thermal.time),
AGeoPoint(ImportGeoPoint(thermal.bottom_location),
FromBE16(thermal.bottom_altitude)),
AGeoPoint(ImportGeoPoint(thermal.top_location),
FromBE16(thermal.top_altitude)),
FromBE16(thermal.lift) / 256.);
}
CloudThermal
CloudThermal::Load(Deserialiser &s)
{
s.Read8();
const unsigned client_key = s.Read64();
std::chrono::steady_clock::time_point time;
s >> time;
SkyLinesTracking::Thermal t;
s.ReadT(t);
CloudThermal thermal(client_key,
AGeoPoint(SkyLinesTracking::ImportGeoPoint(t.bottom_location),
FromBE16(t.bottom_altitude)),
AGeoPoint(SkyLinesTracking::ImportGeoPoint(t.top_location),
FromBE16(t.top_altitude)),
FromBE16(t.lift) / 256.);
thermal.time = time;
return thermal;
}
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));
}
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
AbortTask::FillReachable(const AircraftState &state,
AlternateVector &approx_waypoints,
const GlidePolar &polar, bool only_airfield,
bool final_glide, bool safety)
{
if (IsTaskFull() || approx_waypoints.empty())
return false;
const AGeoPoint p_start(state.location, state.altitude);
bool found_final_glide = false;
reservable_priority_queue<Alternate, AlternateVector, AbortRank> q;
q.reserve(32);
for (auto v = approx_waypoints.begin(); v != approx_waypoints.end();) {
if (only_airfield && !v->waypoint.IsAirport()) {
++v;
continue;
}
UnorderedTaskPoint t(v->waypoint, task_behaviour);
GlideResult result =
TaskSolution::GlideSolutionRemaining(t, state,
task_behaviour.glide, polar);
if (IsReachable(result, final_glide)) {
bool intersects = false;
const bool is_reachable_final = IsReachable(result, true);
if (intersection_test && final_glide && is_reachable_final)
intersects = intersection_test->Intersects(
AGeoPoint(v->waypoint.location, result.min_arrival_altitude));
if (!intersects) {
q.push(Alternate(v->waypoint, result));
// remove it since it's already in the list now
approx_waypoints.erase(v);
if (is_reachable_final)
found_final_glide = true;
continue; // skip incrementing v since we just erased it
}
}
++v;
}
while (!q.empty() && !IsTaskFull()) {
const Alternate top = q.top();
task_points.push_back(AlternateTaskPoint(top.waypoint, task_behaviour,
top.solution));
const int i = task_points.size() - 1;
if (task_points[i].GetWaypoint().id == active_waypoint)
active_task_point = i;
q.pop();
}
return found_final_glide;
}
static void
test_troute(const RasterMap &map, double mwind, double mc, int ceiling)
{
GlideSettings settings;
settings.SetDefaults();
RoutePlannerConfig config;
config.mode = RoutePlannerConfig::Mode::BOTH;
GlidePolar polar(mc);
SpeedVector wind(Angle::Degrees(0), mwind);
TerrainRoute route;
route.UpdatePolar(settings, config, polar, polar, wind);
route.SetTerrain(&map);
GeoPoint origin(map.GetMapCenter());
auto pd = map.PixelDistance(origin, 1);
printf("# pixel size %g\n", (double)pd);
bool retval= true;
{
Directory::Create(Path(_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) {
auto fx = (double)i / (nx - 1) * 2 - 1;
auto fy = (double)j / (ny - 1) * 2 - 1;
GeoPoint x(origin.longitude + Angle::Degrees(0.6 * fx),
origin.latitude + Angle::Degrees(0.4 * fy));
TerrainHeight h = map.GetInterpolatedHeight(x);
fout << x.longitude.Degrees() << " " << x.latitude.Degrees()
<< " " << h.GetValue() << "\n";
}
fout << "\n";
}
fout << "\n";
}
unsigned i=0;
for (double ang = 0; ang < M_2PI; ang += M_PI / 8) {
GeoPoint dest = GeoVector(40000.0, Angle::Radians(ang)).EndPoint(origin);
int hdest = map.GetHeight(dest).GetValueOr0() + 100;
retval = route.Solve(AGeoPoint(origin,
map.GetHeight(origin).GetValueOr0() + 100),
AGeoPoint(dest,
mc > 0
? hdest
: std::max(hdest, 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(0);
// route.UpdatePolar(polar, wind);
}
bool
RoutePlanner::solve(const AGeoPoint& origin,
const AGeoPoint& destination,
const RoutePlannerConfig& config,
const short h_ceiling)
{
on_solve(origin, destination);
rpolars_route.set_config(config, std::max(destination.altitude, origin.altitude),
h_ceiling);
rpolars_reach.set_config(config, std::max(destination.altitude, origin.altitude),
h_ceiling);
m_reach_polar_mode = config.reach_polar_mode;
{
const AFlatGeoPoint s_origin(task_projection.project(origin), origin.altitude);
const AFlatGeoPoint s_destination(task_projection.project(destination), destination.altitude);
if (!(s_origin == origin_last) || !(s_destination == destination_last))
dirty = true;
if (is_trivial())
return false;
dirty = false;
origin_last = s_origin;
destination_last = s_destination;
h_min = std::min(s_origin.altitude, s_destination.altitude);
h_max = rpolars_route.cruise_altitude;
}
solution_route.clear();
solution_route.push_back(origin);
solution_route.push_back(destination);
if (!rpolars_route.terrain_enabled() && !rpolars_route.airspace_enabled())
return false; // trivial
m_search_hull.clear();
m_search_hull.push_back(SearchPoint(origin_last, task_projection));
RoutePoint start = origin_last;
m_astar_goal = destination_last;
RouteLink e_test(start, m_astar_goal, task_projection);
if (e_test.is_short())
return false;
if (!rpolars_route.achievable(e_test))
return false;
count_dij=0;
count_airspace=0;
count_terrain=0;
count_supressed=0;
bool retval = false;
m_planner.restart(start);
unsigned best_d = UINT_MAX;
if (verbose) {
printf("# goal node (%d,%d,%d)\n",
m_astar_goal.Longitude, m_astar_goal.Latitude, m_astar_goal.altitude);
printf("# start node (%d,%d,%d)\n",
start.Longitude, start.Latitude, start.altitude);
}
while (!m_planner.empty()) {
const RoutePoint node = m_planner.pop();
if (verbose>1) {
printf("# processing node (%d,%d,%d) %d,%d q size %d\n",
node.Longitude, node.Latitude, node.altitude,
m_planner.get_node_value(node).g,
m_planner.get_node_value(node).h,
m_planner.queue_size());
}
h_min = std::min(h_min, node.altitude);
h_max = std::max(h_max, node.altitude);
bool is_final = (node == m_astar_goal);
if (is_final) {
if (!retval)
best_d = UINT_MAX;
retval = true;
}
if (is_final) // @todo: allow fallback if failed
{ // copy improving solutions
Route this_solution;
unsigned d = find_solution(node, this_solution);
if (d< best_d) {
best_d = d;
solution_route = this_solution;
}
}
if (retval)
break; // want top solution only
// shoot for final
RouteLink e(node, m_astar_goal, task_projection);
if (set_unique(e))
add_edges(e);
while (!m_links.empty()) {
add_edges(m_links.front());
m_links.pop();
}
}
count_unique = m_unique.size();
if (retval && verbose) {
printf("# solved with %d intermediate points\n", (int)(solution_route.size()-2));
}
if (retval) {
// correct solution for rounding
assert(solution_route.size()>=2);
for (unsigned i=0; i< solution_route.size(); ++i) {
FlatGeoPoint p(task_projection.project(solution_route[i]));
if (p== origin_last) {
solution_route[i] = AGeoPoint(origin, solution_route[i].altitude);
} else if (p== destination_last) {
solution_route[i] = AGeoPoint(destination, solution_route[i].altitude);
}
}
} else {
solution_route.clear();
solution_route.push_back(origin);
solution_route.push_back(destination);
}
m_planner.clear();
m_unique.clear();
// m_search_hull.clear();
return retval;
}
// m_search_hull.clear();
return retval;
}
unsigned
RoutePlanner::find_solution(const RoutePoint &final, Route& this_route) const
{
// we are iterating from goal (aircraft) backwards to start (target)
RoutePoint p(final);
RoutePoint p_last(p);
bool finished = false;
this_route.insert(this_route.begin(),
AGeoPoint(task_projection.unproject(p), p.altitude));
do {
p_last = p;
p = m_planner.get_predecessor(p);
if (p== p_last) {
finished = true;
continue;
}
if ((p.altitude< p_last.altitude) && !((FlatGeoPoint)p == (FlatGeoPoint)p_last)) {
// create intermediate point for part cruise, part glide
const RouteLink l(p, p_last, task_projection);
const short vh = rpolars_route.calc_vheight(l);
