Sub: Change mode button function implementation

ArduSub/Parameters.cpp

@@ -213,48 +213,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @Increment: 1
     GSCALAR(throttle_deadzone,  "THR_DZ",    THR_DZ_DEFAULT),
 
-    // @Param: FLTMODE1
-    // @DisplayName: Flight Mode 1
-    // @Description: Flight mode when Channel 5 pwm is <= 1230
-    // @Values: 0:Stabilize,2:DepthHold,19:Manual
-    // @User: Standard
-    GSCALAR(flight_mode1, "FLTMODE1",               FLIGHT_MODE_1),
-
-    // @Param: FLTMODE2
-    // @DisplayName: Flight Mode 2
-    // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
-    // @Values: 0:Stabilize,2:DepthHold,19:Manual
-    // @User: Standard
-    GSCALAR(flight_mode2, "FLTMODE2",               FLIGHT_MODE_2),
-
-    // @Param: FLTMODE3
-    // @DisplayName: Flight Mode 3
-    // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
-    // @Values: 0:Stabilize,2:DepthHold,19:Manual
-    // @User: Standard
-    GSCALAR(flight_mode3, "FLTMODE3",               FLIGHT_MODE_3),
-
-    // @Param: FLTMODE4
-    // @DisplayName: Flight Mode 4
-    // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
-    // @Values: 0:Stabilize,2:DepthHold,19:Manual
-    // @User: Standard
-    GSCALAR(flight_mode4, "FLTMODE4",               FLIGHT_MODE_4),
-
-    // @Param: FLTMODE5
-    // @DisplayName: Flight Mode 5
-    // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
-    // @Values: 0:Stabilize,2:DepthHold,19:Manual
-    // @User: Standard
-    GSCALAR(flight_mode5, "FLTMODE5",               FLIGHT_MODE_5),
-
-    // @Param: FLTMODE6
-    // @DisplayName: Flight Mode 6
-    // @Description: Flight mode when Channel 5 pwm is >=1750
-    // @Values: 0:Stabilize,2:DepthHold,19:Manual
-    // @User: Standard
-    GSCALAR(flight_mode6, "FLTMODE6",               FLIGHT_MODE_6),
-
     // @Param: LOG_BITMASK
     // @DisplayName: Log bitmask
     // @Description: 4 byte bitmap of log types to enable

ArduSub/Parameters.h

@@ -148,17 +148,8 @@ public:
         k_param_jbtn_15,
 
 
-        // Flight mode selection
-        k_param_flight_mode1 = 120,
-        k_param_flight_mode2,
-        k_param_flight_mode3,
-        k_param_flight_mode4,
-        k_param_flight_mode5,
-        k_param_flight_mode6,
-
-
         // PID Controllers
-        k_param_p_pos_xy,
+        k_param_p_pos_xy = 126,
         k_param_p_alt_hold,
         k_param_pi_vel_xy,
         k_param_p_vel_z,
@@ -259,15 +250,6 @@ public:
     //
     AP_Int16        throttle_deadzone;
 
-    // Flight modes
-    //
-    AP_Int8         flight_mode1;
-    AP_Int8         flight_mode2;
-    AP_Int8         flight_mode3;
-    AP_Int8         flight_mode4;
-    AP_Int8         flight_mode5;
-    AP_Int8         flight_mode6;
-
     // Misc
     //
     AP_Int32        log_bitmask;

ArduSub/Sub.cpp

@@ -22,7 +22,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
  */
 Sub::Sub(void) :
     DataFlash {FIRMWARE_STRING},
-          flight_modes(&g.flight_mode1),
           mission(ahrs,
                   FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
                   FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),

ArduSub/Sub.h

@@ -169,9 +169,6 @@ private:
 
     AP_LeakDetector leak_detector;
 
-    // flight modes convenience array
-    AP_Int8 *flight_modes;
-
     TSYS01 celsius;
     AP_Baro barometer;
     Compass compass;

ArduSub/joystick.cpp

