From c03e50660c2474730225bf014c730695f91ad1a1 Mon Sep 17 00:00:00 2001 From: "liang.tang" <466175335@qq.com> Date: Tue, 10 Jul 2018 14:54:57 +0800 Subject: [PATCH] AC_WPNav: check validity of kP --- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b0d18df3e0..0d9f9d2951 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -386,7 +386,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) // calculate point at which velocity switches from linear to sqrt float linear_velocity = _wp_speed_cms; float kP = _pos_control.get_pos_xy_p().kP(); - if (kP >= 0.0f) { // avoid divide by zero + if (is_positive(kP)) { // avoid divide by zero linear_velocity = _track_accel/kP; }