From c2a290b2c32c2369fe58ff518bf4274c59743b73 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 14 Mar 2017 20:50:32 +0900 Subject: [PATCH] AC_PosControl: protect against POS_Z_P, ACCEL_Z_P divide-by-zero --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 03d5d20902..759951d946 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -288,6 +288,12 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const } } + // avoid divide by zero by using current position if kP is very low or acceleration is zero + if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) { + stopping_point.z = curr_pos_z; + return; + } + // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function linear_velocity = _accel_z_cms/_p_pos_z.kP();