zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sub: Clean joystick code with helpers

mission-4.1.18
Rustom Jehangir 9 years ago committed by Andrew Tridgell
parent
ef53455e2d
commit
802f4f617f
2 changed files with 36 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      ArduSub/Sub.h
  2. 73
      ArduSub/joystick.cpp

3
ArduSub/Sub.h
Unescape View File

@ -885,7 +885,8 @@ private: @@ -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);

73
ArduSub/joystick.cpp
Unescape View File

@ -5,6 +5,7 @@ @@ -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 @@ -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 @@ -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 @@ -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) { @@ -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;
}
}

Write Preview
Loading…
Cancel
Save