AP_Terrain: height_amsl can correct for non-zero terrain alt at home position

libraries/AP_Terrain/AP_Terrain.cpp

@@ -83,7 +83,7 @@ AP_Terrain::AP_Terrain(AP_AHRS &_ahrs, const AP_Mission &_mission, const AP_Rall
 
   This function costs about 20 microseconds on Pixhawk
  */
-bool AP_Terrain::height_amsl(const Location &loc, float &height)
+bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
 {
     if (!enable || !allocate()) {
         return false;
@@ -93,6 +93,10 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
     if (loc.lat == home_loc.lat &&
         loc.lng == home_loc.lng) {
         height = home_height;
+        // apply correction which assumes home altitude is at terrain altitude
+        if (corrected) {
+            height += (ahrs.get_home().alt * 0.01f) - home_height;
+        }
         return true;
     }
 
@@ -143,6 +147,11 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
         home_loc = loc;
     }
 
+    // apply correction which assumes home altitude is at terrain altitude
+    if (corrected) {
+        height += (ahrs.get_home().alt * 0.01f) - home_height;
+    }
+
     return true;
 }
 
@@ -158,7 +167,7 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
 bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool extrapolate)
 {
     float height_home, height_loc;
-    if (!height_amsl(ahrs.get_home(), height_home)) {
+    if (!height_amsl(ahrs.get_home(), height_home, false)) {
         // we don't know the height of home
         return false;
     }
@@ -169,7 +178,7 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool
         return false;
     }
 
-    if (!height_amsl(loc, height_loc)) {
+    if (!height_amsl(loc, height_loc, false)) {
         if (!extrapolate || !have_current_loc_height) {
             // we don't know the height of the given location
             return false;
@@ -260,7 +269,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
         return 0;
     }
     float base_height;
-    if (!height_amsl(loc, base_height)) {
+    if (!height_amsl(loc, base_height, false)) {
         // we don't know our current terrain height
         return 0;
     }
@@ -274,7 +283,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio)
         climb += climb_ratio * grid_spacing;
         distance -= grid_spacing;
         float height;
-        if (height_amsl(loc, height)) {
+        if (height_amsl(loc, height, false)) {
             float rise = (height - base_height) - climb;
             if (rise > lookahead_estimate) {
                 lookahead_estimate = rise;
@@ -298,12 +307,12 @@ void AP_Terrain::update(void)
 
     // try to ensure the home location is populated
     float height;
-    height_amsl(ahrs.get_home(), height);
+    height_amsl(ahrs.get_home(), height, false);
 
     // update the cached current location height
     Location loc;
     bool pos_valid = ahrs.get_position(loc);
-    bool terrain_valid = height_amsl(loc, height);
+    bool terrain_valid = height_amsl(loc, height, false);
     if (pos_valid && terrain_valid) {
         last_current_loc_height = height;
         have_current_loc_height = true;
@@ -351,7 +360,7 @@ void AP_Terrain::log_terrain_data(DataFlash_Class &dataflash)
     float current_height = 0;
     uint16_t pending, loaded;
 
-    height_amsl(loc, terrain_height);
+    height_amsl(loc, terrain_height, false);
     height_above_terrain(current_height, true);
     get_statistics(pending, loaded);
 

libraries/AP_Terrain/AP_Terrain.h

@@ -103,9 +103,16 @@ public:
     void handle_terrain_check(mavlink_channel_t chan, mavlink_message_t *msg);
     void handle_terrain_data(mavlink_message_t *msg);
 
-    // return terrain height in meters above sea level for a location
+    /*
-    // return false if not available
+      find the terrain height in meters above sea level for a location
-    bool height_amsl(const Location &loc, float &height);
+
+      return false if not available
+
+      if corrected is true then terrain alt is adjusted so that
+      the terrain altitude matches the home altitude at the home location
+      (i.e. we assume home is at the terrain altitude)
+     */
+    bool height_amsl(const Location &loc, float &height, bool corrected);
 
     /* 
        find difference between home terrain height and the terrain

libraries/AP_Terrain/TerrainGCS.cpp

@@ -208,8 +208,8 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc
     uint16_t spacing = 0;
     Location current_loc;
     if (ahrs.get_position(current_loc) &&
-        height_amsl(ahrs.get_home(), home_terrain_height) &&
+        height_amsl(ahrs.get_home(), home_terrain_height, false) &&
-        height_amsl(loc, terrain_height)) {
+        height_amsl(loc, terrain_height, false)) {
         // non-zero spacing indicates we have data
         spacing = grid_spacing;
     } else if (extrapolate && have_current_loc_height) {

libraries/AP_Terrain/TerrainMission.cpp

@@ -87,7 +87,7 @@ void AP_Terrain::update_mission_data(void)
 
         // we have a mission command to check
         float height;
-        if (!height_amsl(cmd.content.location, height)) {
+        if (!height_amsl(cmd.content.location, height, false)) {
             // if we can't get data for a mission item then return and
             // check again next time
             return;
@@ -143,7 +143,7 @@ void AP_Terrain::update_rally_data(void)
         loc.lat = rp.lat;
         loc.lng = rp.lng;
         float height;
-        if (!height_amsl(loc, height)) {
+        if (!height_amsl(loc, height, false)) {
             // if we can't get data for a rally item then return and
             // check again next time
             return;