diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index a266a45de9..1f176ce8ec 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -517,6 +517,13 @@ const AP_Param::Info Sub::var_info[] = { // @Range: 30 400 GSCALAR(lights_step, "JS_LIGHTS_STEP", 100), + // @Param: JS_THR_GAIN + // @DisplayName: Throttle gain scalar + // @Description: Scalar for gain on the throttle channel + // @User: Standard + // @Range: 0.5 4.0 + GSCALAR(throttle_gain, "JS_THR_GAIN", 1.0f), + // @Group: BTN0_ // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp GGROUP(jbtn_0, "BTN0_", JSButton), diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 309097682a..45c8a1ba1c 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -206,7 +206,7 @@ public: k_param_disarm_delay, k_param_terrain_follow, k_param_rc_feel_rp, - + k_param_throttle_gain, // Acro Mode parameters k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate @@ -345,6 +345,7 @@ public: AP_Float maxGain; AP_Float minGain; AP_Int8 numGainSettings; + AP_Float throttle_gain; AP_Int16 cam_tilt_step; AP_Int16 lights_step; diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index bb7cffe8d2..7f90909962 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -46,7 +46,7 @@ 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*gain; // Scale -1000-1000 to -400-400 with gain - float throttleScale = 0.8*gain; // Scale 0-1000 to 0-800 with gain + float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain int16_t rpyCenter = 1500; int16_t throttleBase = 1500-500*throttleScale;