mirror of https://github.com/ArduPilot/ardupilot
Copter: move set_mode, to flight_mode.pde
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
b8bb477731
commit
b0673d693b
|
|
@ -1294,80 +1294,6 @@ static void update_GPS(void)
|
|||
failsafe_gps_check();
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
init_simple_bearing()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -338,150 +338,6 @@ static bool GPS_ok()
|
|||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
// update_auto_armed - update status of auto_armed flag
|
||||
static void update_auto_armed()
|
||||
{
|
||||
|
|
@ -556,57 +412,3 @@ static void check_usb_mux(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
//
|
||||
// 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