From b3f182157c438a27570452c321641a87f6b15f98 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 3 Jun 2016 14:10:26 -0700 Subject: [PATCH] Plane: take advantage of rangefinder support in relative_ground_altitude() --- ArduPlane/Plane.h | 2 +- ArduPlane/altitude.cpp | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index eb594bb05a..1dac87e435 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -826,7 +826,7 @@ private: int32_t get_RTL_altitude(); float relative_altitude(void); int32_t relative_altitude_abs_cm(void); - float relative_ground_altitude(void); + float relative_ground_altitude(bool use_rangefinder_if_available); 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 b9acf502fc..05cff9d781 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -137,8 +137,12 @@ int32_t Plane::relative_altitude_abs_cm(void) return relative altitude in meters (relative to terrain, if available, or home otherwise) */ -float Plane::relative_ground_altitude(void) +float Plane::relative_ground_altitude(bool use_rangefinder_if_available) { + if (use_rangefinder_if_available && rangefinder_state.in_range) { + return rangefinder_state.height_estimate; + } + #if AP_TERRAIN_AVAILABLE float altitude; if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(altitude, true)) {