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