ArduCopter: first implementation of surface tracking using sonar

Note: not yet enabled as part of any flight mode

ArduCopter/ArduCopter.pde

@@ -673,6 +673,7 @@ static int16_t climb_rate_error;
 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 bool sonar_alt_ok;   // true if we can trust the altitude from the sonar
 // The climb_rate as reported by sonar in cm/s
 static int16_t sonar_rate;
 // The altitude as reported by Baro in cm – Values can be quite high
@@ -1797,6 +1798,17 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
             throttle_initialised = true;
             break;
 
+        case THROTTLE_SURFACE_TRACKING:
+            if( g.sonar_enabled ) {
+                set_new_altitude(current_loc.alt);          // by default hold the current altitude
+                if ( throttle_mode < THROTTLE_HOLD ) {      // reset the alt hold I terms if previous throttle mode was manual
+                    reset_throttle_I();
+                }
+                throttle_initialised = true;
+            }
+            // To-Do: handle the case where the sonar is not enabled
+            break;
+
         default:
             // To-Do: log an error message to the dataflash or tlogs instead of printing to the serial port
             cliSerial->printf_P(PSTR("Unsupported throttle mode: %d!!"),new_throttle_mode);
@@ -1953,6 +1965,18 @@ void update_throttle_mode(void)
         // landing throttle controller
         get_throttle_land();
         break;
+
+    case THROTTLE_SURFACE_TRACKING:
+        // surface tracking with sonar or other rangefinder plus pilot input of climb rate
+        pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
+        if( sonar_alt_ok ) {
+            // if sonar is ok, use surface tracking
+            get_throttle_surface_tracking(pilot_climb_rate);
+        }else{
+            // if no sonar fall back stabilize rate controller
+            get_throttle_rate_stabilized(pilot_climb_rate);
+        }
+        break;
     }
 }
 

ArduCopter/Attitude.pde

@@ -1052,8 +1052,8 @@ get_throttle_rate_stabilized(int16_t target_rate)
 static void
 get_throttle_land()
 {
-    // if we are above 10m perform regular alt hold descent
+    // if we are above 10m and the sonar does not sense anything perform regular alt hold descent
-    if (current_loc.alt >= LAND_START_ALT) {
+    if (current_loc.alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_ok)) {
         get_throttle_althold(LAND_START_ALT, g.auto_velocity_z_min, -abs(g.land_speed));
     }else{
         get_throttle_rate_stabilized(-abs(g.land_speed));
@@ -1078,6 +1078,32 @@ get_throttle_land()
     }
 }
 
+// get_throttle_surface_tracking - hold copter at the desired distance above the ground
+// updates accel based throttle controller targets
+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;
+
+    uint32_t now = millis();
+
+    // reset target altitude if this controller has just been engaged
+    if( now - last_call_ms > 1000 ) {
+        target_sonar_alt = sonar_alt + next_WP.alt - current_loc.alt;
+    }
+    last_call_ms = millis();
+
+    target_sonar_alt += target_rate * 0.02;
+
+    // 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(target_sonar_alt,sonar_alt-750,sonar_alt+750);
+    set_new_altitude(current_loc.alt+(target_sonar_alt-sonar_alt));
+
+    get_throttle_althold(next_WP.alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250);   // 250 is added to give head room to alt hold controller
+}
+
 /*
  *  reset all I integrators
  */

ArduCopter/defines.h

@@ -39,6 +39,7 @@
 #define THROTTLE_HOLD                       6   // alt hold plus pilot input of climb rate
 #define THROTTLE_AUTO                       7   // auto pilot altitude controller with target altitude held in next_WP.alt
 #define THROTTLE_LAND                       8   // landing throttle controller
+#define THROTTLE_SURFACE_TRACKING           9   // ground tracking with sonar or other rangefinder
 
 
 // active altitude sensor

ArduCopter/sensors.pde

@@ -37,6 +37,12 @@ static int16_t read_sonar(void)
 #if CONFIG_SONAR == ENABLED
     int16_t temp_alt = sonar.read();
 
+    if(temp_alt >= sonar.min_distance && temp_alt <= sonar.max_distance * 0.70) {
+        sonar_alt_ok = true;
+    }else{
+        sonar_alt_ok = false;
+    }
+
  #if SONAR_TILT_CORRECTION == 1
     // correct alt for angle of the sonar
     float temp = cos_pitch_x * cos_roll_x;