diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 83c1e8f6c6..b8c05c87f7 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -219,9 +219,36 @@ bool AP_Terrain::height_relative_home_equivalent(const Location &loc, float terr return true; } +/* + convert a Location altitude to a relative-to-home altitude in meters + This obeys the relative_alt and terrain_alt flags in Location.flags +*/ +bool AP_Terrain::location_to_relative_home(const Location &loc, float &relative_altitude) +{ + if (!loc.flags.terrain_alt) { + // its not a terrain alt + relative_altitude = loc.alt*0.01f; + if (!loc.flags.relative_alt) { + relative_altitude -= ahrs.get_home().alt*0.01f; + } + return true; + } + + if (!height_relative_home_equivalent(loc, loc.alt*0.01f, relative_altitude)) { + return false; + } + + // if terrain_alt is set and relative_alt is not set then Location + // is still offset by home alt + if (!loc.flags.relative_alt) { + relative_altitude -= ahrs.get_home().alt*0.01f; + } + return true; +} + /* - 1Hz update function. This is here to ensure progress is made on disk + 1hz update function. This is here to ensure progress is made on disk IO even if no MAVLink send_request() operations are called for a while. */ diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index c43f90a1f3..7d4202e313 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -141,6 +141,12 @@ public: */ bool height_above_terrain(const Location &loc, float &terrain_altitude); + /* + convert a Location altitude to a relative-to-home altitude in meters + This obeys the relative_alt and terrain_alt flags in Location.flags + */ + bool location_to_relative_home(const Location &loc, float &relative_altitude); + private: // allocate the terrain subsystem data void allocate(void);