From 18974363d5cef2b29275f6adace9c9f3175f4e29 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 24 Jul 2014 16:28:40 +1000
Subject: [PATCH] AP_Terrain: added some helper functions

this adds easier to use helper functions for terrain handling
---
 libraries/AP_Terrain/AP_Terrain.cpp | 31 ++++++++++++++++++++++-------
 libraries/AP_Terrain/AP_Terrain.h   | 12 ++++++++---
 2 files changed, 33 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp
index e3fccaebbe..75d9fd7aae 100644
--- a/libraries/AP_Terrain/AP_Terrain.cpp
+++ b/libraries/AP_Terrain/AP_Terrain.cpp
@@ -145,7 +145,7 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height)
    return false is terrain at the given location or at home
    location is not available
 */
-bool AP_Terrain::height_terrain_difference(const Location &loc, float &terrain_difference)
+bool AP_Terrain::height_terrain_difference_home(const Location &loc, float &terrain_difference)
 {
     float height_home, height_loc;
     if (!height_amsl(ahrs.get_home(), height_home)) {
@@ -170,16 +170,33 @@ bool AP_Terrain::height_terrain_difference(const Location &loc, float &terrain_d
    return false if terrain data is not available either at the given
    location or at the home location.  
 */
-bool AP_Terrain::height_above_terrain(const Location &loc, float relative_altitude, float &terrain_altitude)
+bool AP_Terrain::height_above_terrain(const Location &loc, float relative_home_altitude, float &terrain_altitude)
 {
     float terrain_difference;
-    if (!height_terrain_difference(loc, terrain_difference)) {
+    if (!height_terrain_difference_home(loc, terrain_difference)) {
         return false;
     }
-    terrain_altitude = relative_altitude - terrain_difference;
+    terrain_altitude = relative_home_altitude - terrain_difference;
     return true;
 }
 
+/* 
+   return estimated height above the terrain given a relative-to-home
+   altitude (such as a barometric altitude) for a given location
+       
+   return false if terrain data is not available either at the given
+   location or at the home location.  
+*/
+bool AP_Terrain::height_above_terrain(const Location &loc, float &terrain_altitude)
+{
+    float relative_home_altitude = loc.alt;
+    if (!loc.flags.relative_alt) {
+        // loc.alt has home altitude added, remove it
+        relative_home_altitude -= ahrs.get_home().alt*0.01f;
+    }
+    return height_above_terrain(loc, relative_home_altitude, terrain_altitude);
+}
+
 /* 
    return estimated equivalent relative-to-home altitude in meters of
    a given height above the terrain for a given location.
@@ -192,13 +209,13 @@ bool AP_Terrain::height_above_terrain(const Location &loc, float relative_altitu
    return false if terrain data is not available either at the given
    location or at the home location.  
 */
-bool AP_Terrain::height_relative_equivalent(const Location &loc, float terrain_altitude, float &relative_altitude)
+bool AP_Terrain::height_relative_home_equivalent(const Location &loc, float terrain_altitude, float &relative_home_altitude)
 {
     float terrain_difference;
-    if (!height_terrain_difference(loc, terrain_difference)) {
+    if (!height_terrain_difference_home(loc, terrain_difference)) {
         return false;
     }
-    relative_altitude = terrain_altitude + terrain_difference;
+    relative_home_altitude = terrain_altitude + terrain_difference;
     return true;
 }
 
diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h
index 57f915915d..c43f90a1f3 100644
--- a/libraries/AP_Terrain/AP_Terrain.h
+++ b/libraries/AP_Terrain/AP_Terrain.h
@@ -109,7 +109,7 @@ public:
        return false is terrain at the given location or at home
        location is not available
     */
-    bool height_terrain_difference(const Location &loc, float &terrain_difference);
+    bool height_terrain_difference_home(const Location &loc, float &terrain_difference);
 
     /* 
        return estimated equivalent relative-to-home altitude in meters
@@ -123,7 +123,7 @@ public:
        return false if terrain data is not available either at the given
        location or at the home location.  
     */
-    bool height_relative_equivalent(const Location &loc, float terrain_altitude, float &relative_altitude);
+    bool height_relative_home_equivalent(const Location &loc, float terrain_altitude, float &relative_altitude);
 
     /* 
        return estimated height above the terrain in meters given a
@@ -133,7 +133,13 @@ public:
        return false if terrain data is not available either at the given
        location or at the home location.  
     */
-    bool height_above_terrain(const Location &loc, float relative_altitude, float &terrain_altitude);
+    bool height_above_terrain(const Location &loc, float relative_home_altitude, float &terrain_altitude);
+
+    /* 
+       alternative interface to height_above_terrain where
+       relative_altitude is taken from loc.alt (in centimeters)
+    */
+    bool height_above_terrain(const Location &loc, float &terrain_altitude);
 
 private:
     // allocate the terrain subsystem data