Sub: Match copter mode change stuff

ArduSub/APM_Config.h

@@ -33,6 +33,7 @@
 // features below are disabled by default on all boards
 //#define SPRAYER               ENABLED             // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
 //#define GNDEFFECT_COMPENSATION ENABLED            // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground)
+//#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE   // disable mode changes from GCS during Radio failsafes.  Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link
 
 // other settings
 //#define THROTTLE_IN_DEADBAND   100                // redefine size of throttle deadband in pwm (0 ~ 1000)

ArduSub/GCS_Mavlink.cpp

@@ -59,6 +59,8 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
         // APM does in any mode, as that is defined as "system finds its own goal
         // positions", which APM does not currently do
         break;
+    default:
+    	break;
     }
 
     // all modes except INITIALISING have some form of manual
@@ -173,6 +175,8 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
     case SPORT:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
         break;
+    default:
+    	break;
     }
 
     // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
@@ -1001,7 +1005,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_SET_MODE:       // MAV ID: 11
     {
-        handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::set_mode, bool, uint8_t));
+#ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE
+        if (!sub.failsafe.radio) {
+            handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::gcs_set_mode, bool, uint8_t));
+        } else {
+            // don't allow mode changes while in radio failsafe
+            mavlink_msg_command_ack_send_buf(msg, chan, MAVLINK_MSG_ID_SET_MODE, MAV_RESULT_FAILED);
+        }
+#else
+        handle_set_mode(msg, FUNCTOR_BIND(&sub, &Sub::gcs_set_mode, bool, uint8_t));
+#endif
         break;
     }
 
@@ -1214,19 +1227,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 
         case MAV_CMD_NAV_LOITER_UNLIM:
