From d629d4ba9e6d9387c98c339ad0ff6f8ba3437730 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 29 Mar 2017 17:07:33 -0400 Subject: [PATCH] Sub: Change mode button function implementation --- ArduSub/Parameters.cpp | 42 --------------------- ArduSub/Parameters.h | 20 +--------- ArduSub/Sub.cpp | 1 - ArduSub/Sub.h | 3 -- ArduSub/joystick.cpp | 84 +++++++++++++----------------------------- 5 files changed, 26 insertions(+), 124 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 97aed96dc0..ed57455313 100644 --- a/ArduSub/Parameters.cpp +++ b/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 diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index e241ec77fb..df07b9b6ea 100644 --- a/ArduSub/Parameters.h +++ b/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; diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 9179c85fb7..f2d719ae23 100644 --- a/ArduSub/Sub.cpp +++ b/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 &), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index d802c0441f..c3db0f054e 100644 --- a/ArduSub/Sub.h +++ b/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; diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index bd9570bdb9..b5f03e5647 100644 --- a/ArduSub/joystick.cpp +++ b/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) { - next_mode = (control_mode_t)flight_modes[toggle_mode?1:0].get(); - next_mode_switch_pwm = toggle_mode?1300:1100; - toggle_mode = !toggle_mode; - } + + case JSButton::button_function_t::k_mode_manual: + set_mode(MANUAL, MODE_REASON_TX_COMMAND); break; - case JSButton::button_function_t::k_mode_1: - next_mode = (control_mode_t)flight_modes[0].get(); - next_mode_switch_pwm = 1100; - toggle_mode = true; + case JSButton::button_function_t::k_mode_stabilize: + set_mode(STABILIZE, MODE_REASON_TX_COMMAND); break; - case JSButton::button_function_t::k_mode_2: - next_mode = (control_mode_t)flight_modes[1].get(); - next_mode_switch_pwm = 1300; - toggle_mode = false; + case JSButton::button_function_t::k_mode_depth_hold: + set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND); break; - case JSButton::button_function_t::k_mode_3: - next_mode = (control_mode_t)flight_modes[2].get(); - next_mode_switch_pwm = 1420; - toggle_mode = false; + case JSButton::button_function_t::k_mode_auto: + set_mode(AUTO, MODE_REASON_TX_COMMAND); break; - case JSButton::button_function_t::k_mode_4: - next_mode = (control_mode_t)flight_modes[3].get(); - next_mode_switch_pwm = 1550; - toggle_mode = false; + case JSButton::button_function_t::k_mode_guided: + set_mode(GUIDED, MODE_REASON_TX_COMMAND); break; - case JSButton::button_function_t::k_mode_5: - next_mode = (control_mode_t)flight_modes[4].get(); - next_mode_switch_pwm = 1690; - toggle_mode = false; + case JSButton::button_function_t::k_mode_circle: + set_mode(CIRCLE, MODE_REASON_TX_COMMAND); break; - case JSButton::button_function_t::k_mode_6: - next_mode = (control_mode_t)flight_modes[5].get(); - next_mode_switch_pwm = 1900; - toggle_mode = false; + case JSButton::button_function_t::k_mode_acro: + set_mode(ACRO, MODE_REASON_TX_COMMAND); 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_1, 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_2, 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_stabilize, 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_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},