Plane: added rangefinder correction by terrain data

allows for landing approach with terrain changes

ArduPlane/Plane.h

@@ -863,6 +863,7 @@ private:
     float lookahead_adjustment(void);
     float rangefinder_correction(void);
     void rangefinder_height_update(void);
+    void rangefinder_terrain_correction(float &height);
     void set_next_WP(const struct Location &loc);
     void set_guided_WP(void);
     void update_home();

ArduPlane/altitude.cpp

@@ -576,6 +576,28 @@ float Plane::rangefinder_correction(void)
     return rangefinder_state.correction;
 }
 
+/*
+  correct rangefinder data for terrain height difference between
+  NAV_LAND point and current location
+ */
+void Plane::rangefinder_terrain_correction(float &height)
+{
+#if AP_TERRAIN_AVAILABLE
+    if (!g.rangefinder_landing ||
+        flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND ||
+        g.terrain_follow == 0) {
+        return;
+    }
+    float terrain_amsl1, terrain_amsl2;
+    if (!terrain.height_amsl(current_loc, terrain_amsl1, false) ||
+        !terrain.height_amsl(next_WP_loc, terrain_amsl2, false)) {
+        return;
+    }
+    float correction = (terrain_amsl1 - terrain_amsl2);
+    height += correction;
+#endif
+}
+
 /*
   update the offset between rangefinder height and terrain height
  */
@@ -592,6 +614,8 @@ void Plane::rangefinder_height_update(void)
         // is cos(roll)*cos(pitch))
         rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
 
+        rangefinder_terrain_correction(rangefinder_state.height_estimate);
+
         // we consider ourselves to be fully in range when we have 10
         // good samples (0.2s) that are different by 5% of the maximum
         // range from the initial range we see. The 5% change is to