Sub: Add gain parameters

ArduSub/Parameters.cpp

@@ -622,6 +622,38 @@ const AP_Param::Info Sub::var_info[] = {
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
     GGROUP(rc_14,                   "RC14_", RC_Channel_aux),
 
+	// @Param: JS_GAIN_DEFAULT
+	// @DisplayName: Default gain at boot
+	// @Discription: Default gain at boot, must be in range [JS_GAIN_MIN , JS_GAIN_MAX]
+	// @User: Standard
+	// @Range: 0.1 1.0
+	// @Default: 0.5
+	GSCALAR(gain_default, "JS_GAIN_DEFAULT", 0.5),
+
+	// @Param: JS_GAIN_MAX
+	// @DisplayName: Maximum joystick gain
+	// @Description: Maximum joystick gain
+	// @User: Standard
+	// @Range: 0.2 1.0
+	// @Default: 1.0
+	GSCALAR(maxGain, "JS_GAIN_MAX", 1.0),
+
+	// @Param: JS_GAIN_MIN
+	// @DisplayName: Minimum joystick gain
+	// @Description: Minimum joystick gain
+	// @User: Standard
+	// @Range: 0.1 0.8
+	// @Default: 0.25
+	GSCALAR(minGain, "JS_GAIN_MIN", 0.25),
+
+	// @Param: JS_GAIN_STEPS
+	// @DisplayName: Gain steps
+	// @Discription: Controls the number of steps between minimum and maximum joystick gain when the gain is adjusted using buttons. Set to 1 to always use JS_GAIN_DEFAULT.
+	// @User: Standard
+	// @Range: 1 10
+	// @Default: 4
+	GSCALAR(numGainSettings, "JS_GAIN_STEPS", 4),
+
 	// @Group: BTN0_
 	// @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
 	GGROUP(jbtn_0,                   "BTN0_", JSButton),

ArduSub/Parameters.h

@@ -369,6 +369,12 @@ public:
 		//Sub-specific parameters
 		k_param_surface_depth = 256,
 
+		// Joystic gain parameters
+		k_param_gain_default,
+		k_param_maxGain,
+		k_param_minGain,
+		k_param_numGainSettings,
+		
 		// Joystick button mapping parameters
 		k_param_jbtn_0 = 261,
 		k_param_jbtn_1,
@@ -519,6 +525,11 @@ public:
 
     AP_Int16                rc_speed; // speed of fast RC Channels in Hz
 
+	AP_Float				gain_default;
+	AP_Float				maxGain;
+	AP_Float				minGain;
+	AP_Int8					numGainSettings;
+
     // Joystick button parameters
     JSButton 				jbtn_0;
     JSButton 				jbtn_1;

ArduSub/Sub.h

@@ -921,6 +921,7 @@ private:
     void init_rc_out();
     void enable_motor_output();
     void read_radio();
+    void init_joystick();
     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,bool shift=false,bool held=false);
     void camera_tilt_smooth();

ArduSub/joystick.cpp

@@ -21,10 +21,20 @@ 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;
+	float gain;
+}
+
+void Sub::init_joystick() {
+    if(g.numGainSettings < 1) g.numGainSettings.set_and_save(1);
+
+    if(g.numGainSettings == 1 || (g.gain_default < g.maxGain + 0.01 && g.gain_default > g.minGain - 0.01)) {
+    	gain = constrain_float(g.gain_default, g.minGain, g.maxGain); // Use default gain parameter
+    } else {
+    	// Use setting closest to average of minGain and maxGain
+        gain = g.minGain + (g.numGainSettings/2 - 1) * (g.maxGain - g.minGain) / (g.numGainSettings - 1);
+    }
+
+    gain = constrain_float(gain, 0.1, 1.0);
 }
 
 void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
@@ -208,13 +218,33 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) {
 			break;
 		case JSButton::button_function_t::k_gain_inc:
 			if ( !held ) {
-				gain = constrain_float(gain + (maxGain-minGain)/(numGainSettings-1), minGain, maxGain);
+				// check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
+				g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
+				g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
+				g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
+
+				if(g.numGainSettings == 1) {
+					gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
+				} else {
+					gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
+				}
+
 				gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",gain*100);
 			}
 			break;
 		case JSButton::button_function_t::k_gain_dec:
 			if ( !held ) {
-				gain = constrain_float(gain - (maxGain-minGain)/(numGainSettings-1), minGain, maxGain);
+				// check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
+				g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
+				g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
+				g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
+
+				if(g.numGainSettings == 1) {
+					gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
+				} else {
+					gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
+				}
+
 				gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",gain*100);
 			}
 			break;

ArduSub/system.cpp

@@ -157,6 +157,7 @@ void Sub::init_ardupilot()
     
     init_rc_in();               // sets up rc channels from radio
     init_rc_out();              // sets up motors and output to escs
+    init_joystick();			// joystick initialization
 
     // initialise which outputs Servo and Relay events can use
     ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());