diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index c704b14a16..b388942702 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -357,13 +357,6 @@ void Plane::terrain_update(void) { #if AP_TERRAIN_AVAILABLE terrain.update(); - - // tell the rangefinder our height, so it can go into power saving - // mode if available - float height; - if (terrain.height_above_terrain(height, true)) { - rangefinder.set_estimated_terrain_height(height); - } #endif } diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 9c71ddca55..1d47c8cc8c 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -24,6 +24,29 @@ void Plane::init_rangefinder(void) void Plane::read_rangefinder(void) { #if RANGEFINDER_ENABLED == ENABLED + + // notify the rangefinder of our approximate altitude above ground to allow it to power on + // during low-altitude flight when configured to power down during higher-altitude flight + float height; +#if AP_TERRAIN_AVAILABLE + if (terrain.status() == AP_Terrain::TerrainStatusOK && terrain.height_above_terrain(height, true)) { + rangefinder.set_estimated_terrain_height(height); + } else +#endif + { + // use the best available alt estimate via baro above home + if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || + flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { + // ensure the rangefinder is powered-on when land alt is higher than home altitude. + // This is done using the target alt which we know is below us and we are sinking to it + height = height_above_target(); + } else { + // otherwise just use the best available baro estimate above home. + height = relative_altitude(); + } + rangefinder.set_estimated_terrain_height(height); + } + rangefinder.update(); if (should_log(MASK_LOG_SONAR))