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);
}
