From 7ea971d948f4626b080b5c524368598ce7e7a154 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Jul 2013 11:01:10 +0900 Subject: [PATCH] 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. --- ArduCopter/GCS_Mavlink.pde | 17 +- ArduCopter/commands_logic.pde | 10 +- ArduCopter/commands_process.pde | 4 +- ArduCopter/control_modes.pde | 17 +- ArduCopter/defines.h | 1 + ArduCopter/events.pde | 43 ++-- ArduCopter/fence.pde | 2 +- ArduCopter/system.pde | 336 +++++++++++++++++--------------- 8 files changed, 235 insertions(+), 195 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index eea5c34e73..3fc1e83a6f 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 9ac54e83b2..054f72d328 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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 diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 6678acf9de..e7cda6809f 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -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); + } } } diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 3a12228a6b..8acb2883d0 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -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; } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 86167159af..3cfcf84a74 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 9adb87c8bc..9ea43ca39d 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -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); diff --git a/ArduCopter/fence.pde b/ArduCopter/fence.pde index e9435040d8..ac90126ae5 100644 --- a/ArduCopter/fence.pde +++ b/ArduCopter/fence.pde @@ -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) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e86f7c87f8..0177168f8a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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