Plane: add reason to set_mode()

ArduPlane/ArduPlane.cpp

@@ -510,7 +510,7 @@ void Plane::handle_auto_mode(void)
 
     if (mission.state() != AP_Mission::MISSION_RUNNING) {
         // this should never be reached
-        set_mode(RTL);
+        set_mode(RTL, MODE_REASON_MISSION_END);
         GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
         return;
     }
@@ -779,7 +779,7 @@ void Plane::update_navigation()
     case RTL:
         if (quadplane.available() && quadplane.rtl_mode == 1 &&
             nav_controller->reached_loiter_target()) {
-            set_mode(QRTL);
+            set_mode(QRTL, MODE_REASON_UNKNOWN);
             break;
         } else if (g.rtl_autoland == 1 &&
             !auto_state.checked_for_autoland &&

ArduPlane/GCS_Mavlink.cpp

@@ -1199,7 +1199,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             // location is valid load and set
             if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
                 (plane.control_mode == GUIDED)) {
-                plane.set_mode(GUIDED);
+                plane.set_mode(GUIDED, MODE_REASON_GCS_COMMAND);
                 plane.guided_WP_loc = requested_position;
 
                 // add home alt if needed
@@ -1267,12 +1267,12 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_NAV_LOITER_UNLIM:
-            plane.set_mode(LOITER);
+            plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND);
             result = MAV_RESULT_ACCEPTED;
             break;
 
         case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-            plane.set_mode(RTL);
+            plane.set_mode(RTL, MODE_REASON_GCS_COMMAND);
             result = MAV_RESULT_ACCEPTED;
             break;
 
@@ -1334,7 +1334,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_MISSION_START:
-            plane.set_mode(AUTO);
+            plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND);
             result = MAV_RESULT_ACCEPTED;
             break;
 
@@ -1476,19 +1476,19 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             switch ((uint16_t)packet.param1) {
             case MAV_MODE_MANUAL_ARMED:
             case MAV_MODE_MANUAL_DISARMED:
-                plane.set_mode(MANUAL);
+                plane.set_mode(MANUAL, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
             case MAV_MODE_AUTO_ARMED:
             case MAV_MODE_AUTO_DISARMED:
-                plane.set_mode(AUTO);
+                plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
             case MAV_MODE_STABILIZE_DISARMED:
             case MAV_MODE_STABILIZE_ARMED:
-                plane.set_mode(FLY_BY_WIRE_A);
+                plane.set_mode(FLY_BY_WIRE_A, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 

ArduPlane/Plane.h

@@ -317,7 +317,9 @@ private:
     // This is the state of the flight control system
     // There are multiple states defined such as MANUAL, FBW-A, AUTO
     enum FlightMode control_mode = INITIALISING;
+    mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
     enum FlightMode previous_mode = INITIALISING;
+    mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN;
 
     // Used to maintain the state of the previous control switch position
     // This is set to 254 when we need to re-read the switch
@@ -915,9 +917,9 @@ private:
     void autotune_restore(void);
     void autotune_enable(bool enable);
     bool fly_inverted(void);
-    void failsafe_short_on_event(enum failsafe_state fstype);
-    void failsafe_long_on_event(enum failsafe_state fstype);
-    void failsafe_short_off_event();
+    void failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason);
+    void failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason);
+    void failsafe_short_off_event(mode_reason_t reason);
     void low_battery_event(void);
     void update_events(void);
     uint8_t max_fencepoints(void);
@@ -989,7 +991,7 @@ private:
     void init_ardupilot();
     void startup_ground(void);
     enum FlightMode get_previous_mode();
-    void set_mode(enum FlightMode mode);
+    void set_mode(enum FlightMode mode, mode_reason_t reason);
     bool mavlink_set_mode(uint8_t mode);
     void exit_mode(enum FlightMode mode);
     void check_long_failsafe();

ArduPlane/commands_logic.cpp

@@ -81,7 +81,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-        set_mode(RTL);
+        set_mode(RTL, MODE_REASON_UNKNOWN);
         break;
 
     case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
@@ -989,7 +989,7 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 void Plane::exit_mission_callback()
 {
     if (control_mode == AUTO) {
-        set_mode(RTL);
+        set_mode(RTL, MODE_REASON_MISSION_END);
         gcs_send_text_fmt(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
     }
 }

ArduPlane/control_modes.cpp

@@ -41,7 +41,7 @@ void Plane::read_control_switch()
             return;
         }
 
-        set_mode((enum FlightMode)(flight_modes[switchPosition].get()));
+        set_mode((enum FlightMode)(flight_modes[switchPosition].get()), MODE_REASON_TX_COMMAND);
 
         oldSwitchPosition = switchPosition;
     }

