2016-01-14 15:30:56 -04:00
# include "Sub.h"
2015-12-30 18:57:56 -04:00
/*
* 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 )
2015-12-30 18:57:56 -04:00
{
// boolean to record if flight mode could be set
bool success = false ;
2016-12-09 13:16:56 -04:00
bool ignore_checks = false ; // Always check for now
2015-12-30 18:57:56 -04:00
// return immediately if we are already in the desired mode
if ( mode = = control_mode ) {
2017-02-03 17:33:27 -04:00
prev_control_mode = control_mode ;
prev_control_mode_reason = control_mode_reason ;
2016-04-18 01:38:21 -03:00
2017-02-03 17:33:27 -04:00
control_mode_reason = reason ;
2015-12-30 18:57:56 -04:00
return true ;
}
2017-02-03 17:33:27 -04:00
switch ( mode ) {
case ACRO :
success = acro_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case STABILIZE :
success = stabilize_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case ALT_HOLD :
success = althold_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case AUTO :
success = auto_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case CIRCLE :
success = circle_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case VELHOLD :
success = velhold_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case GUIDED :
success = guided_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case SURFACE :
success = surface_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
# if AUTOTUNE_ENABLED == ENABLED
2017-02-03 17:33:27 -04:00
case AUTOTUNE :
success = autotune_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
# endif
# if POSHOLD_ENABLED == ENABLED
2017-02-03 17:33:27 -04:00
case POSHOLD :
success = poshold_init ( ignore_checks ) ;
break ;
2015-12-30 18:57:56 -04:00
# endif
2017-02-03 17:33:27 -04:00
case MANUAL :
success = manual_init ( ignore_checks ) ;
break ;
2016-06-26 01:07:27 -03:00
2017-02-03 17:33:27 -04:00
default :
success = false ;
break ;
2015-12-30 18:57:56 -04:00
}
// 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 ;
2015-12-30 18:57:56 -04:00
control_mode = mode ;
2016-04-18 01:38:21 -03:00
control_mode_reason = reason ;
DataFlash . Log_Write_Mode ( control_mode , control_mode_reason ) ;
2015-12-30 18:57:56 -04:00
# 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
2017-02-03 17:33:27 -04:00
} else {
2015-12-30 18:57:56 -04:00
// 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 ( )
2015-12-30 18:57:56 -04:00
{
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
switch ( control_mode ) {
2017-02-03 17:33:27 -04:00
case ACRO :
acro_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case STABILIZE :
stabilize_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case ALT_HOLD :
althold_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case AUTO :
auto_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case CIRCLE :
circle_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case VELHOLD :
velhold_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case GUIDED :
guided_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case SURFACE :
surface_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
# if AUTOTUNE_ENABLED == ENABLED
2017-02-03 17:33:27 -04:00
case AUTOTUNE :
autotune_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
# endif
# if POSHOLD_ENABLED == ENABLED
2017-02-03 17:33:27 -04:00
case POSHOLD :
poshold_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
# endif
2017-02-03 17:33:27 -04:00
case MANUAL :
manual_run ( ) ;
break ;
2016-06-26 01:07:27 -03:00
2017-02-03 17:33:27 -04:00
default :
break ;
2015-12-30 18:57:56 -04:00
}
}
// 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 )
2015-12-30 18:57:56 -04:00
{
# 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
}
}
// returns true or false whether mode requires GPS
2017-02-03 17:33:27 -04:00
bool Sub : : mode_requires_GPS ( control_mode_t mode )
{
switch ( mode ) {
case AUTO :
case GUIDED :
case VELHOLD :
case CIRCLE :
case POSHOLD :
return true ;
default :
return false ;
2015-12-30 18:57:56 -04:00
}
return false ;
}
// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
2017-02-03 17:33:27 -04:00
bool Sub : : mode_has_manual_throttle ( control_mode_t mode )
{
switch ( mode ) {
case ACRO :
case STABILIZE :
case MANUAL :
return true ;
default :
return false ;
2015-12-30 18:57:56 -04:00
}
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
2017-02-03 17:33:27 -04:00
bool Sub : : mode_allows_arming ( control_mode_t mode , bool arming_from_gcs )
{
2017-02-17 21:51:03 -04:00
if ( mode_has_manual_throttle ( mode ) | | mode = = VELHOLD | | mode = = ALT_HOLD | | mode = = POSHOLD | | ( arming_from_gcs & & mode = = GUIDED ) ) {
2015-12-30 18:57:56 -04:00
return true ;
}
return false ;
}
// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device
2017-02-03 17:33:27 -04:00
void Sub : : notify_flight_mode ( control_mode_t mode )
{
switch ( mode ) {
case AUTO :
case GUIDED :
case CIRCLE :
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 ;
2015-12-30 18:57:56 -04:00
}
}
//
// 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 )
2015-12-30 18:57:56 -04:00
{
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 ;
2016-08-31 23:47:04 -03:00
case VELHOLD :
port - > print ( " VELHOLD " ) ;
2015-12-30 18:57:56 -04:00
break ;
case CIRCLE :
port - > print ( " CIRCLE " ) ;
break ;
2016-08-25 00:08:30 -03:00
case SURFACE :
port - > print ( " SURFACE " ) ;
2015-12-30 18:57:56 -04:00
break ;
case OF_LOITER :
port - > print ( " OF_LOITER " ) ;
break ;
case AUTOTUNE :
port - > print ( " AUTOTUNE " ) ;
break ;
case POSHOLD :
port - > print ( " POSHOLD " ) ;
break ;
2016-07-04 13:46:37 -03:00
case MANUAL :
2017-02-03 17:33:27 -04:00
port - > print ( " MANUAL " ) ;
break ;
2015-12-30 18:57:56 -04:00
default :
port - > printf ( " Mode(%u) " , ( unsigned ) mode ) ;
break ;
}
}