Rover: add reason to set_mode

APMrover2/GCS_Mavlink.cpp

@@ -796,7 +796,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             switch (packet.command) {
 
             case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-                rover.set_mode(rover.mode_rtl);
+                rover.set_mode(rover.mode_rtl, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
@@ -832,7 +832,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 break;
 
             case MAV_CMD_MISSION_START:
-                rover.set_mode(rover.mode_auto);
+                rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
@@ -889,19 +889,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             switch (static_cast<uint16_t>(packet.param1)) {
             case MAV_MODE_MANUAL_ARMED:
             case MAV_MODE_MANUAL_DISARMED:
-                rover.set_mode(rover.mode_manual);
+                rover.set_mode(rover.mode_manual, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
             case MAV_MODE_AUTO_ARMED:
             case MAV_MODE_AUTO_DISARMED:
-                rover.set_mode(rover.mode_auto);
+                rover.set_mode(rover.mode_auto, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
             case MAV_MODE_STABILIZE_DISARMED:
             case MAV_MODE_STABILIZE_ARMED:
-                rover.set_mode(rover.mode_learning);
+                rover.set_mode(rover.mode_learning, MODE_REASON_GCS_COMMAND);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 

APMrover2/Log.cpp

@@ -526,7 +526,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages()
 {
     // only 200(?) bytes are guaranteed by DataFlash
     Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-    DataFlash.Log_Write_Mode(control_mode->mode_number());
+    DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
     Log_Write_Home_And_Origin();
     gps.Write_DataFlash_Log_Startup_messages();
 }

APMrover2/Rover.h

@@ -233,6 +233,7 @@ private:
     // This is the state of the flight control system
     // There are multiple states defined such as MANUAL, AUTO, ...
     Mode *control_mode;
+    mode_reason_t control_mode_reason = MODE_REASON_INITIALISED;
 
     // Used to maintain the state of the previous control switch position
     // This is set to -1 when we need to re-read the switch
@@ -467,7 +468,7 @@ private:
     void gcs_retry_deferred(void);
 
     Mode *control_mode_from_num(enum mode num);
-    bool set_mode(Mode &mode);
+    bool set_mode(Mode &mode, mode_reason_t reason);
     bool mavlink_set_mode(uint8_t mode);
 
     void do_erase_logs(void);

APMrover2/afs_rover.cpp

@@ -21,7 +21,7 @@ void AP_AdvancedFailsafe_Rover::terminate_vehicle(void)
     rover.disarm_motors();
 
     // Set to HOLD mode
-    rover.set_mode(rover.mode_hold);
+    rover.set_mode(rover.mode_hold, MODE_REASON_CRASH_FAILSAFE);
 }
 
 /*

APMrover2/commands_logic.cpp

@@ -126,7 +126,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
 void Rover::exit_mission()
 {
     gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
-    set_mode(mode_hold);
+    set_mode(mode_hold, MODE_REASON_MISSION_END);
 }
 
 // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
@@ -205,7 +205,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 
 void Rover::do_RTL(void)
 {
-    set_mode(mode_rtl);
+    set_mode(mode_rtl, MODE_REASON_MISSION_COMMAND);
 }
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)

APMrover2/control_modes.cpp

@@ -72,7 +72,7 @@ void Rover::read_control_switch()
 
         Mode *new_mode = control_mode_from_num((enum mode)modes[switchPosition].get());
         if (new_mode != nullptr) {
-            set_mode(*new_mode);
+            set_mode(*new_mode, MODE_REASON_TX_COMMAND);
         }
 
         oldSwitchPosition = switchPosition;
@@ -155,7 +155,7 @@ void Rover::read_trim_switch()
                     }
                 } else if (control_mode == &mode_auto) {
                     // if SW7 is ON in AUTO = set to RTL
-                    set_mode(mode_rtl);
+                    set_mode(mode_rtl, MODE_REASON_TX_COMMAND);
                     break;
                 }
             }

APMrover2/crash_check.cpp

@@ -38,7 +38,7 @@ void Rover::crash_check()
     // send message to gcs
     gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
     // change mode to hold and disarm
-    set_mode(mode_hold);
+    set_mode(mode_hold, MODE_REASON_CRASH_FAILSAFE);
     if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
       disarm_motors();
     }

APMrover2/defines.h

@@ -95,6 +95,8 @@ enum mode {
 #define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE      (1<<7)
 
 // Error message sub systems and error codes
+#define ERROR_SUBSYSTEM_FAILSAFE_FENCE  9
+#define ERROR_SUBSYSTEM_FLIGHT_MODE     10
 #define ERROR_SUBSYSTEM_CRASH_CHECK     12
 // subsystem specific error codes -- crash checker
 #define ERROR_CODE_CRASH_CHECK_CRASH 1
@@ -106,3 +108,13 @@ enum fs_crash_action {
 };
 
 #define DISTANCE_HOME_MAX 0.5f  // Distance max to home location before changing it when disarm
+
+enum mode_reason_t {
+    MODE_REASON_INITIALISED = 0,
+    MODE_REASON_TX_COMMAND,
+    MODE_REASON_GCS_COMMAND,
+    MODE_REASON_FAILSAFE,
+    MODE_REASON_MISSION_END,
+    MODE_REASON_CRASH_FAILSAFE,
+    MODE_REASON_MISSION_COMMAND
+};

APMrover2/failsafe.cpp

@@ -82,10 +82,10 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
             case 0:
                 break;
             case 1:
-                set_mode(mode_rtl);
+                set_mode(mode_rtl, MODE_REASON_FAILSAFE);
                 break;
             case 2:
-                set_mode(mode_hold);
+                set_mode(mode_hold, MODE_REASON_FAILSAFE);
                 break;
         }
     }

APMrover2/mode_rtl.cpp

@@ -23,6 +23,6 @@ void ModeRTL::update_navigation()
 {
     // no loitering around the wp with the rover, goes direct to the wp position
     if (rover.verify_RTL()) {
-        rover.set_mode(rover.mode_hold);
+        rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END);
     }
 }

APMrover2/system.cpp

@@ -213,7 +213,7 @@ void Rover::init_ardupilot()
     if (initial_mode == nullptr) {
         initial_mode = &mode_initializing;
     }
-    set_mode(*initial_mode);
+    set_mode(*initial_mode, MODE_REASON_INITIALISED);
 
 
     // set the correct flight mode
@@ -232,7 +232,7 @@ void Rover::init_ardupilot()
 //*********************************************************************************
 void Rover::startup_ground(void)
 {
-    set_mode(mode_initializing);
+    set_mode(mode_initializing, MODE_REASON_INITIALISED);
 
     gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
 
@@ -283,7 +283,7 @@ void Rover::set_reverse(bool reverse)
     in_reverse = reverse;
 }
 
-bool Rover::set_mode(Mode &new_mode)
+bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
 {
     if (control_mode == &new_mode) {
         // don't switch modes if we are already in the correct mode.
@@ -292,6 +292,9 @@ bool Rover::set_mode(Mode &new_mode)
 
     Mode &old_mode = *control_mode;
     if (!new_mode.enter()) {
+        // Log error that we failed to enter desired flight mode
+        Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE, new_mode.mode_number());
+        gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
         return false;
     }
 
@@ -304,7 +307,8 @@ bool Rover::set_mode(Mode &new_mode)
     old_mode.exit();
 
     if (should_log(MASK_LOG_MODE)) {
-        DataFlash.Log_Write_Mode(control_mode->mode_number());
+        control_mode_reason = reason;
+        DataFlash.Log_Write_Mode(control_mode->mode_number(), reason);
     }
 
     notify_mode((enum mode)control_mode->mode_number());
@@ -320,7 +324,7 @@ bool Rover::mavlink_set_mode(uint8_t mode)
     if (new_mode == nullptr) {
         return false;
     }
-    return set_mode(*new_mode);
+    return set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
 }
 
 void Rover::startup_INS_ground(void)