ArduPlane/defines.h

@@ -68,6 +68,21 @@ enum FlightMode {
     QRTL          = 21
 };
 
+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_FENCE_BREACH,
+    MODE_REASON_AVOIDANCE,
+    MODE_REASON_AVOIDANCE_RECOVERY,
+};
+
 // type of stick mixing enabled
 enum StickMixing {
     STICK_MIXING_DISABLED = 0,

ArduPlane/events.cpp

@@ -2,7 +2,7 @@
 
 #include "Plane.h"
 
-void Plane::failsafe_short_on_event(enum failsafe_state fstype)
+void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason)
 {
     // This is how to handle a short loss of control signal failsafe.
     failsafe.state = fstype;
@@ -21,9 +21,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
         failsafe.saved_mode = control_mode;
         failsafe.saved_mode_set = 1;
         if(g.short_fs_action == 2) {
-            set_mode(FLY_BY_WIRE_A);
+            set_mode(FLY_BY_WIRE_A, reason);
         } else {
-            set_mode(CIRCLE);
+            set_mode(CIRCLE, reason);
         }
         break;
 
@@ -32,7 +32,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
     case QHOVER:
         failsafe.saved_mode = control_mode;
         failsafe.saved_mode_set = 1;
-        set_mode(QLAND);
+        set_mode(QLAND, reason);
         break;
         
     case AUTO:
@@ -43,9 +43,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
             failsafe.saved_mode = control_mode;
             failsafe.saved_mode_set = 1;
             if(g.short_fs_action == 2) {
-                set_mode(FLY_BY_WIRE_A);
+                set_mode(FLY_BY_WIRE_A, reason);
             } else {
-                set_mode(CIRCLE);
+                set_mode(CIRCLE, reason);
             }
         }
         break;
@@ -60,7 +60,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
     gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
 }
 
-void Plane::failsafe_long_on_event(enum failsafe_state fstype)
+void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason)
 {
     // This is how to handle a long loss of control signal failsafe.
     gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, ");
@@ -83,16 +83,16 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
             parachute_release();
 #endif
         } else if (g.long_fs_action == 2) {
-            set_mode(FLY_BY_WIRE_A);
+            set_mode(FLY_BY_WIRE_A, reason);
         } else {
-            set_mode(RTL);
+            set_mode(RTL, reason);
         }
         break;
 
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
-        set_mode(QLAND);
+        set_mode(QLAND, reason);
         break;
         
     case AUTO:
@@ -104,9 +104,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
             parachute_release();
 #endif
         } else if (g.long_fs_action == 2) {
-            set_mode(FLY_BY_WIRE_A);
+            set_mode(FLY_BY_WIRE_A, reason);
         } else if (g.long_fs_action == 1) {
-            set_mode(RTL);
+            set_mode(RTL, reason);
         }
         break;
 
@@ -122,7 +122,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
     gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
 }
 
-void Plane::failsafe_short_off_event()
+void Plane::failsafe_short_off_event(mode_reason_t reason)
 {
     // We're back in radio contact
     gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off");
@@ -132,7 +132,7 @@ void Plane::failsafe_short_off_event()
     // --------------------------------------------------------
     if (control_mode == CIRCLE && failsafe.saved_mode_set) {
         failsafe.saved_mode_set = 0;
-        set_mode(failsafe.saved_mode);
+        set_mode(failsafe.saved_mode, reason);
     }
 }
 
@@ -146,7 +146,7 @@ void Plane::low_battery_event(void)
     if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
         flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
         flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
-    	set_mode(RTL);
+    	set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
     	aparm.throttle_cruise.load();
     }
     failsafe.low_battery = true;

ArduPlane/geofence.cpp

@@ -292,7 +292,7 @@ void Plane::geofence_check(bool altitude_check_only)
             guided_WP_loc.lat == geofence_state->guided_lat &&
             guided_WP_loc.lng == geofence_state->guided_lng) {
             geofence_state->old_switch_position = 254;
-            set_mode(get_previous_mode());
+            set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND);
         }
         return;
     }
