Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: add support for 32 buttons and two additonal axis

This commit is contained in:
Willian Galvani 2023-10-30 15:02:07 -03:00
parent 8253c7e506
commit 99b952f986
5 changed files with 176 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduSub/GCS_Mavlink.cpp
View File

@ -540,7 +540,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'

64
ArduSub/Parameters.cpp
View File

@ -354,6 +354,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

34
ArduSub/Parameters.h
View File

@ -145,6 +145,23 @@ public:
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
@ -292,6 +309,23 @@ public:
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;

10
ArduSub/Sub.h
View File

@ -543,7 +543,15 @@ private:
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);

61
ArduSub/joystick.cpp
View File

@ -18,7 +18,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
@ -52,7 +52,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
@ -66,17 +74,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)) {
@ -84,7 +93,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) {
@ -111,8 +120,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;
}