Sub: Support new AP_Vehicle::set_mode

ArduSub/GCS_Mavlink.cpp

@@ -458,13 +458,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
 {
     switch (packet.command) {
     case MAV_CMD_NAV_LOITER_UNLIM:
-        if (!sub.set_mode(POSHOLD, MODE_REASON_GCS_COMMAND)) {
+        if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_FAILED;
         }
         return MAV_RESULT_ACCEPTED;
 
     case MAV_CMD_NAV_LAND:
-        if (!sub.set_mode(SURFACE, MODE_REASON_GCS_COMMAND)) {
+        if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_FAILED;
         }
         return MAV_RESULT_ACCEPTED;
@@ -494,7 +494,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
         return MAV_RESULT_FAILED;
 
     case MAV_CMD_MISSION_START:
-        if (sub.motors.armed() && sub.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
+        if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_ACCEPTED;
         }
         return MAV_RESULT_FAILED;
@@ -825,11 +825,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long
     return MAV_RESULT_FAILED;
 }
 
-bool GCS_MAVLINK_Sub::set_mode(uint8_t mode)
-{
-    return sub.set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND);
-}
-
 int32_t GCS_MAVLINK_Sub::global_position_int_alt() const {
     if (!sub.ap.depth_sensor_present) {
         return 0;

ArduSub/GCS_Mavlink.h

@@ -18,7 +18,6 @@ protected:
 
     uint8_t sysid_my_gcs() const override;
 
-    bool set_mode(uint8_t mode) override;
     bool should_zero_rc_outputs_on_reboot() const override { return true; }
 
     MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;

ArduSub/Sub.h

@@ -230,10 +230,10 @@ private:
     // This is the state of the flight control system
     // There are multiple states defined such as STABILIZE, ACRO,
     control_mode_t control_mode;
-    mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
+    ModeReason control_mode_reason = ModeReason::UNKNOWN;
 
     control_mode_t prev_control_mode;
-    mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN;
+    ModeReason prev_control_mode_reason = ModeReason::UNKNOWN;
 
 #if RCMAP_ENABLED == ENABLED
     RCMapper rcmap;
@@ -549,7 +549,8 @@ private:
     void mainloop_failsafe_enable();
     void mainloop_failsafe_disable();
     void fence_check();
-    bool set_mode(control_mode_t mode, mode_reason_t reason);
+    bool set_mode(control_mode_t mode, ModeReason reason);
+    bool set_mode(const uint8_t mode, const ModeReason reason) override;
     void update_flight_mode();
     void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode);
     bool mode_requires_GPS(control_mode_t mode);

ArduSub/commands_logic.cpp

@@ -216,7 +216,7 @@ void Sub::exit_mission()
 
     // Try to enter loiter, if that fails, go to depth hold
     if (!auto_loiter_start()) {
-        set_mode(ALT_HOLD, MODE_REASON_MISSION_END);
+        set_mode(ALT_HOLD, ModeReason::MISSION_END);
     }
 }
 

ArduSub/control_surface.cpp

@@ -35,7 +35,7 @@ void Sub::surface_run()
 
     // Already at surface, hold depth at surface
     if (ap.at_surface) {
-        set_mode(ALT_HOLD, MODE_REASON_SURFACE_COMPLETE);
+        set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE);
     }
 
     // convert pilot input to lean angles

ArduSub/defines.h

@@ -30,7 +30,7 @@ enum autopilot_yaw_mode {
 };
 
 // Auto Pilot Modes enumeration
-enum control_mode_t {
+enum control_mode_t : uint8_t {
     STABILIZE =     0,  // manual angle with manual depth/throttle
     ACRO =          1,  // manual body-frame angular rate with manual depth/throttle
     ALT_HOLD =      2,  // manual angle with automatic depth/throttle
@@ -43,24 +43,6 @@ enum control_mode_t {
     MOTOR_DETECT = 20   // Automatically detect motors orientation
 };
 
-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_SURFACE_ESCAPE,
-    MODE_REASON_FENCE_BREACH,
-    MODE_REASON_TERRAIN_FAILSAFE,
-    MODE_REASON_SURFACE_COMPLETE,
-    MODE_REASON_LEAK_FAILSAFE,
-    MODE_REASON_BAD_DEPTH
-};
-
 // Acro Trainer types
 #define ACRO_TRAINER_DISABLED   0
 #define ACRO_TRAINER_LEVELING   1

ArduSub/failsafe.cpp

@@ -90,7 +90,7 @@ void Sub::failsafe_sensors_check()
 
     if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
         // This should always succeed
-        if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
+        if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) {
             // We should never get here
             arming.disarm();
         }
