Copter: add flight_mode.pde
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
38d5148b99
commit
b8bb477731
278
ArduCopter/flight_mode.pde
Normal file
278
ArduCopter/flight_mode.pde
Normal file
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user