diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 7426f79fab..fb70da0402 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -885,7 +885,8 @@ private: void enable_motor_output(); void read_radio(); void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); - void handle_jsbutton_press(uint8_t button,uint8_t shift=0); + void handle_jsbutton_press(uint8_t button,bool shift=false); + JSButton* get_button(uint8_t index); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); void init_barometer(bool full_calibration); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index f587052b4d..1bfef5c86c 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -5,6 +5,7 @@ // Functions that will handle joystick/gamepad input // ---------------------------------------------------------------------------- +// Anonymous namespace to hold variables used only in this file namespace { int16_t mode; int16_t camTilt = 1500; @@ -23,29 +24,19 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t float throttleScale = 0.8; int16_t rpyCenter = 1500; int16_t throttleBase = 1500-500*throttleScale; - uint8_t shift = 0; + bool shift = false; static uint32_t buttonDebounce; // Debouncing timer if ( tnow_ms - buttonDebounce > 50 ) { buttonDebounce = tnow_ms; - if ( (buttons & (1 << 0)) && g.jbtn_0.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 1)) && g.jbtn_1.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 2)) && g.jbtn_2.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 3)) && g.jbtn_3.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 4)) && g.jbtn_4.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 5)) && g.jbtn_5.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 6)) && g.jbtn_6.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 7)) && g.jbtn_7.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 8)) && g.jbtn_8.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 9)) && g.jbtn_9.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 10)) && g.jbtn_10.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 11)) && g.jbtn_11.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 12)) && g.jbtn_12.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 13)) && g.jbtn_13.function() == JSButton::button_function_t::k_shift ) { shift = 1; } - if ( (buttons & (1 << 14)) && g.jbtn_14.function() == JSButton::button_function_t::k_shift ) { shift = 1; } + // 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 ) { shift = true; } + } + // Act if button is pressed for ( uint8_t i = 0 ; i < 16 ; i++ ) { if ( buttons & (1 << i) ) { handle_jsbutton_press(i,shift); @@ -53,6 +44,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t } } + // Set channels to override channels[0] = 1500; // pitch channels[1] = 1500 + rollTrim; // roll channels[2] = z*throttleScale+throttleBase; // throttle @@ -68,29 +60,9 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10); } -void Sub::handle_jsbutton_press(uint8_t button, uint8_t shift) { - uint8_t func; - - switch ( button ) { - case 0: if ( shift ) func = g.jbtn_0.function(); else func = g.jbtn_0.function(true); break; - case 1: if ( shift ) func = g.jbtn_1.function(); else func = g.jbtn_1.function(true); break; - case 2: if ( shift ) func = g.jbtn_2.function(); else func = g.jbtn_2.function(true); break; - case 3: if ( shift ) func = g.jbtn_3.function(); else func = g.jbtn_3.function(true); break; - case 4: if ( shift ) func = g.jbtn_4.function(); else func = g.jbtn_4.function(true); break; - case 5: if ( shift ) func = g.jbtn_5.function(); else func = g.jbtn_5.function(true); break; - case 6: if ( shift ) func = g.jbtn_6.function(); else func = g.jbtn_6.function(true); break; - case 7: if ( shift ) func = g.jbtn_7.function(); else func = g.jbtn_7.function(true); break; - case 8: if ( shift ) func = g.jbtn_8.function(); else func = g.jbtn_8.function(true); break; - case 9: if ( shift ) func = g.jbtn_9.function(); else func = g.jbtn_9.function(true); break; - case 10: if ( shift ) func = g.jbtn_10.function(); else func = g.jbtn_10.function(true); break; - case 11: if ( shift ) func = g.jbtn_11.function(); else func = g.jbtn_11.function(true); break; - case 12: if ( shift ) func = g.jbtn_12.function(); else func = g.jbtn_12.function(true); break; - case 13: if ( shift ) func = g.jbtn_13.function(); else func = g.jbtn_13.function(true); break; - case 14: if ( shift ) func = g.jbtn_14.function(); else func = g.jbtn_14.function(true); break; - case 15: if ( shift ) func = g.jbtn_15.function(); else func = g.jbtn_15.function(true); break; - } - - switch ( func ) { +void Sub::handle_jsbutton_press(uint8_t button, bool shift) { + // Act based on the function assigned to this button + switch ( get_button(button)->function(shift) ) { case JSButton::button_function_t::k_arm_toggle: break; case JSButton::button_function_t::k_arm: @@ -189,3 +161,26 @@ void Sub::handle_jsbutton_press(uint8_t button, uint8_t shift) { break; } } + +JSButton* Sub::get_button(uint8_t index) { + // Help to access appropriate parameter + switch (index) { + case 0: return &g.jbtn_0; + case 1: return &g.jbtn_1; + case 2: return &g.jbtn_2; + case 3: return &g.jbtn_3; + case 4: return &g.jbtn_4; + case 5: return &g.jbtn_5; + case 6: return &g.jbtn_6; + case 7: return &g.jbtn_7; + case 8: return &g.jbtn_8; + case 9: return &g.jbtn_9; + case 10: return &g.jbtn_10; + case 11: return &g.jbtn_11; + case 12: return &g.jbtn_12; + case 13: return &g.jbtn_13; + case 14: return &g.jbtn_14; + case 15: return &g.jbtn_15; + default: return &g.jbtn_0; + } +}