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;
};
}
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 Altitude_Hold::ExecuteCycle() {
int t = hal.scheduler->micros();
float dt = (t - timestamp()) / 1000.0;
set_timestamp(t);
update_curr_height();
float height_err = target_height() - curr_height();
float height_correction = height_pid.get_pid(height_err, dt);
float throttle = hover_throttle() + height_correction * THROTTLE_CORRECTION_SCALE;
++count;
if(!(count % 50) && DEBUG) {
hal.console->printf("Height Correction: %f, Throttle: %f, Err: %f\n", height_correction, throttle, height_err);
}
set_hover_throttle(throttle);
flight_control()->execute(up_cntrl(), throttle, 0);
return false;
}
void AircraftSim::update_mode()
{
const ElementStat stat = task_manager.get_stats().current_leg;
switch (acstate) {
case Cruise:
if ((awp>0) &&
(task_manager.get_stats().total.solution_remaining.DistanceToFinal<= state.Speed)) {
print_mode("# mode fg\n");
acstate = FinalGlide;
} else {
if (state.NavAltitude<=target_height()) {
print_mode("# mode climb\n");
acstate = Climb;
}
}
break;
case FinalGlide:
if (task_manager.get_stats().total.solution_remaining.AltitudeDifference<-fixed_20) {
print_mode("# mode climb\n");
acstate = Climb;
}
break;
case Climb:
if ((awp>0) &&
(task_manager.get_stats().total.solution_remaining.DistanceToFinal<= state.Speed)) {
print_mode("# mode fg\n");
acstate = FinalGlide;
} else if (state.NavAltitude>=fixed_1500) {
acstate = Cruise;
print_mode("# mode cruise\n");
}
break;
};
}
