Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: check set_mode for failure 

Previously if set_mode failed it would return the copter to stabilize
mode.  With this commit set_mode returns a true/false indicating whether
it succeeded or not so the caller can make the decision as to the
appropriate response which could be to stay in the current flight mode
or try another flight mode.
This commit is contained in:
Randy Mackay 2013-07-20 11:01:10 +09:00
parent 2a9f4bbbad
commit 7ea971d948
8 changed files with 235 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/GCS_Mavlink.pde
View File

@ -1268,22 +1268,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// custom_mode instead.
break;
}
switch (packet.custom_mode) {
case STABILIZE:
case ACRO:
case ALT_HOLD:
case AUTO:
case GUIDED:
case LOITER:
case RTL:
case CIRCLE:
case POSITION:
case LAND:
case OF_LOITER:
set_mode(packet.custom_mode);
break;
}
set_mode(packet.custom_mode);
break;
}

10
ArduCopter/commands_logic.pde
View File

@ -321,7 +321,7 @@ static void do_land(const struct Location *cmd)
land_state = LAND_STATE_DESCENDING;
// if we have gps lock, attempt to hold horizontal position
if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
if (GPS_ok()) {
// switch to loiter which restores horizontal control to pilot
// To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
set_roll_pitch_mode(ROLL_PITCH_LOITER);
@ -826,8 +826,12 @@ static void do_guided(const struct Location *cmd)
bool first_time = false;
// switch to guided mode if we're not already in guided mode
if (control_mode != GUIDED) {
set_mode(GUIDED);
first_time = true;
if (set_mode(GUIDED)) {
first_time = true;
}else{
// if we failed to enter guided mode return immediately
return;
}
}
// set wp_nav's destination

4
ArduCopter/commands_process.pde
View File

@ -192,7 +192,9 @@ static void exit_mission()
if(g.rtl_alt_final == 0) {
set_mode(LAND);
}else{
set_mode(LOITER);
if (!set_mode(LOITER)) {
set_mode(LAND);
}
}
}

17
ArduCopter/control_modes.pde
View File

@ -15,12 +15,13 @@ static void read_control_switch()
oldSwitchPosition = switchPosition;
switch_counter = 0;
set_mode(flight_modes[switchPosition]);
if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE) {
// set Simple mode using stored paramters from Mission planner
// rather than by the control switch
set_simple_mode(BIT_IS_SET(g.simple_modes, switchPosition));
// set flight mode and simple mode setting
if (set_mode(flight_modes[switchPosition])) {
if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE) {
// set Simple mode using stored paramters from Mission planner
// rather than by the control switch
set_simple_mode(BIT_IS_SET(g.simple_modes, switchPosition));
}
}
}
@ -135,7 +136,7 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag)
case AUX_SWITCH_RTL:
if (ch_flag) {
// engage RTL
// engage RTL (if not possible we remain in current flight mode)
set_mode(RTL);
}else{
// disengage RTL to previous flight mode if we are currently in RTL or loiter
@ -159,7 +160,7 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag)
if(control_mode == AUTO) {
aux_switch_wp_index = 0;
g.command_total.set_and_save(1);
set_mode(RTL);
set_mode(RTL); // if by chance we are unable to switch to RTL we just stay in AUTO and hope the GPS failsafe will take-over
return;
}

1
ArduCopter/defines.h
View File

@ -416,6 +416,7 @@ enum ap_message {
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLGHT_MODE 10
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1

43
ArduCopter/events.pde
View File

@ -21,8 +21,10 @@ static void failsafe_radio_on_event()
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
}else if(ap.home_is_set == true && g_gps->status() == GPS::GPS_OK_FIX_3D && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
}else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
@ -31,8 +33,10 @@ static void failsafe_radio_on_event()
case AUTO:
// failsafe_throttle is 1 do RTL, 2 means continue with the mission
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// We are very close to home so we will land
set_mode(LAND);
@ -52,8 +56,10 @@ static void failsafe_radio_on_event()
if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
}else if(ap.home_is_set == true && g_gps->status() == GPS::GPS_OK_FIX_3D && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
}else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)){
set_mode(LAND);
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
@ -91,8 +97,11 @@ static void low_battery_event(void)
}
break;
case AUTO:
if(ap.home_is_set == true && g_gps->status() == GPS::GPS_OK_FIX_3D && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
// set_mode to RTL or LAND
if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
@ -211,8 +220,10 @@ static void failsafe_gcs_check()
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
}else if(ap.home_is_set == true && g_gps->status() == GPS::GPS_OK_FIX_3D && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
}else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
@ -221,8 +232,10 @@ static void failsafe_gcs_check()
case AUTO:
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// We are very close to home so we will land
set_mode(LAND);
@ -231,8 +244,10 @@ static void failsafe_gcs_check()
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
if(ap.home_is_set == true && g_gps->status() == GPS::GPS_OK_FIX_3D && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);

