Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Clean joystick code with helpers

Browse Source
This commit is contained in:
Rustom Jehangir 2016-03-12 10:14:40 -08:00 committed by Andrew Tridgell
parent ef53455e2d
commit 802f4f617f
2 changed files with 36 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/Sub.h
View File

@ -885,7 +885,8 @@ private:
void enable_motor_output(); void enable_motor_output();
void read_radio(); 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 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_and_failsafe(uint16_t throttle_pwm);
void set_throttle_zero_flag(int16_t throttle_control); void set_throttle_zero_flag(int16_t throttle_control);
void init_barometer(bool full_calibration); void init_barometer(bool full_calibration);

73
ArduSub/joystick.cpp
View File

@ -5,6 +5,7 @@
// Functions that will handle joystick/gamepad input // Functions that will handle joystick/gamepad input
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
// Anonymous namespace to hold variables used only in this file
namespace { namespace {
int16_t mode; int16_t mode;
int16_t camTilt = 1500; 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; float throttleScale = 0.8;
int16_t rpyCenter = 1500; int16_t rpyCenter = 1500;
int16_t throttleBase = 1500-500*throttleScale; int16_t throttleBase = 1500-500*throttleScale;
uint8_t shift = 0; bool shift = false;
static uint32_t buttonDebounce; static uint32_t buttonDebounce;
// Debouncing timer // Debouncing timer
if ( tnow_ms - buttonDebounce > 50 ) { if ( tnow_ms - buttonDebounce > 50 ) {
buttonDebounce = tnow_ms; buttonDebounce = tnow_ms;
if ( (buttons & (1 << 0)) && g.jbtn_0.function() == JSButton::button_function_t::k_shift ) { shift = 1; } // Detect if any shift button is pressed
if ( (buttons & (1 << 1)) && g.jbtn_1.function() == JSButton::button_function_t::k_shift ) { shift = 1; } for ( uint8_t i = 0 ; i < 16 ; i++ ) {
if ( (buttons & (1 << 2)) && g.jbtn_2.function() == JSButton::button_function_t::k_shift ) { shift = 1; } if ( (buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift ) { shift = true; }
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; }
// Act if button is pressed
for ( uint8_t i = 0 ; i < 16 ; i++ ) { for ( uint8_t i = 0 ; i < 16 ; i++ ) {
if ( buttons & (1 << i) ) { if ( buttons & (1 << i) ) {
handle_jsbutton_press(i,shift); 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[0] = 1500; // pitch
channels[1] = 1500 + rollTrim; // roll channels[1] = 1500 + rollTrim; // roll
channels[2] = z*throttleScale+throttleBase; // throttle 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); failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10);
} }
void Sub::handle_jsbutton_press(uint8_t button, uint8_t shift) { void Sub::handle_jsbutton_press(uint8_t button, bool shift) {
uint8_t func; // Act based on the function assigned to this button
switch ( get_button(button)->function(shift) ) {
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 ) {
case JSButton::button_function_t::k_arm_toggle: case JSButton::button_function_t::k_arm_toggle:
break; break;
case JSButton::button_function_t::k_arm: case JSButton::button_function_t::k_arm:
@ -189,3 +161,26 @@ void Sub::handle_jsbutton_press(uint8_t button, uint8_t shift) {
break; 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;
}
}