diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ec19072c1f..8283338ad1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -810,6 +810,7 @@ private: int32_t get_RTL_altitude(); float relative_altitude(void); int32_t relative_altitude_abs_cm(void); + float relative_ground_altitude(void); void set_target_altitude_current(void); void set_target_altitude_current_adjusted(void); void set_target_altitude_location(const Location &loc); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 14d8fa5de7..3924a83eab 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -132,6 +132,20 @@ int32_t Plane::relative_altitude_abs_cm(void) return labs(current_loc.alt - home.alt); } +/* + return relative altitude in meters (relative to terrain, if available, + or home otherwise) + */ +float Plane::relative_ground_altitude(void) +{ +#if AP_TERRAIN_AVAILABLE + float altitude; + if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(altitude, true)) { + return altitude; + } +#endif + return relative_altitude(); +} /* set the target altitude to the current altitude. This is used when