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

@@ -1268,22 +1268,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // custom_mode instead.
             break;
         }
-        switch (packet.custom_mode) {
+        set_mode(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;
-        }
-
         break;
     }
 

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);
+        if (set_mode(GUIDED)) {
-        first_time = true;
+            first_time = true;
+        }else{
+            // if we failed to enter guided mode return immediately
+            return;
+        }
     }
 
     // set wp_nav's destination

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);
+            }
         }
     }
 

ArduCopter/control_modes.pde

@@ -15,12 +15,13 @@ static void read_control_switch()
             oldSwitchPosition       = switchPosition;
             switch_counter          = 0;
 
-            set_mode(flight_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) {
+                if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE) {
-                // set Simple mode using stored paramters from Mission planner
+                    // set Simple mode using stored paramters from Mission planner
-                // rather than by the control switch
+                    // rather than by the control switch
-                set_simple_mode(BIT_IS_SET(g.simple_modes, switchPosition));
+                    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;
                 }
 

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

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()) {
+            }else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
-                set_mode(RTL);
+                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()) {
+                if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
-                    set_mode(RTL);
+                    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()) {
+            }else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
-                set_mode(RTL);
+                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 to RTL or LAND
-                    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);
@@ -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()) {
+            }else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
-                set_mode(RTL);
+                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()) {
+                if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
-                    set_mode(RTL);
+                    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()) {
+            if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
-                set_mode(RTL);
+                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);

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) {

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
+    // boolean to record if flight mode could be set
-    if(g_gps->status() != GPS::GPS_OK_FIX_3D && mode_requires_GPS(mode)) {
+    bool success = false;
-            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);
 
     // 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)
+    switch(mode) {
-    {
+        case ACRO:
-    case ACRO:
+            success = true;
-    	ap.manual_throttle = 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:
-    	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
             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;
+        control_mode = mode;
-        do_land(NULL);  // land at current location
+        Log_Write_Mode(control_mode);
-        break;
+    }else{
-
+        // Log error that we failed to enter desired flight mode
-    case RTL:
+        Log_Write_Error(ERROR_SUBSYSTEM_FLGHT_MODE,mode);
-    	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;
     }
 
-    if(ap.manual_attitude) {
+    // return success or failure
-        // We are under manual attitude control
+    return success;
-        // remove the navigation from roll and pitch command
-        reset_nav_params();
-    }
-
-    Log_Write_Mode(control_mode);
 }
 
 static void