From 12464ab5b22dae3386ae8952fca2780ef87cc3fb Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Sat, 30 Jan 2016 08:42:55 -0800 Subject: [PATCH] Sub: Added roll trim using gamepad buttons. --- ArduSub/radio.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index fca173a72b..864e8ea1f9 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -148,6 +148,7 @@ 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; + int16_t rollTrim = 0; static int16_t mode; static int16_t camTilt = 1500; @@ -156,12 +157,20 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t init_arm_motors(true); } else if ( buttons & (1 << 5) ) { init_disarm_motors(); - } else if ( buttons & (1 << 0) ) { + } + + if ( buttons & (1 << 0) ) { camTilt = constrain_float(camTilt+20,800,2200); } else if ( buttons & (1 << 1) ) { camTilt = constrain_float(camTilt-20,800,2200); } + if ( (buttons & ( 1 << 14 )) && rollTrim > -200 ) { + rollTrim -= 10; + } else if ( (buttons & ( 1 << 15 )) && rollTrim < 200 ) { + rollTrim += 10; + } + if ( buttons & (1 << 14) ) { mode = 2000; } else if ( buttons & (1 << 12)) { @@ -169,7 +178,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t } channels[0] = 1500; // pitch - channels[1] = 1500; // roll + channels[1] = 1500 + rollTrim; // roll channels[2] = z*throttleScale+throttleBase; // throttle channels[3] = r*rpyScale+rpyCenter; // yaw channels[4] = mode; // for testing only