From 69cd30fd8e0c73a18c64921d985a19f5af3e2bd1 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Fri, 22 Jul 2016 23:26:22 -0700 Subject: [PATCH] Sub: Revert "Sub: Remove gain control from joystick file." This reverts commit 93cc3abab2fdd690928bfa40f398a5f97be66723. --- ArduSub/joystick.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index a1d36aae1d..3819a5e159 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -21,6 +21,10 @@ namespace { int16_t video_switch = 1100; int16_t x_last, y_last, z_last; uint16_t buttons_prev; + float gain = 0.5; + float maxGain = 1.0; + float minGain = 0.25; + int8_t numGainSettings = 4; } void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) { @@ -28,8 +32,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t uint32_t tnow_ms = millis(); - float rpyScale = 0.4; // Scale -1000-1000 to -400-400 - float throttleScale = 0.8; // Scale 0-1000 to 0-800 + float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain + float throttleScale = 0.8*gain; // Scale 0-1000 to 0-800 with gain int16_t rpyCenter = 1500; int16_t throttleBase = 1500-500*throttleScale; @@ -184,20 +188,20 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift) { static bool lowGain = false; lowGain = !lowGain; if ( lowGain ) { - motors.set_gain(0.0f); + gain = 0.5f; } else { - motors.set_gain(1.0f); + gain = 1.0f; } - gcs_send_text_fmt(MAV_SEVERITY_INFO,"Gain: %2.0f%%",motors.get_gain()*100); + gcs_send_text_fmt(MAV_SEVERITY_INFO,"Gain: %2.0f%%",gain*100); } break; case JSButton::button_function_t::k_gain_inc: - motors.increase_gain(); - gcs_send_text_fmt(MAV_SEVERITY_INFO,"Gain: %2.0f%%",motors.get_gain()*100); + gain = constrain_float(gain + (maxGain-minGain)/(numGainSettings-1), minGain, maxGain); + gcs_send_text_fmt(MAV_SEVERITY_INFO,"Gain: %2.0f%%",gain*100); break; case JSButton::button_function_t::k_gain_dec: - motors.decrease_gain(); - gcs_send_text_fmt(MAV_SEVERITY_INFO,"Gain: %2.0f%%",motors.get_gain()*100); + gain = constrain_float(gain - (maxGain-minGain)/(numGainSettings-1), minGain, maxGain); + gcs_send_text_fmt(MAV_SEVERITY_INFO,"Gain: %2.0f%%",gain*100); break; case JSButton::button_function_t::k_trim_roll_inc: rollTrim = constrain_float(rollTrim+10,-200,200);