ardupilot/ArduSub/flight_mode.cpp

336 lines
8.6 KiB
C++
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2016-01-14 15:30:56 -04:00
#include "Sub.h"
/*
* flight.pde - high level calls to set and update flight modes
* logic for individual flight modes is in control_acro.pde, control_stabilize.pde, etc
*/
// 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 succesfully 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
2016-04-18 01:38:21 -03:00
bool Sub::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) {
2016-04-18 01:38:21 -03:00
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode_reason = reason;
return true;
}
switch(mode) {
case ACRO:
2016-02-23 02:15:15 -04:00
success = acro_init(ignore_checks);
break;
case STABILIZE:
2016-02-23 02:15:15 -04:00
success = stabilize_init(ignore_checks);
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 VELHOLD:
success = velhold_init(ignore_checks);
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
2016-08-25 00:08:30 -03:00
case SURFACE:
success = surface_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
#if TRANSECT_ENABLED == ENABLED
case TRANSECT:
success = transect_init(ignore_checks);
break;
#endif
#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
2016-06-26 01:07:27 -03:00
case MANUAL:
success = manual_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);
2016-04-18 01:38:21 -03:00
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
2016-04-18 01:38:21 -03:00
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
#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;
}
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub::update_flight_mode()
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
switch (control_mode) {
case ACRO:
2016-02-23 02:15:15 -04:00
acro_run();
break;
case STABILIZE:
2016-02-23 02:15:15 -04:00
stabilize_run();
break;
case ALT_HOLD:
althold_run();
break;
case AUTO:
auto_run();
break;
case CIRCLE:
circle_run();
break;
case VELHOLD:
velhold_run();
break;
case GUIDED:
guided_run();
break;
2016-08-25 00:08:30 -03:00
case SURFACE:
surface_run();
break;
case RTL:
rtl_run();
break;
#if TRANSECT_ENABLED == ENABLED
case TRANSECT:
transect_run();
break;
#endif
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
autotune_run();
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
poshold_run();
break;
#endif
2016-06-26 01:07:27 -03:00
case MANUAL:
manual_run();
break;
2016-04-18 01:38:21 -03:00
default:
break;
}
}
// exit_mode - high level call to organise cleanup as a flight mode is exited
2016-04-18 01:38:21 -03:00
void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode)
{
#if AUTOTUNE_ENABLED == ENABLED
if (old_control_mode == AUTOTUNE) {
autotune_stop();
}
#endif
// stop mission when we leave auto mode
if (old_control_mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
#if MOUNT == ENABLED
camera_mount.set_mode_to_default();
#endif // MOUNT == ENABLED
}
// smooth throttle transition when switching from manual to automatic flight modes
2016-11-30 12:46:08 -04:00
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed()) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}
}
// returns true or false whether mode requires GPS
2016-04-18 01:38:21 -03:00
bool Sub::mode_requires_GPS(control_mode_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case VELHOLD:
case RTL:
case CIRCLE:
case POSHOLD:
case TRANSECT:
return true;
default:
return false;
}
return false;
}
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
2016-04-18 01:38:21 -03:00
bool Sub::mode_has_manual_throttle(control_mode_t mode) {
switch(mode) {
case ACRO:
case STABILIZE:
2016-07-04 12:26:23 -03:00
case MANUAL:
return true;
default:
return false;
}
return false;
}
// mode_allows_arming - returns true if vehicle can be armed in the specified mode
// arming_from_gcs should be set to true if the arming request comes from the ground station
2016-04-18 01:38:21 -03:00
bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) {
2016-11-25 20:25:35 -04:00
if (mode_has_manual_throttle(mode) || mode == VELHOLD || mode == ALT_HOLD || mode == POSHOLD || mode == TRANSECT || (arming_from_gcs && mode == GUIDED)) {
return true;
}
return false;
}
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
2016-04-18 01:38:21 -03:00
void Sub::notify_flight_mode(control_mode_t mode) {
switch(mode) {
case AUTO:
case GUIDED:
case RTL:
case CIRCLE:
2016-08-25 00:08:30 -03:00
case SURFACE:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;
default:
// all other are manual flight modes
AP_Notify::flags.autopilot_mode = false;
break;
}
}
//
// print_flight_mode - prints flight mode to serial port.
//
2016-01-14 15:30:56 -04:00
void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case STABILIZE:
port->print("STABILIZE");
break;
case ACRO:
port->print("ACRO");
break;
case ALT_HOLD:
port->print("ALT_HOLD");
break;
case AUTO:
port->print("AUTO");
break;
case GUIDED:
port->print("GUIDED");
break;
case VELHOLD:
port->print("VELHOLD");
break;
case RTL:
port->print("RTL");
break;
case CIRCLE:
port->print("CIRCLE");
break;
2016-08-25 00:08:30 -03:00
case SURFACE:
port->print("SURFACE");
break;
case OF_LOITER:
port->print("OF_LOITER");
break;
case TRANSECT:
port->print("TRANSECT");
break;
case AUTOTUNE:
port->print("AUTOTUNE");
break;
case POSHOLD:
port->print("POSHOLD");
break;
case MANUAL:
port->print("MANUAL");
break;
default:
port->printf("Mode(%u)", (unsigned)mode);
break;
}
}