zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: add flight_mode.pde

master
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
38d5148b99
commit
b8bb477731
1 changed files with 278 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 278
      ArduCopter/flight_mode.pde

278
ArduCopter/flight_mode.pde
Unescape View File

@ -0,0 +1,278 @@ @@ -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;
}
}
Write Preview
Loading…
Cancel
Save