zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sub: Implement joystick button actions

master
Rustom Jehangir 9 years ago committed by Andrew Tridgell
parent
44904760cc
commit
ef53455e2d
2 changed files with 151 additions and 45 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. 1
      ArduSub/Sub.h
  2. 195
      ArduSub/joystick.cpp

1
ArduSub/Sub.h
Unescape View File

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

195
ArduSub/joystick.cpp
Unescape View File

@ -2,9 +2,18 @@ @@ -2,9 +2,18 @@
#include "Sub.h"
// Function that will read the radio data, limit servos and trigger a failsafe
// Functions that will handle joystick/gamepad input
// ----------------------------------------------------------------------------
namespace {
int16_t mode;
int16_t camTilt = 1500;
int16_t lights1 = 1100;
int16_t lights2 = 1100;
int16_t rollTrim = 0;
int16_t pitchTrim = 0;
}
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
int16_t channels[10];
@ -14,58 +23,34 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t @@ -14,58 +23,34 @@ 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;
static int16_t rollTrim = 0;
static int16_t mode;
static int16_t camTilt = 1500;
static int16_t lights = 1100;
uint8_t shift = 0;
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; }
for ( uint8_t i = 0 ; i < 16 ; i++ ) {
if ( buttons & (1 << i) ) {
// handle_jsbutton_press(i);
handle_jsbutton_press(i,shift);
}
}
// Button logic to arm/disarm motors (Start and back buttons)
if ( buttons & (1 << 4) ) {
init_arm_motors(true);
} else if ( buttons & (1 << 5) ) {
init_disarm_motors();
}
// Button logic to change camera tilt (D-pad up and down + left joystick click to center)
if ( buttons & (1 << 0) ) {
camTilt = constrain_float(camTilt-30,800,2200);
} else if ( buttons & (1 << 1) ) {
camTilt = constrain_float(camTilt+30,800,2200);
} else if ( buttons & (1 << 6) ) {
camTilt = 1500; // Reset camera tilt
}
// Button logic for roll trim (D-pad left and right)
if ( (buttons & ( 1 << 2 )) && rollTrim > -200 ) {
rollTrim -= 10;
} else if ( (buttons & ( 1 << 3 )) && rollTrim < 200 ) {
rollTrim += 10;
}
// Button logic for mode changes (B for stabilize, Y for altitude hold)
if ( buttons & (1 << 14) ) {
mode = 2000;
} else if ( buttons & (1 << 12)) {
mode = 1000;
}
// Button logic for lights with dimming (right bumper brighter, left bumper dimmer)
if ( buttons & (1 << 8) ) {
lights = constrain_float(lights-100,1100,1900);
} else if ( buttons & (1 << 9) ) {
lights = constrain_float(lights+100,1100,1900);
}
}
channels[0] = 1500; // pitch
@ -76,11 +61,131 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t @@ -76,11 +61,131 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
channels[5] = x*rpyScale+rpyCenter; // forward for ROV
channels[6] = y*rpyScale+rpyCenter; // strafe for ROV
channels[7] = camTilt; // camera tilt
channels[8] = lights;
channels[8] = lights1;
channels[9] = 0;
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10);
}
//void Sub::handle_jsbutton_press(uint8_t button);
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 ) {
case JSButton::button_function_t::k_arm_toggle:
break;
case JSButton::button_function_t::k_arm:
init_arm_motors(true);
break;
case JSButton::button_function_t::k_disarm:
init_disarm_motors();
break;
case JSButton::button_function_t::k_mode_toggle:
init_disarm_motors();
break;
case JSButton::button_function_t::k_mode_1:
mode = 1100;
break;
case JSButton::button_function_t::k_mode_2:
mode = 1300;
break;
case JSButton::button_function_t::k_mode_3:
mode = 1420;
break;
case JSButton::button_function_t::k_mode_4:
mode = 1550;
break;
case JSButton::button_function_t::k_mode_5:
mode = 1690;
break;
case JSButton::button_function_t::k_mode_6:
mode = 1900;
break;
case JSButton::button_function_t::k_mount_center:
camTilt = 1500;
break;
case JSButton::button_function_t::k_mount_tilt_up:
camTilt = constrain_float(camTilt-30,800,2200);
break;
case JSButton::button_function_t::k_mount_tilt_down:
camTilt = constrain_float(camTilt+30,800,2200);
break;
case JSButton::button_function_t::k_camera_trigger:
break;
case JSButton::button_function_t::k_lights1_cycle:
{
static bool increasing = true;
if ( increasing ) {
lights1 = constrain_float(lights1+100,1100,1900);
} else {
lights1 = constrain_float(lights1-100,1100,1900);
}
if ( lights1 >= 1900 || lights1 <= 1100 ) {
increasing = !increasing;
}
}
break;
case JSButton::button_function_t::k_lights1_brighter:
lights1 = constrain_float(lights1+100,1100,1900);
break;
case JSButton::button_function_t::k_lights1_dimmer:
lights1 = constrain_float(lights1-100,1100,1900);
break;
case JSButton::button_function_t::k_lights2_cycle:
{
static bool increasing = true;
if ( increasing ) {
lights2 = constrain_float(lights2+100,1100,1900);
} else {
lights2 = constrain_float(lights2-100,1100,1900);
}
if ( lights2 >= 1900 || lights2 <= 1100 ) {
increasing = !increasing;
}
}
break;
case JSButton::button_function_t::k_lights2_brighter:
lights2 = constrain_float(lights2+100,1100,1900);
break;
case JSButton::button_function_t::k_lights2_dimmer:
lights2 = constrain_float(lights2-100,1100,1900);
break;
case JSButton::button_function_t::k_gain_toggle:
break;
case JSButton::button_function_t::k_gain_inc:
break;
case JSButton::button_function_t::k_gain_dec:
break;
case JSButton::button_function_t::k_trim_roll_inc:
rollTrim = constrain_float(rollTrim+10,-200,200);
break;
case JSButton::button_function_t::k_trim_roll_dec:
rollTrim = constrain_float(rollTrim-10,-200,200);
break;
case JSButton::button_function_t::k_trim_pitch_inc:
pitchTrim = constrain_float(pitchTrim+10,-200,200);
break;
case JSButton::button_function_t::k_trim_pitch_dec:
pitchTrim = constrain_float(pitchTrim-10,-200,200);
break;
}
}

Write Preview
Loading…
Cancel
Save