AP_Terrain: fixed assumption that HOME is on the ground

this fixes height_above_terrain() to give a correct value when HOME is
not on the ground after the user has done a DO_SET_HOME with a
home position that is not at ground level

libraries/AP_Terrain/AP_Terrain.cpp

@@ -234,16 +234,30 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool
 */
 bool AP_Terrain::height_above_terrain(float &terrain_altitude, bool extrapolate)
 {
-    float terrain_difference;
-    if (!height_terrain_difference_home(terrain_difference, extrapolate)) {
+    const AP_AHRS &ahrs = AP::ahrs();
+
+    Location current_loc;
+    if (!ahrs.get_location(current_loc)) {
+        // we don't know where we are
         return false;
     }
 
-    float relative_home_altitude;
-    AP::ahrs().get_relative_position_D_home(relative_home_altitude);
-    relative_home_altitude = -relative_home_altitude;
+    float theight_loc;
+    if (!height_amsl(current_loc, theight_loc)) {
+        if (!extrapolate) {
+            return false;
+        }
+        // we don't have data at the current location, but the caller
+        // has asked for extrapolation, so use the last available
+        // terrain height. This can be used to fill in while new data
+        // is fetched. It should be very rarely used
+        theight_loc = last_current_loc_height;
+    }
+
+    int32_t height_amsl_cm = 0;
+    UNUSED_RESULT(current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height_amsl_cm));
 
-    terrain_altitude = relative_home_altitude - terrain_difference;
+    terrain_altitude = height_amsl_cm*0.01 - theight_loc;
     return true;
 }
 
@@ -271,6 +285,22 @@ bool AP_Terrain::height_relative_home_equivalent(float terrain_altitude,
         return false;
     }
     relative_home_altitude = terrain_altitude + terrain_difference;
+
+    /*
+      adjust for height of home above terrain height at home
+     */
+    const AP_AHRS &ahrs = AP::ahrs();
+    const auto &home = ahrs.get_home();
+    int32_t home_height_amsl_cm = 0;
+    UNUSED_RESULT(home.get_alt_cm(Location::AltFrame::ABSOLUTE, home_height_amsl_cm));
+
+    float theight_home;
+    if (!height_amsl(home, theight_home)) {
+        return false;
+    }
+
+    relative_home_altitude += theight_home - home_height_amsl_cm*0.01;
+
     return true;
 }
 

libraries/AP_Terrain/TerrainGCS.cpp

@@ -212,35 +212,20 @@ void AP_Terrain::handle_data(mavlink_channel_t chan, const mavlink_message_t &ms
 void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc, bool extrapolate)
 {
     float terrain_height = 0;
-    float home_terrain_height = 0;
     uint16_t spacing = 0;
-    Location current_loc;
-    const AP_AHRS &ahrs = AP::ahrs();
-    if (ahrs.get_location(current_loc) &&
-        height_amsl(ahrs.get_home(), home_terrain_height) &&
-        height_amsl(loc, terrain_height)) {
+    if (height_amsl(loc, terrain_height)) {
         // non-zero spacing indicates we have data
         spacing = grid_spacing;
     } else if (extrapolate && have_current_loc_height) {
         // show the extrapolated height, so logs show what height is
         // being used for navigation
         terrain_height = last_current_loc_height;
-    } else {
-        // report terrain height if we can, but can't give current_height
-        height_amsl(loc, terrain_height);
     }
     uint16_t pending, loaded;
     get_statistics(pending, loaded);
 
     float current_height = 0.0f;
-    if (spacing == 0 && !(extrapolate && have_current_loc_height)) {
-        current_height = 0;
-    } else if (!current_loc.is_zero()) {
-        int32_t height_above_home_cm = 0;
-        UNUSED_RESULT(current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, height_above_home_cm));
-        current_height = height_above_home_cm * 0.01f;  // cm -> m
-    }
-    current_height += home_terrain_height - terrain_height;
+    height_above_terrain(current_height, extrapolate);
 
     if (HAVE_PAYLOAD_SPACE(chan, TERRAIN_REPORT)) {
         mavlink_msg_terrain_report_send(chan, loc.lat, loc.lng, spacing,