From af3807bc5788d28317ec4ef0b6cddd972ebfc370 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sat, 5 Mar 2016 10:18:09 -0800 Subject: [PATCH] Sub: Move joystick interpretation to joystick.cpp --- ArduSub/joystick.cpp | 78 ++++++++++++++++++++++++++++++++++++++++++++ ArduSub/radio.cpp | 72 ---------------------------------------- 2 files changed, 78 insertions(+), 72 deletions(-) create mode 100644 ArduSub/joystick.cpp diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp new file mode 100644 index 0000000000..b7b6d398a5 --- /dev/null +++ b/ArduSub/joystick.cpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Sub.h" + +// Function that will read the radio data, limit servos and trigger a failsafe +// ---------------------------------------------------------------------------- + +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]; + + uint32_t tnow_ms = millis(); + + float rpyScale = 0.5; + 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; + static uint32_t buttonDebounce; + + // Debouncing timer + if ( tnow_ms - buttonDebounce > 50 ) { + buttonDebounce = tnow_ms; + + // 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 + channels[1] = 1500 + rollTrim; // roll + channels[2] = z*throttleScale+throttleBase; // throttle + channels[3] = r*rpyScale+rpyCenter; // yaw + channels[4] = mode; // for testing only + 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[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); +} \ No newline at end of file diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index 77508d2f74..ce2286a5c6 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -131,78 +131,6 @@ void Sub::read_radio() } } -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]; - - uint32_t tnow_ms = millis(); - - float rpyScale = 0.5; - 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; - static uint32_t buttonDebounce; - - // Debouncing timer - if ( tnow_ms - buttonDebounce > 50 ) { - buttonDebounce = tnow_ms; - - // 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 - channels[1] = 1500 + rollTrim; // roll - channels[2] = z*throttleScale+throttleBase; // throttle - channels[3] = r*rpyScale+rpyCenter; // yaw - channels[4] = mode; // for testing only - 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[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); -} - #define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value void Sub::set_throttle_and_failsafe(uint16_t throttle_pwm) {