inline double OrderedTask::ScanDistanceMin(const GeoPoint &location, bool full) { if (!full && location.IsValid() && last_min_location.IsValid() && DistanceIsSignificant(location, last_min_location)) { const TaskWaypoint *active = GetActiveTaskPoint(); if (active != nullptr) { const GeoPoint &target = active->GetWaypoint().location; const unsigned last_distance = (unsigned)last_min_location.Distance(target); const unsigned cur_distance = (unsigned)location.Distance(target); /* do the full scan only if the distance to the active task point has changed by more than 5%, otherwise we don't expect any relevant changes */ if (last_distance < 2000 || cur_distance < 2000 || last_distance * 20 >= cur_distance * 21 || cur_distance * 20 >= last_distance * 21) full = true; } } if (full) { RunDijsktraMin(location); last_min_location = location; } return task_points.front()->ScanDistanceMin(); }
void TaskManager::UpdateCommonStatsWaypoints(const AircraftState &state) { common_stats.vector_home = task_abort.GetHomeVector(state); common_stats.landable_reachable = task_abort.HasReachableLandable(); const TaskWaypoint *tp = GetActiveTaskPoint(); if (tp != NULL) { // must make an UnorderedTaskPoint here so we pick up arrival height requirements const UnorderedTaskPoint fp(tp->GetWaypoint(), task_behaviour); // @todo: consider change to task_abort.get_safety_polar(); GlidePolar glide_polar = GetGlidePolar(); common_stats.next_solution = TaskSolution::GlideSolutionRemaining(fp, state, task_behaviour.glide, glide_polar); if (positive(glide_polar.GetMC())) { glide_polar.SetMC(fixed_zero); common_stats.next_solution_mc0 = TaskSolution::GlideSolutionRemaining(fp, state, task_behaviour.glide, glide_polar); } else common_stats.next_solution_mc0 = common_stats.next_solution; } else { common_stats.next_solution.Reset(); common_stats.next_solution_mc0.Reset(); } }
void AbstractTask::UpdateStatsDistances(const GeoPoint &location, const bool full_update) { stats.total.remaining.set_distance(ScanDistanceRemaining(location)); const TaskPoint *active = GetActiveTaskPoint(); if (active != NULL) { stats.current_leg.location_remaining = active->GetLocationRemaining(); stats.current_leg.vector_remaining = active->GetVectorRemaining(location); } else { stats.current_leg.location_remaining = GeoPoint::Invalid(); stats.current_leg.vector_remaining = GeoVector::Invalid(); } if (full_update) stats.distance_nominal = ScanDistanceNominal(); ScanDistanceMinMax(location, full_update, &stats.distance_min, &stats.distance_max); stats.total.travelled.set_distance(ScanDistanceTravelled(location)); stats.total.planned.set_distance(ScanDistancePlanned()); if (IsScored()) { if (!TaskStarted()) stats.distance_scored = fixed_zero; else if (!TaskFinished()) stats.distance_scored = ScanDistanceScored(location); } else stats.distance_scored = fixed_zero; }
fixed UnorderedTask::ScanDistanceRemaining(const GeoPoint &location) { TaskPoint *tp = GetActiveTaskPoint(); if (tp == nullptr || !location.IsValid()) return fixed(0); return tp->Distance(location); }
fixed UnorderedTask::GetFinishHeight() const { TaskPoint *tp = GetActiveTaskPoint(); if (!tp) { return fixed_zero; } return tp->GetElevation(); }
fixed UnorderedTask::ScanDistanceRemaining(const GeoPoint &location) { TaskPoint *tp = GetActiveTaskPoint(); if (!tp) { return fixed_zero; } return tp->Distance(location); }
bool TaskManager::Update(const AircraftState &state, const AircraftState &state_last) { if (!state.location.IsValid()) { /* in case of GPS failure or and during startup (before the first GPS fix), update only the stats */ UpdateCommonStats(state); return false; } /* always update ordered task so even if we are temporarily in a different mode, so the task stats are still updated. Otherwise, the task stats would freeze and sampling etc would not be performed. In actual use, even if you are in Abort/Goto you still may want to go back to the task and have it know where you went with respect to your task turnpoints etc. */ bool retval = false; if (state_last.time > state.time) Reset(); if (task_ordered.TaskSize() > 1) // always update ordered task retval |= task_ordered.Update(state, state_last, glide_polar); // inform the abort task whether it is running as the task or not task_abort.SetActive(active_task == &task_abort); // and tell it where the task destination is (if any) const GeoPoint *destination = &state.location; if (task_behaviour.abort_task_mode == AbortTaskMode::HOME) { const Waypoint *home = task_abort.GetHome(); if (home) destination = &home->location; } else if (task_behaviour.abort_task_mode == AbortTaskMode::TASK) { const TaskWaypoint *twp = GetActiveTaskPoint(); if (twp) destination = &(twp->GetLocationRemaining()); } task_abort.SetTaskDestination(*destination); retval |= task_abort.Update(state, state_last, GetReachPolar()); if (active_task && active_task != &task_ordered && active_task != &task_abort) // update mode task for any that have not yet run retval |= active_task->Update(state, state_last, glide_polar); UpdateCommonStats(state); return retval; }
fixed UnorderedTask::CalcRequiredGlide(const AircraftState &aircraft) const { TaskPoint *tp = GetActiveTaskPoint(); if (!tp) { return fixed_zero; } TaskGlideRequired bgr(tp, aircraft, glide_polar); return bgr.search(fixed_zero); }
fixed UnorderedTask::GetTaskRadius(const GeoPoint& fallback_location) const { TaskPoint *tp = GetActiveTaskPoint(); if (!tp) { return fixed_zero; } else { return half(tp->GetLocation().Distance(fallback_location)); } }
GeoPoint UnorderedTask::GetTaskCenter(const GeoPoint& fallback_location) const { TaskPoint *tp = GetActiveTaskPoint(); if (!tp) { return fallback_location; } else { return tp->GetLocation().Interpolate(fallback_location, fixed_half); } }
bool UnorderedTask::CalcBestMC(const AircraftState &aircraft, fixed& best) const { TaskPoint *tp = GetActiveTaskPoint(); if (!tp) { best = glide_polar.GetMC(); return false; } TaskBestMc bmc(tp, aircraft, glide_polar); return bmc.search(glide_polar.GetMC(), best); }
fixed UnorderedTask::CalcRequiredGlide(const AircraftState &aircraft, const GlidePolar &glide_polar) const { TaskPoint *tp = GetActiveTaskPoint(); if (tp == nullptr || !aircraft.location.IsValid()) return fixed(0); TaskGlideRequired bgr(tp, aircraft, task_behaviour.glide, glide_polar); return bgr.search(fixed(0)); }
bool UnorderedTask::CalcBestMC(const AircraftState &aircraft, const GlidePolar &glide_polar, fixed& best) const { TaskPoint *tp = GetActiveTaskPoint(); if (tp == nullptr || !aircraft.location.IsValid()) { best = glide_polar.GetMC(); return false; } TaskBestMc bmc(tp, aircraft, task_behaviour.glide, glide_polar); return bmc.search(glide_polar.GetMC(), best); }
fixed AbstractTask::CalcLegGradient(const AircraftState &aircraft) const { // Get next turnpoint const TaskWaypoint *tp = GetActiveTaskPoint(); if (!tp) return fixed_zero; // Get the distance to the next turnpoint const fixed d = tp->GetVectorRemaining(aircraft.location).distance; if (!d) return fixed_zero; // Calculate the geometric gradient (height divided by distance) return (aircraft.altitude - tp->GetElevation()) / d; }
void UnorderedTask::GlideSolutionRemaining(const AircraftState &state, const GlidePolar &polar, GlideResult &total, GlideResult &leg) { GlideResult res; TaskPoint* tp = GetActiveTaskPoint(); if (tp) { res = TaskSolution::GlideSolutionRemaining(*tp, state, polar); res.CalcDeferred(); } else res.Reset(); total = res; leg = res; }
void UnorderedTask::GlideSolutionRemaining(const AircraftState &state, const GlidePolar &polar, GlideResult &total, GlideResult &leg) { GlideResult res; TaskPoint* tp = GetActiveTaskPoint(); if (tp != nullptr && state.location.IsValid()) { res = TaskSolution::GlideSolutionRemaining(*tp, state, task_behaviour.glide, polar); res.CalcDeferred(); } else res.Reset(); total = res; leg = res; }
bool UnorderedTask::CheckTask() const { return (GetActiveTaskPoint()!=NULL); }