From e49d29f8cdcf0467e4dbeeca1d9ce906f8315d61 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 23 Nov 2017 21:14:52 +0900 Subject: [PATCH] AC_WPNav: protect against divide by zero This could occur if the spline origin and destination were the same location In these cases we mark the vehicle as having reached the destination avoid all calculations --- libraries/AC_WPNav/AC_WPNav.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 15c4b90b01..deb90b327d 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -1093,7 +1093,15 @@ bool AC_WPNav::advance_spline_target_along_track(float dt) // update target position and velocity from spline calculator calc_spline_pos_vel(_spline_time, target_pos, target_vel); - _pos_delta_unit = target_vel/target_vel.length(); + // if target velocity is zero the origin and destination must be the same + // so flag reached destination (and protect against divide by zero) + float target_vel_length = target_vel.length(); + if (is_zero(target_vel_length)) { + _flags.reached_destination = true; + return true; + } + + _pos_delta_unit = target_vel / target_vel_length; calculate_wp_leash_length(); // get current location @@ -1149,10 +1157,7 @@ bool AC_WPNav::advance_spline_target_along_track(float dt) _spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit); // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator - float target_vel_length = target_vel.length(); - if (!is_zero(target_vel_length)) { - _spline_time_scale = _spline_vel_scaler/target_vel_length; - } + _spline_time_scale = _spline_vel_scaler / target_vel_length; // update target position target_pos.z += terr_offset;