@@ -157,7 +157,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
 
     switch((Failsafe_Action)action) {
         case Failsafe_Action_Surface:
-            set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
+            set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE);
             break;
         case Failsafe_Action_Disarm:
             arming.disarm();
@@ -300,7 +300,7 @@ void Sub::failsafe_leak_check()
 
     // Handle failsafe action
     if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
-        set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE);
+        set_mode(SURFACE, ModeReason::LEAK_FAILSAFE);
     }
 }
 
@@ -347,11 +347,11 @@ void Sub::failsafe_gcs_check()
     if (g.failsafe_gcs == FS_GCS_DISARM) {
         arming.disarm();
     } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
-        if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
+        if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) {
             arming.disarm();
         }
     } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
-        if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
+        if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) {
             arming.disarm();
         }
     }
@@ -477,14 +477,14 @@ void Sub::failsafe_terrain_act()
 {
     switch (g.failsafe_terrain) {
     case FS_TERRAIN_HOLD:
-        if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) {
-            set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE);
+        if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) {
+            set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE);
         }
         AP_Notify::events.failsafe_mode_change = 1;
         break;
 
     case FS_TERRAIN_SURFACE:
-        set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE);
+        set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE);
         AP_Notify::events.failsafe_mode_change = 1;
         break;
 

ArduSub/flight_mode.cpp

@@ -2,7 +2,7 @@
 
 // change flight mode and perform any necessary initialisation
 // returns true if mode was successfully set
-bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
+bool Sub::set_mode(control_mode_t mode, ModeReason reason)
 {
     // boolean to record if flight mode could be set
     bool success = false;
@@ -99,6 +99,12 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
     return success;
 }
 
+bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason)
+{
+    static_assert(sizeof(control_mode_t) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
+    return sub.set_mode((control_mode_t)new_mode, reason);
+}
+
 // update_flight_mode - calls the appropriate attitude controllers based on flight mode
 // called at 100hz or more
 void Sub::update_flight_mode()

ArduSub/joystick.cpp

@@ -35,7 +35,7 @@ void Sub::init_joystick()
     lights1 = RC_Channels::rc_channel(8)->get_radio_min();
     lights2 = RC_Channels::rc_channel(9)->get_radio_min();
 
-    set_mode(MANUAL, MODE_REASON_TX_COMMAND); // Initialize flight mode
+    set_mode(MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode
 
     if (g.numGainSettings < 1) {
         g.numGainSettings.set_and_save(1);
@@ -158,28 +158,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
         break;
 
     case JSButton::button_function_t::k_mode_manual:
-        set_mode(MANUAL, MODE_REASON_TX_COMMAND);
+        set_mode(MANUAL, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_stabilize:
-        set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
+        set_mode(STABILIZE, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_depth_hold:
-        set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND);
+        set_mode(ALT_HOLD, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_auto:
-        set_mode(AUTO, MODE_REASON_TX_COMMAND);
+        set_mode(AUTO, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_guided:
-        set_mode(GUIDED, MODE_REASON_TX_COMMAND);
+        set_mode(GUIDED, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_circle:
-        set_mode(CIRCLE, MODE_REASON_TX_COMMAND);
+        set_mode(CIRCLE, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_acro:
-        set_mode(ACRO, MODE_REASON_TX_COMMAND);
+        set_mode(ACRO, ModeReason::RC_COMMAND);
         break;
     case JSButton::button_function_t::k_mode_poshold:
-        set_mode(POSHOLD, MODE_REASON_TX_COMMAND);
+        set_mode(POSHOLD, ModeReason::RC_COMMAND);
         break;
 
     case JSButton::button_function_t::k_mount_center: