From c87929e907d17d6f1a7dd5d753d6b84f29e7d9c5 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Mon, 3 Aug 2015 09:27:59 +1000 Subject: [PATCH] Plane: Fix landing to stop divide by zero if params are 0 If someone mistakenly puts all 0's in their LAND command then total_distance will be calculated as 0 and cause a divide by 0 error below thus crashing ArduPilot. Lets avoid that. --- ArduPlane/landing.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index ca2dd73ea9..9aed3557f5 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -131,6 +131,12 @@ void Plane::setup_landing_glide_slope(void) int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); float total_distance = get_distance(prev_WP_loc, next_WP_loc); + // If someone mistakenly puts all 0's in their LAND command then total_distance + // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. + if (total_distance < 1) { + total_distance = 1; + } + // height we need to sink for this WP float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f;