Copter: add SONAR_GAIN to tune reaction to sonar

Some users report the response to sonar is too violent, this allows that
reaction to be reduced

ArduCopter/Attitude.pde

@@ -1215,7 +1215,7 @@ get_throttle_surface_tracking(int16_t target_rate)
     target_sonar_alt += target_rate * 0.02f;
 
     distance_error = (target_sonar_alt-sonar_alt);
-    sonar_induced_slew_rate = constrain(fabsf(THR_SURFACE_TRACKING_P * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);
+    sonar_induced_slew_rate = constrain(fabsf(g.sonar_gain * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX);
 
     // do not let target altitude get too far from current altitude above ground
     // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition

ArduCopter/Parameters.h

@@ -81,7 +81,8 @@ public:
         k_param_wp_yaw_behavior,
         k_param_acro_trainer_enabled,
         k_param_pilot_velocity_z_max,
-        k_param_circle_rate,           // 29
+        k_param_circle_rate,
+        k_param_sonar_gain,             // 30
 
         // 65: AP_Limits Library
         k_param_limits = 65,
@@ -262,6 +263,7 @@ public:
     AP_Int8         sonar_type;       // 0 = XL, 1 = LV,
                                       // 2 = XLL (XL with 10m range)
                                       // 3 = HRLV
+    AP_Float        sonar_gain;
     AP_Int8         battery_monitoring;         // 0=disabled, 3=voltage only,
                                                 // 4=voltage and current
     AP_Float        volt_div_ratio;

ArduCopter/Parameters.pde

@@ -77,6 +77,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(sonar_type,     "SONAR_TYPE",           AP_RANGEFINDER_MAXSONARXL),
 
+    // @Param: SONAR_GAIN
+    // @DisplayName: Sonar gain
+    // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter
+    // @Range: 0.01 0.5
+    // @Increment: 0.01
+    // @User: Standard
+    GSCALAR(sonar_gain,     "SONAR_GAIN",           SONAR_GAIN_DEFAULT),
+
     // @Param: BATT_MONITOR
     // @DisplayName: Battery monitoring
     // @Description: Controls enabling monitoring of the battery's voltage and current

ArduCopter/config.h

@@ -305,12 +305,12 @@
  # define SONAR_ALT_HEALTH_MAX 3            // number of good reads that indicates a healthy sonar
 #endif
 
-#ifndef THR_SURFACE_TRACKING_P
- # define THR_SURFACE_TRACKING_P 0.2        // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
+#ifndef SONAR_GAIN_DEFAULT
+ # define SONAR_GAIN_DEFAULT 0.2            // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
 #endif
 
 #ifndef THR_SURFACE_TRACKING_VELZ_MAX
- # define THR_SURFACE_TRACKING_VELZ_MAX 30  // max speed number of good reads that indicates a healthy sonar
+ # define THR_SURFACE_TRACKING_VELZ_MAX 30  // max vertical speed change while surface tracking with sonar
 #endif
 
 //////////////////////////////////////////////////////////////////////////////