-            if (sub.set_mode(LOITER)) {
+            if (sub.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
 
         case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-            if (sub.set_mode(RTL)) {
+            if (sub.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
 
         case MAV_CMD_NAV_LAND:
-            if (sub.set_mode(LAND)) {
+            if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
@@ -1344,7 +1357,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_MISSION_START:
-            if (sub.motors.armed() && sub.set_mode(AUTO)) {
+            if (sub.motors.armed() && sub.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
                 sub.set_auto_armed(true);
                 if (sub.mission.state() != AP_Mission::MISSION_RUNNING) {
                     sub.mission.start_or_resume();

ArduSub/Log.cpp

@@ -731,7 +731,7 @@ void Sub::Log_Write_Vehicle_Startup_Messages()
 {
     // only 200(?) bytes are guaranteed by DataFlash
     DataFlash.Log_Write_Message("Frame: " FRAME_CONFIG_STRING);
-    DataFlash.Log_Write_Mode(control_mode);
+    DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
 }
 
 

ArduSub/Sub.h

@@ -249,7 +249,11 @@ private:
 
     // This is the state of the flight control system
     // There are multiple states defined such as STABILIZE, ACRO,
-    int8_t control_mode;
+    control_mode_t control_mode;
+    mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
+
+    control_mode_t prev_control_mode;
+    mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN;
 
     // Structure used to detect changes in the flight mode control switch
     struct {
@@ -761,7 +765,7 @@ private:
     void land_nogps_run();
     float get_land_descent_speed();
     void land_do_not_use_GPS();
-    void set_mode_land_with_pause();
+    void set_mode_land_with_pause(mode_reason_t reason);
     bool landing_with_GPS();
     bool loiter_init(bool ignore_checks);
     void loiter_run();
@@ -812,24 +816,26 @@ private:
     void esc_calibration_startup_check();
     void esc_calibration_passthrough();
     void esc_calibration_auto();
+    bool should_disarm_on_failsafe();
     void failsafe_radio_on_event();
     void failsafe_radio_off_event();
     void failsafe_battery_event(void);
     void failsafe_gcs_check();
     void failsafe_gcs_off_event(void);
-    void set_mode_RTL_or_land_with_pause();
+    void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
     void update_events();
     void failsafe_enable();
     void failsafe_disable();
     void fence_check();
     void fence_send_mavlink_status(mavlink_channel_t chan);
-    bool set_mode(uint8_t mode);
+    bool set_mode(control_mode_t mode, mode_reason_t reason);
+    bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); }
     void update_flight_mode();
-    void exit_mode(uint8_t old_control_mode, uint8_t new_control_mode);
+    void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode);
-    bool mode_requires_GPS(uint8_t mode);
+    bool mode_requires_GPS(control_mode_t mode);
-    bool mode_has_manual_throttle(uint8_t mode);
+    bool mode_has_manual_throttle(control_mode_t mode);
-    bool mode_allows_arming(uint8_t mode, bool arming_from_gcs);
+    bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs);
-    void notify_flight_mode(uint8_t mode);
+    void notify_flight_mode(control_mode_t mode);
     void check_dynamic_flight(void);
     void read_inertia();
     void read_inertial_altitude();

ArduSub/commands_logic.cpp

@@ -250,7 +250,7 @@ void Sub::exit_mission()
     if(!ap.land_complete) {
         // try to enter loiter but if that fails land
         if(!auto_loiter_start()) {
-            set_mode(LAND);
+            set_mode(LAND, MODE_REASON_MISSION_END);
         }
     }else{
 #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED

ArduSub/config.h

@@ -161,11 +161,6 @@
  # define GNDEFFECT_COMPENSATION          DISABLED
 #endif
 
-// possible values for FS_GCS parameter
-#define FS_GCS_DISABLED                     0
-#define FS_GCS_ENABLED_ALWAYS_RTL           1
-#define FS_GCS_ENABLED_CONTINUE_MISSION     2
-
 // Radio failsafe while using RC_override
 #ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
  # define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS  1000    // RC Radio failsafe triggers after 1 second while using RC_override from ground station

ArduSub/control_auto.cpp

@@ -324,8 +324,8 @@ void Sub::auto_land_run()
         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            if (!set_mode(LOITER)) {
+            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                set_mode(ALT_HOLD);
+                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
             }
         }
         

ArduSub/control_flip.cpp

@@ -33,7 +33,8 @@
 #define FLIP_PITCH_FORWARD  -1      // used to set flip_dir
 
 FlipState flip_state;               // current state of flip
-uint8_t   flip_orig_control_mode;   // flight mode when flip was initated
+control_mode_t   flip_orig_control_mode;   // flight mode when flip was initated
+mode_reason_t	 flip_orig_control_mode_reason;
 uint32_t  flip_start_time;          // time since flip began
 int8_t    flip_roll_dir;            // roll direction (-1 = roll left, 1 = roll right)
 int8_t    flip_pitch_dir;           // pitch direction (-1 = pitch forward, 1 = pitch back)
@@ -63,6 +64,7 @@ bool Sub::flip_init(bool ignore_checks)
 
     // capture original flight mode so that we can return to it after completion
     flip_orig_control_mode = control_mode;
+    flip_orig_control_mode_reason = control_mode_reason;
 
     // initialise state
     flip_state = Flip_Start;
@@ -199,9 +201,9 @@ void Sub::flip_run()
         // check for successful recovery
         if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
             // restore original flight mode
-            if (!set_mode(flip_orig_control_mode)) {
+            if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) {
                 // this should never happen but just in case
-                set_mode(STABILIZE);
+                set_mode(STABILIZE, MODE_REASON_UNKNOWN);
             }
             // log successful completion
             Log_Write_Event(DATA_FLIP_END);
@@ -210,9 +212,9 @@ void Sub::flip_run()
 
     case Flip_Abandon:
         // restore original flight mode
-        if (!set_mode(flip_orig_control_mode)) {
+        if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) {
             // this should never happen but just in case
-            set_mode(STABILIZE);
+            set_mode(STABILIZE, MODE_REASON_UNKNOWN);
         }
         // log abandoning flip
         Log_Write_Error(ERROR_SUBSYSTEM_FLIP,ERROR_CODE_FLIP_ABANDONED);

ArduSub/control_land.cpp

@@ -87,8 +87,8 @@ void Sub::land_gps_run()
         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            if (!set_mode(LOITER)) {
+            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                set_mode(ALT_HOLD);
+                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
             }
         }
         
@@ -182,7 +182,7 @@ void Sub::land_nogps_run()
 #else
         // disarm when the landing detector says we've landed
         if (ap.at_surface) {
-        	set_mode(ALT_HOLD);
+        	set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
         }
 #endif
         return;
@@ -243,9 +243,9 @@ void Sub::land_do_not_use_GPS()
 
 // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
 //  this is always called from a failsafe so we trigger notification to pilot
-void Sub::set_mode_land_with_pause()
+void Sub::set_mode_land_with_pause(mode_reason_t reason)
 {
-    set_mode(LAND);
+    set_mode(LAND, reason);
     land_pause = true;
 
     // alert pilot to mode change

ArduSub/control_rtl.cpp

@@ -293,8 +293,8 @@ void Sub::rtl_descent_run()
         if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            if (!set_mode(LOITER)) {
+            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                set_mode(ALT_HOLD);
+                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
             }
         }
         

ArduSub/defines.h

@@ -94,7 +94,7 @@ enum aux_sw_func {
 #define HIL_MODE_SENSORS                1
 
 // Auto Pilot Modes enumeration
-enum autopilot_modes {
+enum control_mode_t {
     STABILIZE =     0,  // manual airframe angle with manual throttle
     ACRO =          1,  // manual body-frame angular rate with manual throttle
     ALT_HOLD =      2,  // manual airframe angle with automatic throttle
@@ -114,6 +114,20 @@ enum autopilot_modes {
 	THROW =        18   // throw to launch mode using inertial/GPS system, no pilot input
 };
 
+enum mode_reason_t {
+    MODE_REASON_UNKNOWN=0,
+    MODE_REASON_TX_COMMAND,
+    MODE_REASON_GCS_COMMAND,
+    MODE_REASON_RADIO_FAILSAFE,
+    MODE_REASON_BATTERY_FAILSAFE,
+    MODE_REASON_GCS_FAILSAFE,
+    MODE_REASON_EKF_FAILSAFE,
+    MODE_REASON_GPS_GLITCH,
+    MODE_REASON_MISSION_END,
+    MODE_REASON_THROTTLE_LAND_ESCAPE,
+    MODE_REASON_FENCE_BREACH
+};
+
 // Tuning enumeration
 enum tuning_func {
     TUNING_NONE =                        0, //
@@ -428,6 +442,11 @@ enum ThrowModeState {
 #define FS_BATT_LAND                        1       // switch to LAND mode on battery failsafe
 #define FS_BATT_RTL                         2       // switch to RTL mode on battery failsafe
 
+// GCS failsafe definitions (FS_GCS_ENABLE parameter)
+#define FS_GCS_DISABLED                     0
+#define FS_GCS_ENABLED_ALWAYS_RTL           1
+#define FS_GCS_ENABLED_CONTINUE_MISSION     2
+
 // EKF failsafe definitions (FS_EKF_ACTION parameter)
 #define FS_EKF_ACTION_LAND                  1       // switch to LAND mode on EKF failsafe
 #define FS_EKF_ACTION_ALTHOLD               2       // switch to ALTHOLD mode on EKF failsafe

ArduSub/ekf_check.cpp

@@ -141,12 +141,12 @@ void Sub::failsafe_ekf_event()
     switch (g.fs_ekf_action) {
         case FS_EKF_ACTION_ALTHOLD:
             // AltHold
-            if (failsafe.radio || !set_mode(ALT_HOLD)) {
+            if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) {
-                set_mode_land_with_pause();
+                set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
             }
             break;
         default:
-            set_mode_land_with_pause();
+            set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
             break;
     }
 

ArduSub/events.cpp

@@ -13,76 +13,20 @@ void Sub::failsafe_radio_on_event()
         return;
     }
 
-    // This is how to handle a failsafe.
+    if (should_disarm_on_failsafe()) {
-    switch(control_mode) {
-        case STABILIZE:
-        case ACRO:
-            // if throttle is zero OR vehicle is landed disarm motors
-            if (ap.throttle_zero || ap.land_complete) {
         init_disarm_motors();
-
-            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
-            }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                set_mode_land_with_pause();
-
-            // if far from home then RTL
-            } else if(home_distance > FS_CLOSE_TO_HOME_CM) {
-                // switch to RTL or if that fails, LAND
-                set_mode_RTL_or_land_with_pause();
-
-            // We have no GPS or are very close to home so we will land
     } else {
-                set_mode_land_with_pause();
+        if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
-            }
+            // continue mission
-            break;
+        } else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
-
+            // continue landing
-        case AUTO:
-            // if mission has not started AND vehicle is landed, disarm motors
-            if (!ap.auto_armed && ap.land_complete) {
-                init_disarm_motors();
-
-            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
-            } else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                set_mode_land_with_pause();
-
-            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_RTL do RTL
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
-                if (home_distance > FS_CLOSE_TO_HOME_CM) {
-                    // switch to RTL or if that fails, LAND
-                    set_mode_RTL_or_land_with_pause();
         } else {
-                    // We are very close to home so we will land
+            if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                    set_mode_land_with_pause();
+                set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
-                }
-            }
-            // failsafe_throttle must be FS_THR_ENABLED_CONTINUE_MISSION so no need to do anything
-            break;
-
-        case LAND:
-            // continue to land if battery failsafe is also active otherwise fall through to default handling
-            if (g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
-                break;
-            }
-            // no break
-        default:
-            // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
-            // if landed disarm
-            if (ap.land_complete) {
-                init_disarm_motors();
-
-            // if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
-            } else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
-                set_mode_land_with_pause();
-
-            // if far from home then RTL
-            } else if(home_distance > FS_CLOSE_TO_HOME_CM) {
-                // switch to RTL or if that fails, LAND
-                set_mode_RTL_or_land_with_pause();
             } else {
-                // We have no GPS or are very close to home so we will land
+                set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
-                set_mode_land_with_pause();
+            }
         }
-            break;
     }
 
     // log the error to the dataflash
@@ -109,50 +53,15 @@ void Sub::failsafe_battery_event(void)
 
     // failsafe check
 	if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
-        switch(control_mode) {
+		if (should_disarm_on_failsafe()) {
-            case STABILIZE:
-            case ACRO:
-                // if throttle is zero OR vehicle is landed disarm motors
-                if (ap.throttle_zero || ap.land_complete) {
 			init_disarm_motors();
 		} else {
-                    // set mode to RTL or LAND
+			if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
-                    if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
+				set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
-                        // switch to RTL or if that fails, LAND
-                        set_mode_RTL_or_land_with_pause();
 			} else {
-                        set_mode_land_with_pause();
+				set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
 			}
 		}
-                break;
-            case AUTO:
-                // if mission has not started AND vehicle is landed, disarm motors
-                if (!ap.auto_armed && ap.land_complete) {
-                    init_disarm_motors();
-
-                // set mode to RTL or LAND
-                } else if (home_distance > FS_CLOSE_TO_HOME_CM) {
-                    // switch to RTL or if that fails, LAND
-                    set_mode_RTL_or_land_with_pause();
-                } else {
-                    set_mode_land_with_pause();
-                }
-                break;
-            default:
-                // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
-                // if landed disarm
-                if (ap.land_complete) {
-                    init_disarm_motors();
-
-                // set mode to RTL or LAND
-                } else if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > FS_CLOSE_TO_HOME_CM) {
-                    // switch to RTL or if that fails, LAND
-                    set_mode_RTL_or_land_with_pause();
-                } else {
-                    set_mode_land_with_pause();
-                }
-                break;
-        }
 	}
 
     // set the low battery flag
@@ -216,18 +125,37 @@ void Sub::failsafe_gcs_off_event(void)
 
 // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
 //  this is always called from a failsafe so we trigger notification to pilot
-void Sub::set_mode_RTL_or_land_with_pause()
+void Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
 {
     // attempt to switch to RTL, if this fails then switch to Land
-    if (!set_mode(RTL)) {
+    if (!set_mode(RTL, reason)) {
         // set mode to land will trigger mode change notification to pilot
-        set_mode_land_with_pause();
+        set_mode_land_with_pause(reason);
     } else {
         // alert pilot to mode change
         AP_Notify::events.failsafe_mode_change = 1;
     }
 }
 
+bool Sub::should_disarm_on_failsafe() {
+    switch(control_mode) {
+        case STABILIZE:
+        case ACRO:
+            // if throttle is zero OR vehicle is landed disarm motors
+            return ap.throttle_zero || ap.land_complete;
+            break;
+        case AUTO:
+            // if mission has not started AND vehicle is landed, disarm motors
+            return !ap.auto_armed && ap.land_complete;
+            break;
+        default:
+            // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
+            // if landed disarm
+            return ap.land_complete;
+            break;
+    }
+}
+
 void Sub::update_events()
 {
     ServoRelayEvents.update_events();

ArduSub/fence.cpp

@@ -37,12 +37,12 @@ void Sub::fence_check()
             }else{
                 // if we are within 100m of the fence, RTL
                 if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
-                    if (!set_mode(RTL)) {
+                    if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
-                        set_mode(LAND);
+                        set_mode(LAND, MODE_REASON_FENCE_BREACH);
                     }
                 }else{
                     // if more than 100m outside the fence just force a land
-                    set_mode(LAND);
+                    set_mode(LAND, MODE_REASON_FENCE_BREACH);
                 }
             }
         }

ArduSub/flight_mode.cpp

@@ -11,7 +11,7 @@
 // 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
-bool Sub::set_mode(uint8_t mode)
+bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
 {
     // boolean to record if flight mode could be set
     bool success = false;
@@ -19,6 +19,10 @@ bool Sub::set_mode(uint8_t mode)
 
     // return immediately if we are already in the desired mode
     if (mode == control_mode) {
+    	prev_control_mode = control_mode;
+    	prev_control_mode_reason = control_mode_reason;
+
+    	control_mode_reason = reason;
         return true;
     }
 
@@ -100,8 +104,13 @@ bool Sub::set_mode(uint8_t mode)
     if (success) {
         // perform any cleanup required by previous flight mode
         exit_mode(control_mode, mode);
+
+        prev_control_mode = control_mode;
+        prev_control_mode_reason = control_mode_reason;
+
         control_mode = mode;
-        DataFlash.Log_Write_Mode(control_mode);
+        control_mode_reason = reason;
+        DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
 
 #if AC_FENCE == ENABLED
         // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
@@ -198,11 +207,13 @@ void Sub::update_flight_mode()
         case THROW:
         	throw_run();
         	break;
+        default:
+        	break;
     }
 }
 
 // exit_mode - high level call to organise cleanup as a flight mode is exited
-void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
+void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode)
 {
 #if AUTOTUNE_ENABLED == ENABLED
     if (old_control_mode == AUTOTUNE) {
@@ -235,7 +246,7 @@ void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
 }
 
 // returns true or false whether mode requires GPS
-bool Sub::mode_requires_GPS(uint8_t mode) {
+bool Sub::mode_requires_GPS(control_mode_t mode) {
     switch(mode) {
         case AUTO:
         case GUIDED:
@@ -255,7 +266,7 @@ bool Sub::mode_requires_GPS(uint8_t mode) {
 }
 
 // mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle)
-bool Sub::mode_has_manual_throttle(uint8_t mode) {
+bool Sub::mode_has_manual_throttle(control_mode_t mode) {
     switch(mode) {
         case ACRO:
         case STABILIZE:
@@ -269,7 +280,7 @@ bool Sub::mode_has_manual_throttle(uint8_t mode) {
 
 // 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
-bool Sub::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
+bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) {
 	if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && mode == GUIDED)) {
         return true;
     }
@@ -277,7 +288,7 @@ bool Sub::mode_allows_arming(uint8_t mode, bool arming_from_gcs) {
 }
 
 // notify_flight_mode - sets notify object based on flight mode.  Only used for OreoLED notify device
-void Sub::notify_flight_mode(uint8_t mode) {
+void Sub::notify_flight_mode(control_mode_t mode) {
     switch(mode) {
         case AUTO:
         case GUIDED:

ArduSub/motors.cpp

@@ -207,7 +207,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
     Log_Write_Event(DATA_ARMED);
 
     // log flight mode in case it was changed while vehicle was disarmed
-    DataFlash.Log_Write_Mode(control_mode);
+    DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
 
     // reenable failsafe
     failsafe_enable();

ArduSub/setup.cpp

@@ -359,7 +359,7 @@ void Sub::report_flight_modes()
     print_divider();
 
     for(int16_t i = 0; i < 6; i++ ) {
-        print_switch(i, flight_modes[i], BIT_IS_SET(g.simple_modes, i));
+    	print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(g.simple_modes, i));
     }
     print_blanks(2);
 }

ArduSub/switches.cpp

@@ -43,7 +43,7 @@ void Sub::read_control_switch()
 
     if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
         // set flight mode and simple mode setting
-        if (set_mode(flight_modes[switch_position])) {
+    	if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) {
             // play a tone
             if (control_switch_state.debounced_switch_position != -1) {
                 // alert user to mode change failure (except if autopilot is just starting up)
@@ -258,7 +258,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
         case AUXSW_FLIP:
             // flip if switch is on, positive throttle and we're actually flying
             if(ch_flag == AUX_SWITCH_HIGH) {
-                set_mode(FLIP);
+                set_mode(FLIP, MODE_REASON_TX_COMMAND);
             }
             break;
 
@@ -275,7 +275,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
         case AUXSW_RTL:
             if (ch_flag == AUX_SWITCH_HIGH) {
                 // engage RTL (if not possible we remain in current flight mode)
-                set_mode(RTL);
+                set_mode(RTL, MODE_REASON_TX_COMMAND);
             }else{
                 // return to flight mode switch's flight mode if we are currently in RTL
                 if (control_mode == RTL) {
@@ -424,7 +424,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
 
         case AUXSW_AUTO:
             if (ch_flag == AUX_SWITCH_HIGH) {
-                set_mode(AUTO);
+                set_mode(AUTO, MODE_REASON_TX_COMMAND);
             }else{
                 // return to flight mode switch's flight mode if we are currently in AUTO
                 if (control_mode == AUTO) {
@@ -454,7 +454,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
 
         case AUXSW_LAND:
             if (ch_flag == AUX_SWITCH_HIGH) {
-                set_mode(LAND);
+                set_mode(LAND, MODE_REASON_TX_COMMAND);
             }else{
                 // return to flight mode switch's flight mode if we are currently in LAND
                 if (control_mode == LAND) {
@@ -576,7 +576,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
         case AUXSW_BRAKE:
             // brake flight mode
             if (ch_flag == AUX_SWITCH_HIGH) {
-                set_mode(BRAKE);
+                set_mode(BRAKE, MODE_REASON_TX_COMMAND);
             }else{
                 // return to flight mode switch's flight mode if we are currently in BRAKE
                 if (control_mode == BRAKE) {
@@ -588,7 +588,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
         case AUXSW_THROW:
             // throw flight mode
             if (ch_flag == AUX_SWITCH_HIGH) {
-                set_mode(THROW);
+                set_mode(THROW, MODE_REASON_TX_COMMAND);
             } else {
                 // return to flight mode switch's flight mode if we are currently in throw mode
                 if (control_mode == THROW) {

ArduSub/takeoff.cpp

@@ -41,6 +41,8 @@ bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
                 set_auto_armed(true);
                 takeoff_timer_start(takeoff_alt_cm);
                 return true;
+            default:
+            	return false;
         }
     }
     return false;