From 43df1088ab350df48bf9e03ee753c62e22da6002 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 26 Feb 2020 14:23:23 +1100 Subject: [PATCH] Copter: rewrite_get_alt_above_ground_cm for clarity It's not entirely clear at a glance that we don't return an uninitialised value off the stack here. --- ArduCopter/mode.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 0c2c2ee5f141d..91726d2d6cf82 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -508,14 +508,19 @@ void Mode::make_safe_spool_down() */ int32_t Mode::get_alt_above_ground_cm(void) { - int32_t alt_above_ground; - if (!copter.get_rangefinder_height_interpolated_cm(alt_above_ground)) { - bool navigating = pos_control->is_active_xy(); - if (!navigating || !copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) { - alt_above_ground = copter.current_loc.alt; - } + int32_t alt_above_ground_cm; + if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) { + return alt_above_ground_cm; + } + if (!pos_control->is_active_xy()) { + return copter.current_loc.alt; } - return alt_above_ground; + if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) { + return alt_above_ground_cm; + } + + // Assume the Earth is flat: + return copter.current_loc.alt; } void Mode::land_run_vertical_control(bool pause_descent)