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