From 1ebe0a25835c39cee2568d3cb18c65805405991c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Apr 2016 12:06:57 +1000 Subject: [PATCH] Plane: prevent a division by zero in takeoff code --- ArduPlane/takeoff.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 768ac9ca4f..822239dde2 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -165,13 +165,15 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) } // are we entering the region where we want to start leveling off before we reach takeoff alt? - float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate); - if (sec_to_target > 0 && - relative_alt_cm >= 1000 && - sec_to_target <= g.takeoff_pitch_limit_reduction_sec) { - // make a note of that altitude to use it as a start height for scaling - gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100); - auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm; + if (auto_state.sink_rate < -0.1f) { + float sec_to_target = (remaining_height_to_target_cm * 0.01f) / (-auto_state.sink_rate); + if (sec_to_target > 0 && + relative_alt_cm >= 1000 && + sec_to_target <= g.takeoff_pitch_limit_reduction_sec) { + // make a note of that altitude to use it as a start height for scaling + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", remaining_height_to_target_cm/100); + auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm; + } } } return auto_state.takeoff_pitch_cd;