2
ArduCopter/fence.pde
View File

@ -36,7 +36,7 @@ void fence_check()
init_disarm_motors();
}else{
// if we have a GPS
if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
if (GPS_ok()) {
// if we are within 100m of the fence, RTL
if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if(control_mode != RTL) {

336
ArduCopter/system.pde
View File

@ -312,6 +312,16 @@ static void startup_ground(void)
reset_I_all();
}
// returns true if the GPS is ok and home position is set
static bool GPS_ok()
{
if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D) {
return true;
}else{
return false;
}
}
// returns true or false whether mode requires GPS
static bool mode_requires_GPS(uint8_t mode) {
switch(mode) {
@ -330,170 +340,192 @@ static bool mode_requires_GPS(uint8_t mode) {
}
// set_mode - change flight mode and perform any necessary initialisation
static void set_mode(uint8_t mode)
// returns true if mode was succesfully set
// STABILIZE, ACRO and LAND 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)
{
// Switch to stabilize mode if requested mode requires a GPS lock
if(g_gps->status() != GPS::GPS_OK_FIX_3D && mode_requires_GPS(mode)) {
mode = STABILIZE;
} else if(mode == RTL && !ap.home_is_set) {
mode = STABILIZE;
}
// Switch to stabilize if OF_LOITER requested but no optical flow sensor
if (mode == OF_LOITER && !g.optflow_enabled ) {
mode = STABILIZE;
}
control_mode = mode;
control_mode = constrain_int16(control_mode, 0, NUM_MODES - 1);
// boolean to record if flight mode could be set
bool success = false;
// if we change modes, we must clear landed flag
// To-Do: this should be initialised in one of the flight modes
set_land_complete(false);
// report the GPS and Motor arming status
// To-Do: this should be initialised somewhere else related to the LEDs
led_mode = NORMAL_LEDS;
switch(control_mode)
{
case ACRO:
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(ACRO_YAW);
set_roll_pitch_mode(ACRO_RP);
set_throttle_mode(ACRO_THR);
set_nav_mode(NAV_NONE);
// reset acro axis targets to current attitude
if(g.axis_enabled){
roll_axis = 0;
pitch_axis = 0;
nav_yaw = 0;
}
break;
case STABILIZE:
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
set_nav_mode(NAV_NONE);
break;
case ALT_HOLD:
ap.manual_throttle = false;
ap.manual_attitude = true;
set_yaw_mode(ALT_HOLD_YAW);
set_roll_pitch_mode(ALT_HOLD_RP);
set_throttle_mode(ALT_HOLD_THR);
set_nav_mode(NAV_NONE);
break;
case AUTO:
ap.manual_throttle = false;
ap.manual_attitude = false;
// roll-pitch, throttle and yaw modes will all be set by the first nav command
init_commands(); // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
break;
case CIRCLE:
ap.manual_throttle = false;
ap.manual_attitude = false;
set_roll_pitch_mode(CIRCLE_RP);
set_throttle_mode(CIRCLE_THR);
set_nav_mode(CIRCLE_NAV);
set_yaw_mode(CIRCLE_YAW);
break;
case LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(LOITER_YAW);
set_roll_pitch_mode(LOITER_RP);
set_throttle_mode(LOITER_THR);
set_nav_mode(LOITER_NAV);
break;
case POSITION:
ap.manual_throttle = true;
ap.manual_attitude = false;
set_yaw_mode(POSITION_YAW);
set_roll_pitch_mode(POSITION_RP);
set_throttle_mode(POSITION_THR);
set_nav_mode(POSITION_NAV);
break;
case GUIDED:
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(get_wp_yaw_mode(false));
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
set_nav_mode(GUIDED_NAV);
break;
case LAND:
// To-Do: it is messy to set manual_attitude here because the do_land function is reponsible for setting the roll_pitch_mode
if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
// switch to loiter if we have gps
ap.manual_attitude = false;
}else{
// otherwise remain with stabilize roll and pitch
switch(mode) {
case ACRO:
success = true;
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(ACRO_YAW);
set_roll_pitch_mode(ACRO_RP);
set_throttle_mode(ACRO_THR);
set_nav_mode(NAV_NONE);
// reset acro axis targets to current attitude
if(g.axis_enabled){
roll_axis = 0;
pitch_axis = 0;
nav_yaw = 0;
}
break;
case STABILIZE:
success = true;
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
set_nav_mode(NAV_NONE);
break;
case ALT_HOLD:
success = true;
ap.manual_throttle = false;
ap.manual_attitude = true;
set_yaw_mode(ALT_HOLD_YAW);
set_roll_pitch_mode(ALT_HOLD_RP);
set_throttle_mode(ALT_HOLD_THR);
set_nav_mode(NAV_NONE);
break;
case AUTO:
// check we have a GPS and at least one mission command (note the home position is always command 0)
if (GPS_ok() && g.command_total > 1) {
success = true;
ap.manual_throttle = false;
ap.manual_attitude = false;
// roll-pitch, throttle and yaw modes will all be set by the first nav command
init_commands(); // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
}
break;
case CIRCLE:
if (GPS_ok()) {
success = true;
ap.manual_throttle = false;
ap.manual_attitude = false;
set_roll_pitch_mode(CIRCLE_RP);
set_throttle_mode(CIRCLE_THR);
set_nav_mode(CIRCLE_NAV);
set_yaw_mode(CIRCLE_YAW);
}
break;
case LOITER:
if (GPS_ok()) {
success = true;
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(LOITER_YAW);
set_roll_pitch_mode(LOITER_RP);
set_throttle_mode(LOITER_THR);
set_nav_mode(LOITER_NAV);
}
break;
case POSITION:
if (GPS_ok()) {
success = true;
ap.manual_throttle = true;
ap.manual_attitude = false;
set_yaw_mode(POSITION_YAW);
set_roll_pitch_mode(POSITION_RP);
set_throttle_mode(POSITION_THR);
set_nav_mode(POSITION_NAV);
}
break;
case GUIDED:
if (GPS_ok()) {
success = true;
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(get_wp_yaw_mode(false));
set_roll_pitch_mode(GUIDED_RP);
set_throttle_mode(GUIDED_THR);
set_nav_mode(GUIDED_NAV);
}
break;
case LAND:
success = true;
// To-Do: it is messy to set manual_attitude here because the do_land function is reponsible for setting the roll_pitch_mode
ap.manual_attitude = !GPS_ok();
ap.manual_throttle = false;
do_land(NULL); // land at current location
break;
case RTL:
if (GPS_ok()) {
success = true;
ap.manual_throttle = false;
ap.manual_attitude = false;
do_RTL();
}
break;
case OF_LOITER:
if (g.optflow_enabled) {
success = true;
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(OF_LOITER_YAW);
set_roll_pitch_mode(OF_LOITER_RP);
set_throttle_mode(OF_LOITER_THR);
set_nav_mode(OF_LOITER_NAV);
}
break;
// THOR
// These are the flight modes for Toy mode
// See the defines for the enumerated values
case TOY_A:
success = true;
ap.manual_throttle = false;
ap.manual_attitude = true;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
set_throttle_mode(THROTTLE_AUTO);
set_nav_mode(NAV_NONE);
// save throttle for fast exit of Alt hold
saved_toy_throttle = g.rc_3.control_in;
break;
case TOY_M:
success = true;
ap.manual_throttle = false;
ap.manual_attitude = true;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
set_nav_mode(NAV_NONE);
set_throttle_mode(THROTTLE_HOLD);
break;
default:
success = false;
break;
}
// update flight mode
if (success) {
if(ap.manual_attitude) {
// We are under manual attitude control so initialise nav parameter for when we next enter an autopilot mode
reset_nav_params();
}
ap.manual_throttle = false;
do_land(NULL); // land at current location
break;
case RTL:
ap.manual_throttle = false;
ap.manual_attitude = false;
do_RTL();
break;
case OF_LOITER:
ap.manual_throttle = false;
ap.manual_attitude = false;
set_yaw_mode(OF_LOITER_YAW);
set_roll_pitch_mode(OF_LOITER_RP);
set_throttle_mode(OF_LOITER_THR);
set_nav_mode(OF_LOITER_NAV);
break;
// THOR
// These are the flight modes for Toy mode
// See the defines for the enumerated values
case TOY_A:
ap.manual_throttle = false;
ap.manual_attitude = true;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
set_throttle_mode(THROTTLE_AUTO);
set_nav_mode(NAV_NONE);
// save throttle for fast exit of Alt hold
saved_toy_throttle = g.rc_3.control_in;
break;
case TOY_M:
ap.manual_throttle = false;
ap.manual_attitude = true;
set_yaw_mode(YAW_TOY);
set_roll_pitch_mode(ROLL_PITCH_TOY);
set_nav_mode(NAV_NONE);
set_throttle_mode(THROTTLE_HOLD);
break;
default:
break;
control_mode = mode;
Log_Write_Mode(control_mode);
}else{
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLGHT_MODE,mode);
}
if(ap.manual_attitude) {
// We are under manual attitude control
// remove the navigation from roll and pitch command
reset_nav_params();
}
Log_Write_Mode(control_mode);
// return success or failure
return success;
}
static void