void
TaskAutoPilot::update_mode(const TaskAccessor& task,
const AircraftState& state)
{
switch (acstate) {
case Cruise:
/* XXX this condition is probably broken */
if (awp > 0 && !negative(task.remaining_alt_difference())) {
acstate = FinalGlide;
on_mode_change();
} else {
if (state.altitude<=target_height(task)) {
acstate = Climb;
on_mode_change();
}
}
break;
case FinalGlide:
if (task.remaining_alt_difference()<-fixed_20) {
acstate = Climb;
on_mode_change();
}
break;
case Climb:
/* XXX this condition is probably broken */
if (awp > 0 && !negative(task.remaining_alt_difference())) {
acstate = FinalGlide;
on_mode_change();
} else if (state.altitude>=fixed_1500) {
acstate = Cruise;
on_mode_change();
}
break;
};
}
bool
TaskAutoPilot::far_from_target(const TaskAccessor& task, const AIRCRAFT_STATE& state)
{
// are we considered close to the target?
if (task.is_empty())
return w[0].distance(state.Location)>state.Speed;
bool d_far = (task.leg_stats().remaining.get_distance() > fixed(100));
if (!task.is_ordered())
// cheat for non-ordered tasks
return d_far;
bool entered = task.has_entered(awp);
if (current_has_target(task))
return d_far || !entered;
fixed dc = w[0].distance(state.Location);
if (awp==0) {
return (dc>state.Speed);
}
return (dc>state.Speed) || !entered;
}
void
TaskAutoPilot::update_mode(const TaskAccessor& task,
const AIRCRAFT_STATE& state)
{
switch (acstate) {
case Cruise:
if ((awp>0) &&
(task.distance_to_final()<= state.Speed)) {
acstate = FinalGlide;
on_mode_change();
} else {
if (state.NavAltitude<=target_height(task)) {
acstate = Climb;
on_mode_change();
}
}
break;
case FinalGlide:
if (task.remaining_alt_difference()<-fixed_20) {
acstate = Climb;
on_mode_change();
}
break;
case Climb:
if ((awp>0) &&
(task.distance_to_final()<= state.Speed)) {
acstate = FinalGlide;
on_mode_change();
} else if (state.NavAltitude>=fixed_1500) {
acstate = Cruise;
on_mode_change();
}
break;
};
}
bool
TaskAutoPilot::has_finished(TaskAccessor& task)
{
if (task.is_finished())
return true;
if (task.is_ordered()) {
return awp>= task.size();
} else {
return awp>= 1;
}
}
bool
TaskAutoPilot::do_advance(TaskAccessor& task)
{
if (task.is_ordered() && (awp==0)) {
awp++;
}
awp++;
if (has_finished(task))
return false;
task.setActiveTaskPoint(awp);
get_awp(task);
return true;
}
void
TaskAutoPilot::Start(const TaskAccessor& task)
{
// construct list of places we will fly to.
// we dont do this dynamically so it is remembered even if
// the task is advanced/retreated.
if (task.is_ordered()) {
// construct list
// this pilot is inaccurate, he flies to a random point in the OZ,
// and starts in the start OZ.
w[1] = task.random_oz_point(0, parms.target_noise);
w[0] = task.random_oz_point(1, parms.target_noise);
} else {
// for non-ordered tasks, start at the default location
w[1] = location_start;
if (task.is_empty()) {
// go somewhere nearby
w[0] = location_previous;
} else {
// go directly to the target
w[0] = task.random_oz_point(0, parms.target_noise);
}
}
// pick up the locations from the task to be used to initialise
// the aircraft simulator
location_start = get_start_location(task);
location_previous = get_start_location(task, false);
awp= 0;
// reset the heading
heading = Angle::Zero();
heading_filt.Reset(fixed_zero);
acstate = Cruise;
}
GeoPoint
TaskAutoPilot::target(const TaskAccessor& task) const
{
if (current_has_target(task))
// in this mode, we go directly to the target
return task.getActiveTaskPointLocation();
else
// head towards the rough location
return w[0];
}
GeoPoint
TaskAutoPilot::get_start_location(const TaskAccessor& task, bool previous)
{
if (!previous && (task.is_ordered())) {
// set start location to 200 meters directly behind start
// (otherwise start may fail to trigger)
Angle brg = w[0].Bearing(w[1]);
return GeoVector(fixed(200), brg).EndPoint(w[1]);
} else {
return w[0];
}
}
bool
TaskAutoPilot::far_from_target(const TaskAccessor& task, const AircraftState& state)
{
// are we considered close to the target?
if (task.is_empty() || !task.leg_stats().remaining.IsDefined())
return w[0].Distance(state.location)>state.ground_speed;
bool d_far = (task.leg_stats().remaining.GetDistance() > fixed(100));
if (!task.is_ordered())
// cheat for non-ordered tasks
return d_far;
bool entered = awp >= task.size() || task.has_entered(awp);
if (current_has_target(task))
return d_far || !entered;
fixed dc = w[0].Distance(state.location);
if (awp==0) {
return (dc>state.ground_speed);
}
return (dc>state.ground_speed) || !entered;
}
void
TaskAutoPilot::update_state(const TaskAccessor& task,
AircraftState& state, const fixed timestep)
{
const GlidePolar &glide_polar = task.get_glide_polar();
switch (acstate) {
case Cruise:
case FinalGlide:
{
const ElementStat &stat = task.leg_stats();
if (positive(stat.solution_remaining.v_opt)) {
state.true_airspeed = stat.solution_remaining.v_opt*speed_factor;
} else {
state.true_airspeed = glide_polar.GetVBestLD();
}
state.indicated_airspeed = state.true_airspeed;
state.vario = -glide_polar.SinkRate(state.true_airspeed)*parms.sink_factor;
update_cruise_bearing(task, state, timestep);
}
break;
case Climb:
{
state.true_airspeed = glide_polar.GetVMin();
fixed d = parms.turn_speed*timestep;
if (d< fixed_360) {
heading += Angle::Degrees(d);
}
if (positive(parms.bearing_noise)) {
heading += heading_deviation()*timestep;
}
heading = heading.AsBearing();
state.vario = climb_rate*parms.climb_factor;
}
break;
};
state.netto_vario = state.vario+glide_polar.SinkRate(state.true_airspeed);
}
void
TaskAutoPilot::update_state(const TaskAccessor& task,
AIRCRAFT_STATE& state, const fixed timestep)
{
const GlidePolar &glide_polar = task.get_glide_polar();
switch (acstate) {
case Cruise:
case FinalGlide:
{
const ElementStat stat = task.leg_stats();
if (positive(stat.solution_remaining.VOpt)) {
state.TrueAirspeed = stat.solution_remaining.VOpt*speed_factor;
} else {
state.TrueAirspeed = glide_polar.get_VbestLD();
}
state.IndicatedAirspeed = state.TrueAirspeed;
state.Vario = -glide_polar.SinkRate(state.TrueAirspeed)*parms.sink_factor;
update_cruise_bearing(task, state, timestep);
}
break;
case Climb:
{
state.TrueAirspeed = glide_polar.get_Vmin();
fixed d = parms.turn_speed*timestep;
if (d< fixed_360) {
heading += Angle::degrees(d);
}
if (positive(parms.bearing_noise)) {
heading += heading_deviation()*timestep;
}
heading = heading.as_bearing();
state.Vario = climb_rate*parms.climb_factor;
}
break;
};
state.NettoVario = state.Vario+glide_polar.SinkRate(state.TrueAirspeed);
}
void
TaskAutoPilot::update_cruise_bearing(const TaskAccessor& task,
const AircraftState& state,
const fixed timestep)
{
const ElementStat &stat = task.leg_stats();
Angle bct = stat.solution_remaining.cruise_track_bearing;
Angle bearing;
if (current_has_target(task)) {
bearing = stat.solution_remaining.vector.bearing;
if (parms.enable_bestcruisetrack && (stat.solution_remaining.vector.distance>fixed_1000)) {
bearing = bct;
}
} else {
bearing = state.location.Bearing(target(task));
}
if (positive(state.wind.norm) && positive(state.true_airspeed)) {
const fixed sintheta = (state.wind.bearing-bearing).sin();
if (fabs(sintheta)>fixed(0.0001)) {
bearing +=
Angle::Radians(asin(sintheta * state.wind.norm / state.true_airspeed));
}
}
Angle diff = (bearing-heading).AsDelta();
fixed d = diff.Degrees();
fixed max_turn = parms.turn_speed*timestep;
heading += Angle::Degrees(max(-max_turn, min(max_turn, d)));
if (positive(parms.bearing_noise)) {
heading += heading_deviation()*timestep;
}
heading = heading.AsBearing();
}
void
TaskAutoPilot::update_cruise_bearing(const TaskAccessor& task,
const AIRCRAFT_STATE& state,
const fixed timestep)
{
const ElementStat stat = task.leg_stats();
Angle bct = stat.solution_remaining.CruiseTrackBearing;
Angle bearing;
if (current_has_target(task)) {
bearing = stat.solution_remaining.Vector.Bearing;
if (parms.enable_bestcruisetrack && (stat.solution_remaining.Vector.Distance>fixed_1000)) {
bearing = bct;
}
} else {
bearing = state.Location.bearing(target(task));
}
if (positive(state.wind.norm) && positive(state.TrueAirspeed)) {
const fixed sintheta = (state.wind.bearing-bearing).sin();
if (fabs(sintheta)>fixed(0.0001)) {
bearing +=
Angle::radians(asin(sintheta * state.wind.norm / state.TrueAirspeed));
}
}
Angle diff = (bearing-heading).as_delta();
fixed d = diff.value_degrees();
fixed max_turn = parms.turn_speed*timestep;
heading += Angle::degrees(max(-max_turn, min(max_turn, d)));
if (positive(parms.bearing_noise)) {
heading += heading_deviation()*timestep;
}
heading = heading.as_bearing();
}
void
TaskAutoPilot::advance_if_required(TaskAccessor& task)
{
bool manual_start = false;
if (task.is_started() && (task.getActiveTaskPointIndex()==0)) {
manual_start = true;
awp++;
}
if (current_has_target(task) || manual_start) {
if (task.getActiveTaskPointIndex() < awp) {
// manual advance
task.setActiveTaskPoint(awp);
on_manual_advance();
get_awp(task);
}
}
if (task.getActiveTaskPointIndex() > awp) {
awp = task.getActiveTaskPointIndex();
get_awp(task);
}
}
void
TaskAutoPilot::get_awp(TaskAccessor& task)
{
w[0] = task.random_oz_point(awp, parms.target_noise);
}
fixed
TaskAutoPilot::target_height(const TaskAccessor& task) const
{
return task.target_height();
}