@@ -311,7 +311,7 @@ void Plane::geofence_check(bool altitude_check_only)
     struct Location loc;
 
     // Never trigger a fence breach in the final stage of landing
-    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
+    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) {
         return;
     }
 
@@ -380,9 +380,9 @@ void Plane::geofence_check(bool altitude_check_only)
         int8_t saved_auto_trim = g.auto_trim;
         g.auto_trim.set(0);
         if (g.fence_action == FENCE_ACTION_RTL) {
-            set_mode(RTL);
+            set_mode(RTL, MODE_REASON_FENCE_BREACH);
         } else {
-            set_mode(GUIDED);
+            set_mode(GUIDED, MODE_REASON_FENCE_BREACH);
         }
         g.auto_trim.set(saved_auto_trim);
 

ArduPlane/landing.cpp

@@ -33,7 +33,7 @@ bool Plane::verify_land()
             mission.resume();
             if (!success) {
                 // on a restart failure lets RTL or else the plane may fly away with nowhere to go!
-                set_mode(RTL);
+                set_mode(RTL, MODE_REASON_MISSION_END);
             }
             // make sure to return false so it leaves the mission index alone
         }
@@ -368,7 +368,9 @@ bool Plane::jump_to_landing_sequence(void)
     uint16_t land_idx = mission.get_landing_sequence_start();
     if (land_idx != 0) {
         if (mission.set_current_cmd(land_idx)) {
-            set_mode(AUTO);
+
+            // in case we're in RTL
+            set_mode(AUTO, MODE_REASON_UNKNOWN);
 
             //if the mission has ended it has to be restarted
             if (mission.state() == AP_Mission::MISSION_STOPPED) {

ArduPlane/system.cpp

@@ -245,7 +245,7 @@ void Plane::init_ardupilot()
     // choose the nav controller
     set_nav_controller();
 
-    set_mode((FlightMode)g.initial_mode.get());
+    set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN);
 
     // set the correct flight mode
     // ---------------------------
@@ -265,7 +265,7 @@ void Plane::init_ardupilot()
 //********************************************************************************
 void Plane::startup_ground(void)
 {
-    set_mode(INITIALISING);
+    set_mode(INITIALISING, MODE_REASON_UNKNOWN);
 
     gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> Ground start");
 
@@ -311,7 +311,7 @@ enum FlightMode Plane::get_previous_mode() {
     return previous_mode; 
 }
 
-void Plane::set_mode(enum FlightMode mode)
+void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
 {
     if(control_mode == mode) {
         // don't switch modes if we are already in the correct mode.
@@ -352,6 +352,8 @@ void Plane::set_mode(enum FlightMode mode)
     // set mode
     previous_mode = control_mode;
     control_mode = mode;
+    previous_mode_reason = control_mode_reason;
+    control_mode_reason = reason;
 
     if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
         // restore last gains
@@ -518,7 +520,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
     case QLOITER:
     case QLAND:
     case QRTL:
-        set_mode((enum FlightMode)mode);
+        set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
         return true;
     }
     return false;
@@ -552,19 +554,19 @@ void Plane::check_long_failsafe()
             flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
         if (failsafe.state == FAILSAFE_SHORT &&
                    (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
-            failsafe_long_on_event(FAILSAFE_LONG);
+            failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
         } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO &&
                    failsafe.last_heartbeat_ms != 0 &&
                    (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
-            failsafe_long_on_event(FAILSAFE_GCS);
+            failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
         } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT &&
                    failsafe.last_heartbeat_ms != 0 &&
                    (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
-            failsafe_long_on_event(FAILSAFE_GCS);
+            failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
         } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI && 
                    gcs[0].last_radio_status_remrssi_ms != 0 &&
                    (tnow - gcs[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
-            failsafe_long_on_event(FAILSAFE_GCS);
+            failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
         }
     } else {
         // We do not change state but allow for user to change mode
@@ -588,13 +590,13 @@ void Plane::check_short_failsafe()
             flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
         // The condition is checked and the flag ch3_failsafe is set in radio.cpp
         if(failsafe.ch3_failsafe) {
-            failsafe_short_on_event(FAILSAFE_SHORT);
+            failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);
         }
     }
 
     if(failsafe.state == FAILSAFE_SHORT) {
         if(!failsafe.ch3_failsafe) {
-            failsafe_short_off_event();
+            failsafe_short_off_event(MODE_REASON_RADIO_FAILSAFE);
         }
     }
 }