Skip to content

Commit

Permalink
Sub: add support for 32 buttons and two additonal axis
Browse files Browse the repository at this point in the history
  • Loading branch information
@Williangalvani
Williangalvani committed Nov 30, 2023
1 parent ce824b7 commit 8ec039a
Show file tree
Hide file tree
Showing 5 changed files with 176 additions and 11 deletions.
18 changes: 17 additions & 1 deletion ArduSub/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
64 changes: 64 additions & 0 deletions ArduSub/Parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
34 changes: 34 additions & 0 deletions ArduSub/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down
10 changes: 9 additions & 1 deletion ArduSub/Sub.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
61 changes: 52 additions & 9 deletions ArduSub/joystick.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -65,25 +73,26 @@ 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)) {
handle_jsbutton_release(i, shift);
}
}

buttons_prev = buttons;
buttons_prev = all_buttons;

// attitude mode:
if (roll_pitch_flag == 1) {
Expand All @@ -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
Expand Down Expand Up @@ -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;
}
Expand Down

0 comments on commit 8ec039a

Please sign in to comment.