char *ft_term(t_shell *shell, int j, int f)
{
char buf[5];
t_line *tmp;
char *str;
tmp = shell->hist;
str = NULL;
ft_putstr(tgetstr("sc", NULL));
f = 0;
while (42)
{
ft_read(buf);
if (buf[0] == '\n')
return (exit_mode(shell, str, j));
else if (K_UP(buf))
f *= history(&tmp, 'A', &str);
else if (K_DOWN(buf))
f *= history(&tmp, 'B', &str);
else if (K_END(buf))
f = 0;
else if (K_HOME(buf))
f = ft_strlen(str);
else
term_center(shell, &str, buf, &f);
}
return (str);
}
void Plane::set_mode(enum FlightMode mode)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
// zero locked course
steer_state.locked_course_err = 0;
// set mode
previous_mode = control_mode;
control_mode = mode;
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// zero initial pitch and highest airspeed on mode change
auto_state.highest_airspeed = 0;
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
auto_state.fbwa_tdrag_takeoff_mode = false;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
break;
case RTL:
auto_throttle_mode = true;
prev_WP_loc = current_loc;
do_RTL();
break;
case LOITER:
auto_throttle_mode = true;
do_loiter_at_location();
break;
case GUIDED:
auto_throttle_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode
if (mode == control_mode) {
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode_reason = reason;
return true;
}
switch(mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
success = heli_acro_init(ignore_checks);
#else
success = acro_init(ignore_checks);
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
break;
case ALT_HOLD:
success = althold_init(ignore_checks);
break;
case AUTO:
success = auto_init(ignore_checks);
break;
case CIRCLE:
success = circle_init(ignore_checks);
break;
case LOITER:
success = loiter_init(ignore_checks);
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
case LAND:
success = land_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
case DRIFT:
success = drift_init(ignore_checks);
break;
case SPORT:
success = sport_init(ignore_checks);
break;
case ALT_POS:
success = altpos_init(ignore_checks);
break;
case FLIP:
success = flip_init(ignore_checks);
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = autotune_init(ignore_checks);
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = poshold_init(ignore_checks);
break;
#endif
case BRAKE:
success = brake_init(ignore_checks);
break;
case THROW:
success = throw_init(ignore_checks);
break;
case AVOID_ADSB:
success = avoid_adsb_init(ignore_checks);
break;
case GUIDED_NOGPS:
success = guided_nogps_init(ignore_checks);
break;
default:
success = false;
break;
}
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode, mode);
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
}else{
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
}
// update notify object
if (success) {
notify_flight_mode(control_mode);
}
// return success or failure
return success;
}
// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was successfully set
// ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
// return immediately if we are already in the desired mode
if (mode == control_mode) {
control_mode_reason = reason;
return true;
}
Copter::Mode *new_flightmode = mode_from_mode_num(mode);
if (new_flightmode == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()){
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
#endif
if (!new_flightmode->init(ignore_checks)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
// perform any cleanup required by previous flight mode
exit_mode(flightmode, new_flightmode);
// update flight mode
flightmode = new_flightmode;
control_mode = mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, reason);
#if ADSB_ENABLED == ENABLED
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
#endif
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
#if DEVO_TELEM_ENABLED == ENABLED
devo_telemetry.update_control_mode(control_mode);
#endif
#if CAMERA == ENABLED
camera.set_is_auto_mode(control_mode == AUTO);
#endif
// update notify object
notify_flight_mode();
// return success
return true;
}
void Plane::set_mode(enum FlightMode mode)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
// not in pre-flare
auto_state.land_pre_flare = false;
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// always reset this because we don't know who called set_mode. In evasion
// behavior you should set this flag after set_mode so you know the evasion
// logic is controlling the mode. This allows manual override of the mode
// to exit evasion behavior automatically but if the mode is manually switched
// then we won't resume AUTO after an evasion
adsb_state.is_evading = false;
// set mode
previous_mode = control_mode;
control_mode = mode;
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// zero initial pitch and highest airspeed on mode change
auto_state.highest_airspeed = 0;
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
auto_state.fbwa_tdrag_takeoff_mode = false;
// start with previous WP at current location
prev_WP_loc = current_loc;
// new mode means new loiter
loiter.start_time_ms = 0;
// assume non-VTOL mode
auto_state.vtol_mode = false;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
auto_state.vtol_mode = false;
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
break;
case RTL:
auto_throttle_mode = true;
prev_WP_loc = current_loc;
do_RTL();
break;
case LOITER:
auto_throttle_mode = true;
do_loiter_at_location();
break;
case GUIDED:
auto_throttle_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
// change flight mode and perform any necessary initialisation
// returns true if mode was successfully set
bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
{
// boolean to record if flight mode could be set
bool success = false;
// return immediately if we are already in the desired mode
if (mode == control_mode) {
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode_reason = reason;
return true;
}
switch (mode) {
case ACRO:
success = acro_init();
break;
case STABILIZE:
success = stabilize_init();
break;
case ALT_HOLD:
success = althold_init();
break;
case AUTO:
success = auto_init();
break;
case CIRCLE:
success = circle_init();
break;
case GUIDED:
success = guided_init();
break;
case SURFACE:
success = surface_init();
break;
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = poshold_init();
break;
#endif
case MANUAL:
success = manual_init();
break;
default:
success = false;
break;
}
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode, mode);
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
control_mode_reason = reason;
logger.Write_Mode(control_mode, control_mode_reason);
// update notify object
notify_flight_mode(control_mode);
#if CAMERA == ENABLED
camera.set_is_auto_mode(control_mode == AUTO);
#endif
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
} else {
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
}
// return success or failure
return success;
}
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_no_crosstrack = true;
// reset landing check
auto_state.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
// not in pre-flare
auto_state.land_pre_flare = false;
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// reset external attitude guidance
guided_state.last_forced_rpy_ms.zero();
guided_state.last_forced_throttle_ms = 0;
// set mode
previous_mode = control_mode;
control_mode = mode;
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
// restore last gains
autotune_restore();
}
// zero initial pitch and highest airspeed on mode change
auto_state.highest_airspeed = 0;
auto_state.initial_pitch_cd = ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
auto_state.fbwa_tdrag_takeoff_mode = false;
// start with previous WP at current location
prev_WP_loc = current_loc;
// new mode means new loiter
loiter.start_time_ms = 0;
// assume non-VTOL mode
auto_state.vtol_mode = false;
auto_state.vtol_loiter = false;
switch(control_mode)
{
case INITIALISING:
auto_throttle_mode = true;
auto_navigation_mode = false;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
auto_throttle_mode = false;
auto_navigation_mode = false;
break;
case AUTOTUNE:
auto_throttle_mode = false;
auto_navigation_mode = false;
autotune_start();
break;
case ACRO:
auto_throttle_mode = false;
auto_navigation_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
auto_navigation_mode = false;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
auto_navigation_mode = false;
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
auto_throttle_mode = true;
auto_navigation_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
auto_throttle_mode = true;
auto_navigation_mode = true;
if (quadplane.available() && quadplane.enable == 2) {
auto_state.vtol_mode = true;
} else {
auto_state.vtol_mode = false;
}
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
break;
case RTL:
auto_throttle_mode = true;
auto_navigation_mode = true;
prev_WP_loc = current_loc;
do_RTL(get_RTL_altitude());
break;
case LOITER:
auto_throttle_mode = true;
auto_navigation_mode = true;
do_loiter_at_location();
break;
case AVOID_ADSB:
case GUIDED:
auto_throttle_mode = true;
auto_navigation_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
auto_navigation_mode = false;
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
adsb.set_is_auto_mode(auto_navigation_mode);
if (should_log(MASK_LOG_MODE))
DataFlash.Log_Write_Mode(control_mode);
// reset attitude integrators on mode change
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
steerController.reset_I();
}
