Plane: added RNGFND_LANDING option

this allows the use of a rangefinder for landing flare and landing
approach

ArduPlane/Parameters.h

@@ -115,7 +115,7 @@ public:
         k_param_takeoff_rotate_speed,
         k_param_takeoff_throttle_slewrate,
         k_param_takeoff_throttle_max,
-        k_param_sonar,
+        k_param_rangefinder,
         k_param_terrain,
         k_param_terrain_follow,
         k_param_stab_pitch_down_cd_old, // deprecated
@@ -123,6 +123,7 @@ public:
         k_param_stab_pitch_down,
         k_param_terrain_lookahead,
         k_param_fbwa_tdrag_chan,
+        k_param_rangefinder_landing,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -170,7 +171,7 @@ public:
         k_param_curr_amp_per_volt,  // unused
         k_param_input_voltage, // deprecated, can be deleted
         k_param_pack_capacity,      // unused
-        k_param_sonar_enabled,
+        k_param_sonar_enabled_old,  // unused
         k_param_ahrs,  // AHRS group
         k_param_barometer,   // barometer ground calibration
         k_param_airspeed,  // AP_Airspeed parameters
@@ -459,6 +460,7 @@ public:
 #endif
     AP_Int16 glide_slope_threshold;
     AP_Int8 fbwa_tdrag_chan;
+    AP_Int8 rangefinder_landing;
 
     // RC channels
     RC_Channel rc_1;

ArduPlane/Parameters.pde

@@ -916,7 +916,14 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
-    GOBJECT(sonar,                  "RNGFND", RangeFinder),
+    GOBJECT(rangefinder,            "RNGFND", RangeFinder),
+
+    // @Param: RNGFND_LANDING
+    // @DisplayName: Enable rangefinder for landing
+    // @Description: This enables the use of a rangefinder for automatic landing. The rangefinder will be used both on the landing approach and for final flare
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    GSCALAR(rangefinder_landing,    "RNGFND_LANDING",   0),
 
 #if AP_TERRAIN_AVAILABLE
     // @Group: TERRAIN_

ArduPlane/altitude.pde

@@ -42,7 +42,7 @@ static void adjust_altitude_target()
             location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
             set_target_altitude_location(loc);
         } else {
-            set_target_altitude_proportion(next_WP_loc, 
+            set_target_altitude_proportion(loc, 
                                            (wp_distance-flare_distance) / (wp_totalDistance-flare_distance));
         }
         // stay within the range of the start and end locations in altitude
@@ -223,12 +223,19 @@ static int32_t relative_target_altitude_cm(void)
         // add lookahead adjustment the target altitude
         target_altitude.lookahead = lookahead_adjustment();
         relative_home_height += target_altitude.lookahead;
+
+        // correct for rangefinder data
+        relative_home_height += rangefinder_correction();
+
         // we are following terrain, and have terrain data for the
         // current location. Use it.
         return relative_home_height*100;
     }
 #endif
-    return target_altitude.amsl_cm - home.alt + (int32_t(g.alt_offset)*100);
+    int32_t relative_alt = target_altitude.amsl_cm - home.alt;
+    relative_alt += int32_t(g.alt_offset)*100;
+    relative_alt += rangefinder_correction() * 100;
+    return relative_alt;
 }
 
 /*
@@ -497,3 +504,39 @@ static float lookahead_adjustment(void)
     return 0;
 #endif
 }
+
+
+/*
+  correct target altitude using rangefinder data. Returns offset in
+  meters to correct target altitude. A positive number means we need
+  to ask the speed/height controller to fly higher
+ */
+static float rangefinder_correction(void)
+{
+    if (!rangefinder_state.in_range) {
+        return 0;
+    }
+
+    // for now we only support the rangefinder for landing 
+    bool using_rangefinder = (g.rangefinder_landing &&
+                              control_mode == AUTO && 
+                              (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+                               flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL));
+    if (!using_rangefinder) {
+        return false;
+    }
+
+    // base correction is the difference between baro altitude and
+    // rangefinder estimate
+    float correction = relative_altitude() - rangefinder_state.height_estimate;
+
+#if AP_TERRAIN_AVAILABLE
+    // if we are terrain following then correction is based on terrain data
+    float terrain_altitude;
+    if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true)) {
+        correction = terrain_altitude - rangefinder_state.height_estimate;
+    }
+#endif    
+
+    return correction;
+}

ArduPlane/landing.pde

@@ -16,6 +16,9 @@ static bool verify_land()
 
     float height = height_above_target();
 
+    // use rangefinder to correct if possible
+    height -= rangefinder_correction();
+
     // calculate the sink rate.
     float sink_rate;
     Vector3f vel;