ArduCopter: restored landing detector to landing throttle mode.

Added LAND_SPEED parameter to control the rate of descent for the last 10m of a landing.
Added CH6 tuning for accel based throttle controller.

ArduCopter/ArduCopter.pde

@@ -720,9 +720,8 @@ static byte throttle_mode;
 ////////////////////////////////////////////////////////////////////////////////
 // An additional throttle added to keep the copter at the same altitude when banking
 static int16_t angle_boost;
-// for controlling the landing throttle curve
-//verifies landings
-static int16_t ground_detector;
+// counter to verify landings
+static uint16_t land_detector;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -1691,7 +1690,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
         case THROTTLE_HOLD:
         case THROTTLE_AUTO:
             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
+            if ( throttle_mode < THROTTLE_HOLD ) {      // reset the alt hold I terms if previous throttle mode was manual
                 reset_throttle_I();
             }
             throttle_accel_controller_active = true;    // this controller uses accel based throttle controller
@@ -1700,7 +1699,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
 
         case THROTTLE_LAND:
             set_land_complete(false);   // mark landing as incomplete
-            ground_detector = 0;        // A counter that goes up if our climb rate stalls out.
+            land_detector = 0;          // A counter that goes up if our climb rate stalls out.
             set_new_altitude(0);        // Set a new target altitude
             throttle_accel_controller_active = true;    // this controller uses accel based throttle controller
             throttle_initialised = true;
@@ -2153,6 +2152,18 @@ static void tuning(){
         inertial_nav.set_time_constant_z(tuning_value);
         break;
 #endif
+
+    case CH6_THR_ACCEL_KP:
+        g.pid_throttle_accel.kP(tuning_value);
+        break;
+
+    case CH6_THR_ACCEL_KI:
+        g.pid_throttle_accel.kI(tuning_value);
+        break;
+
+    case CH6_THR_ACCEL_KD:
+        g.pid_throttle_accel.kD(tuning_value);
+        break;
     }
 }
 

ArduCopter/Attitude.pde

@@ -711,8 +711,8 @@ static void update_throttle_cruise(int16_t throttle)
     if( throttle_avg == 0 ) {
         throttle_avg = g.throttle_cruise;
     }
-    // calc average throttle
-    if ((throttle > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) {
+    // calc average throttle if we are in a level hover
+    if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
         throttle_avg = throttle_avg * .99 + (float)throttle * .01;
         g.throttle_cruise = throttle_avg;
     }
@@ -963,6 +963,11 @@ get_throttle_rate(int16_t z_target_speed)
 
     // set target for accel based throttle controller
     set_throttle_accel_target(target_accel);
+
+    // update throttle cruise
+    if( z_target_speed == 0 ) {
+        update_throttle_cruise(g.rc_3.servo_out);
+    }
 }
 
 // get_throttle_rate_stabilized - rate controller with additional 'stabilizer'
@@ -1037,18 +1042,38 @@ get_throttle_althold(int32_t target_alt, int16_t max_climb_rate)
 // get_throttle_land - high level landing logic
 // sends the desired acceleration in the accel based throttle controller
 // called at 50hz
-#define LAND_START_ALT 1000     // altitude in cm where land controller switches to slow rate of descent
-#define LAND_DESCENT_SPEED -50  // minimum descent speed in cm/s
+#define LAND_START_ALT 1000         // altitude in cm where land controller switches to slow rate of descent
+#define LAND_DETECTOR_TRIGGER 250   // number of 50hz iterations with near zero climb rate that triggers landing complete.
 static void
 get_throttle_land()
 {
     // if we are above 10m perform regular alt hold descent
     if (current_loc.alt >= LAND_START_ALT) {
-        get_throttle_althold(LAND_START_ALT, LAND_DESCENT_SPEED);
+        get_throttle_althold(LAND_START_ALT, -abs(g.land_speed));
     }else{
-        get_throttle_rate_stabilized(LAND_DESCENT_SPEED);
+        get_throttle_rate_stabilized(-abs(g.land_speed));
+
+        // detect whether we have landed
+#if INERTIAL_NAV == ENABLED
+        if (abs(inertial_nav._velocity.z) < 20) {
+#else
+        if (abs(climb_rate) < 20) {
+#endif
+            if( land_detector < LAND_DETECTOR_TRIGGER ) {
+                land_detector++;
+            }else{
+                set_land_complete(true);
+                if( g.rc_3.control_in == 0 ) {
+                    init_disarm_motors();
+                }
+            }
+        }else{
+            // we've sensed movement up or down so decrease land_detector
+            if (land_detector > 0 ) {
+                land_detector--;
+            }
+        }
     }
-    // TO-DO: add detection of whether we've landed or not and set ap.land_complete
 }
 
 /*

ArduCopter/Parameters.h

@@ -201,6 +201,7 @@ public:
         k_param_waypoint_radius,
         k_param_loiter_radius,
         k_param_waypoint_speed_max,
+        k_param_land_speed, // 217
 
         //
         // 220: PI/D Controllers
@@ -281,6 +282,7 @@ public:
     AP_Float        crosstrack_gain;
     AP_Int16 		crosstrack_min_distance;
     AP_Int32        auto_land_timeout;
+    AP_Int16        land_speed;
 
 
     // Throttle

ArduCopter/Parameters.pde

@@ -193,6 +193,15 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     GSCALAR(auto_land_timeout,      "AUTO_LAND",    AUTO_LAND_TIME*1000),
 
+    // @Param: LAND_SPEED
+    // @DisplayName: Land speed
+    // @Description: The descent speed for the final stage of landing in cm/s
+    // @Units: cm/s
+    // @Range: 10 200
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(land_speed,             "LAND_SPEED",   LAND_SPEED),
+
     // @Param: THR_MIN
     // @DisplayName: Minimum Throttle
     // @Description: The minimum throttle which the autopilot will apply.

ArduCopter/config.h

@@ -437,6 +437,9 @@
  # define AUTO_LAND_TIME 5
 #endif
 
+#ifndef LAND_SPEED
+ # define LAND_SPEED    50          // the descent speed for the final stage of landing in cm/s
+#endif
 
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/defines.h

@@ -167,8 +167,11 @@
 #define CH6_RATE_KD 21
 #define CH6_YAW_RATE_KP 6
 #define CH6_YAW_RATE_KD 26
-// Altitude rate controller
+// Throttle
 #define CH6_THROTTLE_KP 7
+#define CH6_THR_ACCEL_KP 33
+#define CH6_THR_ACCEL_KI 34
+#define CH6_THR_ACCEL_KD 35
 // Extras
 #define CH6_TOP_BOTTOM_RATIO 8
 #define CH6_RELAY 9