static void
test_glide_cb(const fixed h, const fixed W, const fixed Wangle,
std::ostream &hfile)
{
GlideSettings settings;
settings.SetDefaults();
GlidePolar polar(fixed(1));
AircraftState ac;
ac.wind.norm = fabs(W);
if (negative(W)) {
ac.wind.bearing = Angle::Degrees(fixed(180)+Wangle);
} else {
ac.wind.bearing = Angle::Degrees(Wangle);
}
ac.altitude = h;
GeoVector vect(fixed(400.0), Angle::Zero());
GlideState gs (vect, fixed(0), ac.altitude, ac.wind);
GlideResult gr = MacCready::Solve(settings, polar, gs);
gr.CalcDeferred();
hfile << (double)W << " "
<< (double)Wangle << " "
<< (double)gr.vector.bearing.Degrees() << " "
<< (double)gr.cruise_track_bearing.Degrees() << " "
<< "\n";
}
void
OrderedTask::GlideSolutionPlanned(const AircraftState &aircraft,
const GlidePolar &glide_polar,
GlideResult &total,
GlideResult &leg,
DistanceStat &total_remaining_effective,
DistanceStat &leg_remaining_effective,
const GlideResult &solution_remaining_total,
const GlideResult &solution_remaining_leg)
{
TaskMacCreadyTotal tm(task_points, active_task_point,
task_behaviour.glide, glide_polar);
total = tm.glide_solution(aircraft);
leg = tm.get_active_solution();
if (solution_remaining_total.IsOk())
total_remaining_effective.SetDistance(tm.effective_distance(solution_remaining_total.time_elapsed));
else
total_remaining_effective.Reset();
if (solution_remaining_leg.IsOk())
leg_remaining_effective.SetDistance(tm.effective_leg_distance(solution_remaining_leg.time_elapsed));
else
leg_remaining_effective.Reset();
}
static void
test_glide_cb(const fixed h, const fixed W, const fixed Wangle,
std::ostream &hfile)
{
GlidePolar polar(fixed_one);
AircraftState ac;
ac.wind.norm = fabs(W);
if (negative(W)) {
ac.wind.bearing = Angle::degrees(fixed(180)+Wangle);
} else {
ac.wind.bearing = Angle::degrees(Wangle);
}
ac.altitude = h;
GeoVector vect(fixed(400.0));
GlideState gs (vect, fixed_zero, ac.altitude, ac.wind);
GlideResult gr = MacCready::solve(polar, gs);
gr.CalcDeferred(ac);
hfile << (double)W << " "
<< (double)Wangle << " "
<< (double)gr.vector.Bearing.value_degrees() << " "
<< (double)gr.cruise_track_bearing.value_degrees() << " "
<< "\n";
}
gcc_pure
static bool
IsReachable(const GlideResult &result, bool final_glide)
{
return final_glide
? result.IsFinalGlide()
: result.IsAchievable();
}
static void
CheckLegEqualsTotal(const GlideResult &leg, const GlideResult &total)
{
ok1(total.IsOk());
ok1(equals(total.height_climb, leg.height_climb));
ok1(equals(total.height_glide, leg.height_glide));
ok1(equals(total.altitude_difference, leg.altitude_difference));
ok1(equals(total.GetRequiredAltitudeWithDrift(), leg.GetRequiredAltitudeWithDrift()));
}
static void
SetValueFromAltDiff(InfoBoxData &data, const TaskStats &task_stats,
const GlideResult &solution)
{
if (!task_stats.task_valid || !solution.IsAchievable()) {
data.SetInvalid();
return;
}
const ComputerSettings &settings = CommonInterface::GetComputerSettings();
fixed altitude_difference =
solution.SelectAltitudeDifference(settings.task.glide);
data.SetValueFromArrival(altitude_difference);
}
void
RoutePolar::Initialise(const GlidePolar& polar, const SpeedVector& wind,
const bool is_glide)
{
for (unsigned i = 0; i < ROUTEPOLAR_POINTS; ++i) {
const Angle ang = IndexToAngle(i);
GlideResult res = SolveTask(polar, wind, ang, is_glide);
if (res.IsOk()) {
RoutePolarPoint point(res.time_elapsed, res.height_glide);
points[i] = point;
} else
points[i].valid = false;
}
}
void
InfoBoxContentNextAltitudeArrival::Update(InfoBoxData &data)
{
// pilots want this to be assuming terminal flight to this wp
const MoreData &basic = CommonInterface::Basic();
const TaskStats &task_stats = XCSoarInterface::Calculated().task_stats;
const GlideResult next_solution = XCSoarInterface::Calculated().common_stats.next_solution;
if (!task_stats.task_valid || !next_solution.IsAchievable()) {
data.SetInvalid();
return;
}
data.SetValueFromAltitude(next_solution.GetArrivalAltitude(basic.nav_altitude));
}
bool
AbortTask::is_reachable(const GlideResult &result,
const bool final_glide) const
{
return !positive(result.Vector.Distance) ||
(!negative(result.TimeElapsed) && result.glide_reachable(final_glide));
}
double
GlidePolar::SpeedToFly(const AircraftState &state,
const GlideResult &solution, const bool block_stf) const
{
assert(IsValid());
double V_stf;
const auto g_scaling = block_stf
? 1.
: sqrt(fabs(state.g_load));
if (!block_stf && (state.netto_vario > mc + Smin)) {
// stop to climb
V_stf = Vmin;
} else {
const auto head_wind = !positive(GetMC()) && solution.IsDefined()
? solution.head_wind
: 0.;
const auto stf_sink_rate = block_stf
? 0.
: -state.netto_vario;
GlidePolarSpeedToFly gp_stf(*this, stf_sink_rate, head_wind, Vmin, Vmax);
V_stf = gp_stf.solve(Vmax);
}
return std::max(Vmin, V_stf * g_scaling);
}
void
UpdateInfoBoxNextAltitudeArrival(InfoBoxData &data)
{
// pilots want this to be assuming terminal flight to this wp
const MoreData &basic = CommonInterface::Basic();
const TaskStats &task_stats = CommonInterface::Calculated().task_stats;
const GlideResult next_solution = task_stats.current_leg.solution_remaining;
if (!basic.NavAltitudeAvailable() ||
!task_stats.task_valid || !next_solution.IsAchievable()) {
data.SetInvalid();
return;
}
data.SetValueFromAltitude(next_solution.GetArrivalAltitude(basic.nav_altitude));
}
void
OrderedTask::GlideSolutionTravelled(const AircraftState &aircraft,
const GlidePolar &glide_polar,
GlideResult &total,
GlideResult &leg)
{
if (!aircraft.location.IsValid() || task_points.empty()) {
total.Reset();
leg.Reset();
return;
}
TaskMacCreadyTravelled tm(task_points.cbegin(), active_task_point,
task_behaviour.glide, glide_polar);
total = tm.glide_solution(aircraft);
leg = tm.get_active_solution();
}
static void
Copy(DistanceStat &stat, const GlideResult &solution)
{
if (solution.IsDefined())
stat.set_distance(solution.vector.distance);
else
stat.Reset();
}
void
RoutePolar::Initialise(const GlideSettings &settings, const GlidePolar& polar,
const SpeedVector& wind,
const bool is_glide)
{
static constexpr Angle ang_step = Angle::FullCircle() / ROUTEPOLAR_POINTS;
Angle ang = Angle::QuarterCircle();
for (unsigned i = 0; i < ROUTEPOLAR_POINTS; ++i, ang -= ang_step) {
GlideResult res = SolveTask(settings, polar, wind, ang, is_glide);
if (res.IsOk()) {
RoutePolarPoint point(res.time_elapsed, res.height_glide);
points[i] = point;
} else
points[i].valid = false;
}
}
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
OrderedTask::GlideSolutionRemaining(const AircraftState &aircraft,
const GlidePolar &polar,
GlideResult &total,
GlideResult &leg)
{
if (!aircraft.location.IsValid()) {
total.Reset();
leg.Reset();
return;
}
TaskMacCreadyRemaining tm(task_points.cbegin(), task_points.cend(),
active_task_point,
task_behaviour.glide, polar);
total = tm.glide_solution(aircraft);
leg = tm.get_active_solution();
}
/**
* Function to optimise in search
*
* \note the f(x) is magnified because with fixed, find_min can
* fail with too small df/dx
*
* @param V cruise true air speed (m/s)
* @return Inverse LD
*/
double f(const double v) {
res = mac.SolveGlide(task, v, allow_partial);
if (!res.IsOk() || res.vector.distance <= 0)
/* the solver failed: return a large value that will be
discarded by ZeroFinder */
return 1000000;
return res.height_glide * 1024 / res.vector.distance;
}
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;
}
void
CrossSectionRenderer::PaintGlide(ChartRenderer &chart) const
{
if (!gps_info.NavAltitudeAvailable() || !glide_polar.IsValid())
return;
const fixed altitude = gps_info.nav_altitude;
const MacCready mc(glide_settings, glide_polar);
const GlideState task(vec, fixed(0), altitude,
calculated_info.GetWindOrZero());
const GlideResult result = mc.SolveStraight(task);
if (!result.IsOk())
return;
chart.DrawLine(fixed(0), altitude, result.vector.distance,
result.GetArrivalAltitude(),
ChartLook::STYLE_BLUETHIN);
}
GlideResult
MacCready::SolveStraight(const GlideState &task) const
{
if (!glide_polar.IsValid()) {
/* can't solve without a valid GlidePolar() */
GlideResult result;
result.Reset();
return result;
}
if (task.vector.distance <= 0)
return SolveVertical(task);
if (glide_polar.GetMC() <= 0)
// whole task must be glide
return OptimiseGlide(task);
return SolveGlide(task, glide_polar.GetVBestLD());
}
fixed
TaskSolveTravelled::time_error()
{
GlideResult res = tm.glide_solution(aircraft);
if (!res.IsOk())
/* what can we do if there's no solution? This is an attempt to
make ZeroFinder ignore this call, by returning a large value.
I'm not sure if this kludge is correct. */
return fixed(999999);
#ifdef SOLVE_ZERO
fixed d = res.time_elapsed-dt;
#else
fixed d = fabs(res.time_elapsed-dt);
#endif
d += res.time_virtual;
return d*inv_dt;
}
void CalculateReachabilityDirect(const MoreData &basic,
const SpeedVector &wind,
const MacCready &mac_cready,
const TaskBehaviour &task_behaviour) {
assert(basic.location_available);
assert(basic.NavAltitudeAvailable());
const auto elevation = waypoint->elevation +
task_behaviour.safety_height_arrival;
const GlideState state(GeoVector(basic.location, waypoint->location),
elevation, basic.nav_altitude, wind);
const GlideResult result = mac_cready.SolveStraight(state);
if (!result.IsOk())
return;
reach.direct = result.pure_glide_altitude_difference;
if (result.pure_glide_altitude_difference > 0)
reachable = WaypointRenderer::ReachableTerrain;
}
void
GlideResult::Add(const GlideResult &s2)
{
if ((unsigned)s2.validity > (unsigned)validity)
/* downgrade the validity */
validity = s2.validity;
if (!IsDefined())
return;
vector.distance += s2.vector.distance;
if (!IsOk())
/* the other attributes are not valid if validity is not OK or
PARTIAL */
return;
if (s2.GetRequiredAltitudeWithDrift() < min_arrival_altitude) {
/* must meet the safety height of the first leg */
assert(s2.min_arrival_altitude < s2.GetArrivalAltitudeWithDrift(min_arrival_altitude));
/* calculate a new minimum arrival height that considers the
"mountain top" in the middle */
min_arrival_altitude = s2.GetArrivalAltitudeWithDrift(min_arrival_altitude);
} else {
/* must meet the safety height of the second leg */
/* apply the increased altitude requirement */
altitude_difference -=
s2.GetRequiredAltitudeWithDrift() - min_arrival_altitude;
/* adopt the minimum height of the second leg */
min_arrival_altitude = s2.min_arrival_altitude;
}
pure_glide_height += s2.pure_glide_height;
time_elapsed += s2.time_elapsed;
height_glide += s2.height_glide;
height_climb += s2.height_climb;
time_virtual += s2.time_virtual;
}
GlideResult
MacCready::Solve(const GlideState &task) const
{
if (!glide_polar.IsValid()) {
/* can't solve without a valid GlidePolar() */
GlideResult result;
result.Reset();
return result;
}
if (task.vector.distance <= 0)
return SolveVertical(task);
if (glide_polar.GetMC() <= 0)
// whole task must be glide
return OptimiseGlide(task, false);
if (task.altitude_difference < 0)
// whole task climb-cruise
return SolveCruise(task);
// task partial climb-cruise, partial glide
// calc first final glide part
GlideResult result_fg = SolveGlide(task, glide_polar.GetVBestLD(), true);
if (result_fg.validity == GlideResult::Validity::OK &&
task.vector.distance - result_fg.vector.distance <= 0)
// whole task final glided
return result_fg;
// climb-cruise remainder of way
GlideState sub_task = task;
sub_task.vector.distance -= result_fg.vector.distance;
sub_task.altitude_difference -= result_fg.height_glide;
GlideResult result_cc = SolveCruise(sub_task);
result_fg.Add(result_cc);
return result_fg;
}
void
InfoBoxContentNextAltitudeArrival::Update(InfoBoxData &data)
{
// pilots want this to be assuming terminal flight to this wp
const TaskStats &task_stats = XCSoarInterface::Calculated().task_stats;
const GlideResult next_solution = XCSoarInterface::Calculated().common_stats.next_solution;
if (!task_stats.task_valid || !next_solution.IsFinalGlide()) {
data.SetInvalid();
return;
}
// Set Value
TCHAR tmp[32];
fixed alt = next_solution.GetArrivalAltitude(XCSoarInterface::Basic().nav_altitude);
Units::FormatUserAltitude(alt, tmp, 32, false);
data.SetValue(tmp);
// Set Unit
data.SetValueUnit(Units::current.altitude_unit);
}
/**
* Specialisation of AirspaceAircraftPerformance for tasks where
* part of the path is in cruise, part in final glide. This is
* intended to be used temporarily only.
*
* This simplifies the path by assuming flight is constant altitude
* or descent to the task point elevation.
*/
AirspaceAircraftPerformance(const GlidePolar &polar,
const GlideResult &solution)
:vertical_tolerance(0.001),
cruise_speed(positive(solution.time_elapsed)
? solution.vector.distance / solution.time_elapsed
: fixed(1)),
cruise_descent(positive(solution.time_elapsed)
? (positive(solution.height_climb)
? -solution.height_climb
: solution.height_glide) / solution.time_elapsed
: fixed(0)),
descent_rate(polar.GetSBestLD()),
climb_rate(positive(solution.time_elapsed) &&
positive(solution.height_climb)
? polar.GetMC()
: fixed(0)),
max_speed(cruise_speed) {
assert(polar.IsValid());
assert(solution.IsOk());
assert(solution.IsAchievable());
}
void
InfoBoxContentNextAltitudeArrival::Update(InfoBoxWindow &infobox)
{
// pilots want this to be assuming terminal flight to this wp
const TaskStats &task_stats = XCSoarInterface::Calculated().task_stats;
const GlideResult next_solution = XCSoarInterface::Calculated().common_stats.next_solution;
if (!task_stats.task_valid || !next_solution.glide_reachable(true)) {
infobox.SetInvalid();
return;
}
// Set Value
TCHAR tmp[32];
fixed alt = XCSoarInterface::Basic().NavAltitude-next_solution.HeightGlide;
Units::FormatUserAltitude(alt, tmp, 32, false);
infobox.SetValue(tmp);
// Set Unit
infobox.SetValueUnit(Units::AltitudeUnit);
}
void
UnorderedTask::GlideSolutionPlanned(const AircraftState &state,
GlideResult &total,
GlideResult &leg,
DistanceStat &total_remaining_effective,
DistanceStat &leg_remaining_effective,
const GlideResult &solution_remaining_total,
const GlideResult &solution_remaining_leg)
{
total = solution_remaining_total;
leg = solution_remaining_leg;
if (total.IsOk())
total_remaining_effective.set_distance(total.vector.distance);
else
total_remaining_effective.Reset();
if (leg.IsOk())
leg_remaining_effective.set_distance(leg.vector.distance);
else
leg_remaining_effective.Reset();
}
fixed
GlidePolar::speed_to_fly(const AIRCRAFT_STATE &state,
const GlideResult &solution, const bool block_stf) const
{
fixed V_stf;
const fixed g_scaling (block_stf ? fixed_one : sqrt(fabs(state.Gload)));
if (!block_stf && (state.NettoVario > mc + Smin)) {
// stop to climb
V_stf = Vmin;
} else {
const fixed head_wind (solution.is_final_glide() ? solution.HeadWind : fixed_zero);
const fixed stf_sink_rate (block_stf ? fixed_zero : -state.NettoVario);
GlidePolarSpeedToFly gp_stf(*this, stf_sink_rate, head_wind, Vmin, Vmax);
V_stf = gp_stf.solve(Vmax);
}
return max(Vmin, V_stf*g_scaling);
}
fixed
GlidePolar::SpeedToFly(const AircraftState &state,
const GlideResult &solution, const bool block_stf) const
{
assert(IsValid());
fixed V_stf;
const fixed g_scaling (block_stf ? fixed(1) : sqrt(fabs(state.g_load)));
if (!block_stf && (state.netto_vario > mc + Smin)) {
// stop to climb
V_stf = Vmin;
} else {
const fixed head_wind(!positive(GetMC()) && solution.IsDefined()
? solution.head_wind
: fixed(0));
const fixed stf_sink_rate (block_stf ? fixed(0) : -state.netto_vario);
GlidePolarSpeedToFly gp_stf(*this, stf_sink_rate, head_wind, Vmin, Vmax);
V_stf = gp_stf.solve(Vmax);
}
return std::max(Vmin, V_stf * g_scaling);
}
