Copter: surface tracking improvement

Separate correction speeds for pilot input vs altitude-above-ground
errors to allow slower correction to sonar signals.
Change SONAR_GAIN parameter to be altitude error -> desired speed.  This
means it should be set 10x larger than before.
Replace hard-coded sonar cut-off percentage of 70% with
SONAR_RELIABLE_DISTANCE_PCT.
Reduce sonar cut-off percentage to 60%.
Add desired sonar altitude to dataflash log's CTUN message.

ArduCopter/ArduCopter.pde

@@ -631,6 +631,7 @@ static int16_t climb_rate;
 // The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
 static int16_t sonar_alt;
 static uint8_t sonar_alt_health;   // true if we can trust the altitude from the sonar
+static float target_sonar_alt;      // desired altitude in cm above the ground
 // The altitude as reported by Baro in cm – Values can be quite high
 static int32_t baro_alt;
 

ArduCopter/Attitude.pde

@@ -1359,10 +1359,9 @@ static bool update_land_detector()
 static void
 get_throttle_surface_tracking(int16_t target_rate)
 {
-    static float target_sonar_alt = 0;   // The desired altitude in cm above the ground
     static uint32_t last_call_ms = 0;
     float distance_error;
-    float sonar_induced_slew_rate;
+    float velocity_correction;
 
     uint32_t now = millis();
 
@@ -1372,23 +1371,22 @@ get_throttle_surface_tracking(int16_t target_rate)
     }
     last_call_ms = now;
 
-    // adjust target alt if motors have not hit their limits
+    // adjust sonar target alt if motors have not hit their limits
     if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
         target_sonar_alt += target_rate * 0.02f;
     }
 
-    distance_error = (target_sonar_alt-sonar_alt);
-    sonar_induced_slew_rate = constrain_float(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
     target_sonar_alt = constrain_float(target_sonar_alt,sonar_alt-750,sonar_alt+750);
-    controller_desired_alt = current_loc.alt+(target_sonar_alt-sonar_alt);
 
-    // update target altitude for reporting purposes
-    set_target_alt_for_reporting(controller_desired_alt);
+    // calc desired velocity correction from target sonar alt vs actual sonar alt
+    distance_error = target_sonar_alt-sonar_alt;
+    velocity_correction = distance_error * g.sonar_gain;
+    velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
 
-    get_throttle_althold_with_slew(controller_desired_alt, target_rate-sonar_induced_slew_rate, target_rate+sonar_induced_slew_rate);   // VELZ_MAX limits how quickly we react
+    // call regular rate stabilize alt hold controller
+    get_throttle_rate_stabilized(target_rate + velocity_correction);
 }
 
 /*

ArduCopter/Log.pde

@@ -319,7 +319,7 @@ struct PACKED log_Control_Tuning {
     int16_t sonar_alt;
     int32_t baro_alt;
     float   next_wp_alt;
-    int16_t nav_throttle;
+    int16_t desired_sonar_alt;
     int16_t angle_boost;
     int16_t climb_rate;
     int16_t throttle_out;
@@ -335,7 +335,7 @@ static void Log_Write_Control_Tuning()
         sonar_alt           : sonar_alt,
         baro_alt            : baro_alt,
         next_wp_alt         : get_target_alt_for_reporting() / 100.0f,
-        nav_throttle        : nav_throttle,
+        desired_sonar_alt   : target_sonar_alt,
         angle_boost         : angle_boost,
         climb_rate          : climb_rate,
         throttle_out        : g.rc_3.servo_out,
@@ -763,7 +763,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),       
       "NTUN", "Ecffffffffee",    "WPDst,WPBrg,PErX,PErY,DVelX,DVelY,VelX,VelY,DAcX,DAcY,DRol,DPit" },
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),     
-      "CTUN", "hcefhhhhh",   "ThrIn,SonAlt,BarAlt,WPAlt,NavThr,AngBst,CRate,ThrOut,DCRate" },
+      "CTUN", "hcefchhhh",   "ThrIn,SonAlt,BarAlt,WPAlt,DesSonAlt,AngBst,CRate,ThrOut,DCRate" },
     { LOG_COMPASS_MSG, sizeof(log_Compass),             
       "MAG", "hhhhhhhhh",    "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 

ArduCopter/config.h

@@ -293,12 +293,16 @@
  # define SONAR_ALT_HEALTH_MAX 3            // number of good reads that indicates a healthy sonar
 #endif
 
+#ifndef SONAR_RELIABLE_DISTANCE_PCT
+ # define SONAR_RELIABLE_DISTANCE_PCT 0.60f // we trust the sonar out to 60% of it's maximum range
+#endif
+
 #ifndef SONAR_GAIN_DEFAULT
- # define SONAR_GAIN_DEFAULT 0.2            // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction)
+ # define SONAR_GAIN_DEFAULT 2.0            // 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 vertical speed change while surface tracking with sonar
+ # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with sonar
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/sensors.pde

@@ -40,7 +40,7 @@ static int16_t read_sonar(void)
 
     int16_t temp_alt = sonar->read();
 
-    if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70f) {
+    if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * SONAR_RELIABLE_DISTANCE_PCT) {
         if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) {
             sonar_alt_health++;
         }