From b8bb477731319f00fc11cbcedde6a3cd22a58a03 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Feb 2014 17:57:31 +0900 Subject: [PATCH] Copter: add flight_mode.pde --- ArduCopter/flight_mode.pde | 278 +++++++++++++++++++++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 ArduCopter/flight_mode.pde diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde new file mode 100644 index 0000000000..ccf7f7642e --- /dev/null +++ b/ArduCopter/flight_mode.pde @@ -0,0 +1,278 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * 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 +static bool set_mode(uint8_t mode) +{ + // 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) { + 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 OF_LOITER: + success = ofloiter_init(ignore_checks); + break; + + case DRIFT: + success = drift_init(ignore_checks); + break; + + case SPORT: + success = sport_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 + + default: + success = false; + break; + } + + // update flight mode + if (success) { + // perform any cleanup required by previous flight mode + exit_mode(control_mode); + control_mode = mode; + Log_Write_Mode(control_mode); + }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; +} + +// update_flight_mode - calls the appropriate attitude controllers based on flight mode +// called at 100hz or more +static void update_flight_mode() +{ + switch (control_mode) { + case ACRO: + #if FRAME_CONFIG == HELI_FRAME + heli_acro_run(); + #else + acro_run(); + #endif + break; + + case STABILIZE: + #if FRAME_CONFIG == HELI_FRAME + heli_stabilize_run(); + #else + stabilize_run(); + #endif + break; + + case ALT_HOLD: + althold_run(); + break; + + case AUTO: + auto_run(); + break; + + case CIRCLE: + circle_run(); + break; + + case LOITER: + loiter_run(); + break; + + case GUIDED: + guided_run(); + break; + + case LAND: + land_run(); + break; + + case RTL: + rtl_run(); + break; + + case OF_LOITER: + ofloiter_run(); + break; + + case DRIFT: + drift_run(); + break; + + case SPORT: + sport_run(); + break; + + case FLIP: + flip_run(); + break; + +#if AUTOTUNE_ENABLED == ENABLED + case AUTOTUNE: + autotune_run(); + break; +#endif + } +} + +// exit_mode - high level call to organise cleanup as a flight mode is exited +static void exit_mode(uint8_t old_control_mode) +{ +#if AUTOTUNE_ENABLED == ENABLED + if (old_control_mode == AUTOTUNE) { + autotune_stop(); + } +#endif +} + +// returns true or false whether mode requires GPS +static bool mode_requires_GPS(uint8_t mode) { + switch(mode) { + case AUTO: + case GUIDED: + case LOITER: + case RTL: + case CIRCLE: + case DRIFT: + return true; + default: + return false; + } + + return false; +} + +// manual_flight_mode - returns true if flight mode is completely manual (i.e. roll, pitch and yaw controlled by pilot) +static bool manual_flight_mode(uint8_t mode) { + switch(mode) { + case ACRO: + case STABILIZE: + case DRIFT: + case SPORT: + return true; + default: + return false; + } + + return false; +} + +// +// print_flight_mode - prints flight mode to serial port. +// +static void +print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) +{ + switch (mode) { + case STABILIZE: + port->print_P(PSTR("STABILIZE")); + break; + case ACRO: + port->print_P(PSTR("ACRO")); + break; + case ALT_HOLD: + port->print_P(PSTR("ALT_HOLD")); + break; + case AUTO: + port->print_P(PSTR("AUTO")); + break; + case GUIDED: + port->print_P(PSTR("GUIDED")); + break; + case LOITER: + port->print_P(PSTR("LOITER")); + break; + case RTL: + port->print_P(PSTR("RTL")); + break; + case CIRCLE: + port->print_P(PSTR("CIRCLE")); + break; + case LAND: + port->print_P(PSTR("LAND")); + break; + case OF_LOITER: + port->print_P(PSTR("OF_LOITER")); + break; + case DRIFT: + port->print_P(PSTR("DRIFT")); + break; + case SPORT: + port->print_P(PSTR("SPORT")); + break; + case FLIP: + port->print_P(PSTR("FLIP")); + break; + case AUTOTUNE: + port->print_P(PSTR("AUTOTUNE")); + break; + default: + port->printf_P(PSTR("Mode(%u)"), (unsigned)mode); + break; + } +}