@@ -5,7 +5,6 @@
 
 // Anonymous namespace to hold variables used only in this file
 namespace {
-int16_t mode_switch_pwm = 1100;
 float cam_tilt = 1500.0;
 int16_t lights1 = 1100;
 int16_t lights2 = 1100;
@@ -18,7 +17,6 @@ int16_t video_switch = 1100;
 int16_t x_last, y_last, z_last;
 uint16_t buttons_prev;
 float gain;
-bool toggle_mode = true;
 
 // Servo control output channels
 // TODO: Allow selecting output channels
@@ -33,7 +31,7 @@ void Sub::init_joystick()
 {
     default_js_buttons();
 
-    set_mode((control_mode_t)flight_modes[0].get(), MODE_REASON_TX_COMMAND); // Initialize flight mode
+    set_mode(MANUAL, MODE_REASON_TX_COMMAND); // Initialize flight mode
 
     if (g.numGainSettings < 1) {
         g.numGainSettings.set_and_save(1);
@@ -97,7 +95,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
 
     channels[2] = constrain_int16((z+zTrim)*throttleScale+throttleBase,1100,1900); // throttle
     channels[3] = constrain_int16(r*rpyScale+rpyCenter,1100,1900);                 // yaw
-    channels[4] = mode_switch_pwm;                                                 // for testing only
+    //channels[4] = mode_switch_pwm;                                                 // for testing only
 
     if (!roll_pitch_flag) {
         // adjust forward and lateral with joystick input instead of roll and pitch
@@ -125,10 +123,6 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
 
 void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 {
-    // For attempts to change control mode
-    control_mode_t next_mode = control_mode;
-    uint16_t next_mode_switch_pwm = mode_switch_pwm;
-
     // Act based on the function assigned to this button
     switch (get_button(button)->function(shift)) {
     case JSButton::button_function_t::k_arm_toggle:
@@ -144,43 +138,32 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
     case JSButton::button_function_t::k_disarm:
         init_disarm_motors();
         break;
-    case JSButton::button_function_t::k_mode_toggle:
+
-        if (!held) {
+    case JSButton::button_function_t::k_mode_manual:
-            next_mode = (control_mode_t)flight_modes[toggle_mode?1:0].get();
+        set_mode(MANUAL, MODE_REASON_TX_COMMAND);
-            next_mode_switch_pwm = toggle_mode?1300:1100;
-            toggle_mode = !toggle_mode;
-        }
         break;
-    case JSButton::button_function_t::k_mode_1:
+    case JSButton::button_function_t::k_mode_stabilize:
-        next_mode = (control_mode_t)flight_modes[0].get();
+        set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
-        next_mode_switch_pwm = 1100;
-        toggle_mode = true;
         break;
-    case JSButton::button_function_t::k_mode_2:
+    case JSButton::button_function_t::k_mode_depth_hold:
-        next_mode = (control_mode_t)flight_modes[1].get();
+        set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND);
-        next_mode_switch_pwm = 1300;
-        toggle_mode = false;
         break;
-    case JSButton::button_function_t::k_mode_3:
+    case JSButton::button_function_t::k_mode_auto:
-        next_mode = (control_mode_t)flight_modes[2].get();
+        set_mode(AUTO, MODE_REASON_TX_COMMAND);
-        next_mode_switch_pwm = 1420;
-        toggle_mode = false;
         break;
-    case JSButton::button_function_t::k_mode_4:
+    case JSButton::button_function_t::k_mode_guided:
-        next_mode = (control_mode_t)flight_modes[3].get();
+        set_mode(GUIDED, MODE_REASON_TX_COMMAND);
-        next_mode_switch_pwm = 1550;
-        toggle_mode = false;
         break;
-    case JSButton::button_function_t::k_mode_5:
+    case JSButton::button_function_t::k_mode_circle:
-        next_mode = (control_mode_t)flight_modes[4].get();
+        set_mode(CIRCLE, MODE_REASON_TX_COMMAND);
-        next_mode_switch_pwm = 1690;
-        toggle_mode = false;
         break;
-    case JSButton::button_function_t::k_mode_6:
+    case JSButton::button_function_t::k_mode_acro:
-        next_mode = (control_mode_t)flight_modes[5].get();
+        set_mode(ACRO, MODE_REASON_TX_COMMAND);
-        next_mode_switch_pwm = 1900;
-        toggle_mode = false;
         break;
+    case JSButton::button_function_t::k_mode_poshold:
+        set_mode(POSHOLD, MODE_REASON_TX_COMMAND);
+        break;
+
     case JSButton::button_function_t::k_mount_center:
         cam_tilt = g.cam_tilt_center;
         break;
@@ -501,23 +484,6 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         // Not implemented
         break;
     }
-
-    // Update the control mode if needed
-    if (control_mode != next_mode) {
-        if (set_mode(next_mode, MODE_REASON_TX_COMMAND)) {
-            // Notify user
-            if (ap.initialised) {
-                AP_Notify::events.user_mode_change = 1;
-            }
-            // Update CH5 pwm value (For GCS)
-            mode_switch_pwm = next_mode_switch_pwm;
-        } else {
-            // Notify user
-            if (ap.initialised) {
-                AP_Notify::events.user_mode_change_failed = 1;
-            }
-        }
-    }
 }
 
 JSButton* Sub::get_button(uint8_t index)
@@ -564,10 +530,10 @@ JSButton* Sub::get_button(uint8_t index)
 void Sub::default_js_buttons()
 {
     JSButton::button_function_t defaults[16][2] = {
-        {JSButton::button_function_t::k_none,                   JSButton::button_function_t::k_none},
+        {JSButton::button_function_t::k_mode_manual,                   JSButton::button_function_t::k_none},
-        {JSButton::button_function_t::k_mode_1,                 JSButton::button_function_t::k_none},
+        {JSButton::button_function_t::k_mode_stabilize,                 JSButton::button_function_t::k_none},
-        {JSButton::button_function_t::k_mode_3,                 JSButton::button_function_t::k_none},
+        {JSButton::button_function_t::k_mode_depth_hold,                 JSButton::button_function_t::k_none},
-        {JSButton::button_function_t::k_mode_2,                 JSButton::button_function_t::k_none},
+        {JSButton::button_function_t::k_none,                 JSButton::button_function_t::k_none},
 
         {JSButton::button_function_t::k_disarm,                 JSButton::button_function_t::k_none},
         {JSButton::button_function_t::k_shift,                  JSButton::button_function_t::k_none},