From c9931fa6804fe43b26b4eb69b49d58f01ab000dd Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 30 Oct 2023 15:02:07 -0300 Subject: [PATCH 1/2] Sub: add support for 32 buttons and two additonal axis --- ArduSub/GCS_Mavlink.cpp | 18 +++++++++++- ArduSub/Parameters.cpp | 64 +++++++++++++++++++++++++++++++++++++++++ ArduSub/Parameters.h | 34 ++++++++++++++++++++++ ArduSub/Sub.h | 10 ++++++- ArduSub/joystick.cpp | 61 +++++++++++++++++++++++++++++++++------ 5 files changed, 176 insertions(+), 11 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 78a3e288b9466..aa4c39b3268a7 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -583,7 +583,23 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) break; // only accept control aimed at us } - sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); + sub.transform_manual_control_to_rc_override( + packet.x, + packet.y, + packet.z, + packet.r, + packet.buttons, + packet.buttons2, + packet.enabled_extensions, + packet.s, + packet.t, + packet.aux1, + packet.aux2, + packet.aux3, + packet.aux4, + packet.aux5, + packet.aux6 + ); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); // a RC override message is considered to be a 'heartbeat' diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 83ce1933c3028..d43b149820f74 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -350,6 +350,70 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp GGROUP(jbtn_15, "BTN15_", JSButton), + // @Group: BTN16_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_16, "BTN16_", JSButton), + + // @Group: BTN17_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_17, "BTN17_", JSButton), + + // @Group: BTN18_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_18, "BTN18_", JSButton), + + // @Group: BTN19_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_19, "BTN19_", JSButton), + + // @Group: BTN20_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_20, "BTN20_", JSButton), + + // @Group: BTN21_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_21, "BTN21_", JSButton), + + // @Group: BTN22_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_22, "BTN22_", JSButton), + + // @Group: BTN23_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_23, "BTN23_", JSButton), + + // @Group: BTN24_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_24, "BTN24_", JSButton), + + // @Group: BTN25_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_25, "BTN25_", JSButton), + + // @Group: BTN26_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_26, "BTN26_", JSButton), + + // @Group: BTN27_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_27, "BTN27_", JSButton), + + // @Group: BTN28_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_28, "BTN28_", JSButton), + + // @Group: BTN29_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_29, "BTN29_", JSButton), + + // @Group: BTN30_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_30, "BTN30_", JSButton), + + // @Group: BTN31_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_31, "BTN31_", JSButton), + // @Param: RC_SPEED // @DisplayName: ESC Update Speed // @Description: This is the speed in Hertz that your ESCs will receive updates diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 433d002180a98..95ca7923ab86f 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -149,6 +149,23 @@ class Parameters { k_param_jbtn_14, k_param_jbtn_15, + // 16 more for MANUAL_CONTROL extensions + k_param_jbtn_16, + k_param_jbtn_17, + k_param_jbtn_18, + k_param_jbtn_19, + k_param_jbtn_20, + k_param_jbtn_21, + k_param_jbtn_22, + k_param_jbtn_23, + k_param_jbtn_24, + k_param_jbtn_25, + k_param_jbtn_26, + k_param_jbtn_27, + k_param_jbtn_28, + k_param_jbtn_29, + k_param_jbtn_30, + k_param_jbtn_31, // PID Controllers k_param_p_pos_xy = 126, // deprecated @@ -295,6 +312,23 @@ class Parameters { JSButton jbtn_13; JSButton jbtn_14; JSButton jbtn_15; + // 16 - 31 from manual_control extension + JSButton jbtn_16; + JSButton jbtn_17; + JSButton jbtn_18; + JSButton jbtn_19; + JSButton jbtn_20; + JSButton jbtn_21; + JSButton jbtn_22; + JSButton jbtn_23; + JSButton jbtn_24; + JSButton jbtn_25; + JSButton jbtn_26; + JSButton jbtn_27; + JSButton jbtn_28; + JSButton jbtn_29; + JSButton jbtn_30; + JSButton jbtn_31; // Acro parameters AP_Float acro_rp_p; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 2e4c3a4136044..482aabfc38892 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -455,7 +455,15 @@ class Sub : public AP_Vehicle { void init_rc_out(); void enable_motor_output(); void init_joystick(); - void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); + void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, + int16_t s, + int16_t t, + int16_t aux1, + int16_t aux2, + int16_t aux3, + int16_t aux4, + int16_t aux5, + int16_t aux6); void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false); void handle_jsbutton_release(uint8_t button, bool shift); JSButton* get_button(uint8_t index); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index f10d7d3a45108..0eeafe94fb403 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -17,7 +17,7 @@ int16_t xTrim = 0; int16_t yTrim = 0; int16_t video_switch = 1100; int16_t x_last, y_last, z_last; -uint16_t buttons_prev; +uint32_t buttons_prev; // Servo control output channels // TODO: Allow selecting output channels @@ -51,7 +51,15 @@ void Sub::init_joystick() gain = constrain_float(gain, 0.1, 1.0); } -void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, + int16_t s, + int16_t t, + int16_t aux1, + int16_t aux2, + int16_t aux3, + int16_t aux4, + int16_t aux5, + int16_t aux6) { float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain @@ -65,17 +73,18 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t cam_tilt = 1500; cam_pan = 1500; + uint32_t all_buttons = buttons | (buttons2 << 16); // Detect if any shift button is pressed - for (uint8_t i = 0 ; i < 16 ; i++) { - if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) { + for (uint8_t i = 0 ; i < 32 ; i++) { + if ((all_buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) { shift = true; } } // Act if button is pressed // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick. - for (uint8_t i = 0 ; i < 16 ; i++) { - if ((buttons & (1 << i))) { + for (uint8_t i = 0 ; i < 32 ; i++) { + if ((all_buttons & (1 << i))) { handle_jsbutton_press(i,shift,(buttons_prev & (1 << i))); // buttonDebounce = tnow_ms; } else if (buttons_prev & (1 << i)) { @@ -83,7 +92,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t } } - buttons_prev = buttons; + buttons_prev = all_buttons; // attitude mode: if (roll_pitch_flag == 1) { @@ -110,8 +119,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t xTot = x + xTrim; } - RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch - RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900), tnow); // roll + RC_Channels::set_override(0, constrain_int16(s + pitchTrim + rpyCenter,1100,1900), tnow); // pitch + RC_Channels::set_override(1, constrain_int16(t + rollTrim + rpyCenter,1100,1900), tnow); // roll RC_Channels::set_override(2, constrain_int16((zTot)*throttleScale+throttleBase,1100,1900), tnow); // throttle RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow); // yaw @@ -706,6 +715,40 @@ JSButton* Sub::get_button(uint8_t index) return &g.jbtn_14; case 15: return &g.jbtn_15; + + // add 16 more cases for 32 buttons with MANUAL_CONTROL extensions + case 16: + return &g.jbtn_16; + case 17: + return &g.jbtn_17; + case 18: + return &g.jbtn_18; + case 19: + return &g.jbtn_19; + case 20: + return &g.jbtn_20; + case 21: + return &g.jbtn_21; + case 22: + return &g.jbtn_22; + case 23: + return &g.jbtn_23; + case 24: + return &g.jbtn_24; + case 25: + return &g.jbtn_25; + case 26: + return &g.jbtn_26; + case 27: + return &g.jbtn_27; + case 28: + return &g.jbtn_28; + case 29: + return &g.jbtn_29; + case 30: + return &g.jbtn_30; + case 31: + return &g.jbtn_31; default: return &g.jbtn_0; } From 43eecb5c850534abe2db63e355bf22010ac16b72 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 30 Nov 2023 15:00:20 -0300 Subject: [PATCH 2/2] Mavlink: bump submodule for MANUAL_CONTROL extensions --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index dccd8555cd601..71af5c43fc24f 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit dccd8555cd601467dc793ea9abf85caa63c0a9e8 +Subproject commit 71af5c43fc24ffb237d3a93dc0c8358d67aa0cc0