From cd6e5b48a183d9662355fd5e6d13942c5fd33b53 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 May 2019 16:42:18 +1000 Subject: [PATCH] AC_Avoid: move check for zero-desired-velocity into adjust_velocity_polygon This is the method which divides by the length of this vector; the check belongs in here. --- libraries/AC_Avoidance/AC_Avoid.cpp | 20 +++++--------------- 1 file changed, 5 insertions(+), 15 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 02d59010f1..d80b9b3631 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -356,11 +356,6 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2 return; } - // exit immediately if no desired velocity - if (desired_vel_cms.is_zero()) { - return; - } - // get polygon boundary uint16_t num_points; const Vector2f* boundary = _fence.get_boundary_points(num_points); @@ -381,11 +376,6 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f return; } - // exit immediately if no desired velocity - if (desired_vel_cms.is_zero()) { - return; - } - // get boundary from beacons uint16_t num_points; const Vector2f* boundary = _beacon->get_boundary_points(num_points); @@ -418,11 +408,6 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &d return; } - // exit immediately if no desired velocity - if (desired_vel_cms.is_zero()) { - return; - } - // get boundary from proximity sensor uint16_t num_points; const Vector2f *boundary = _proximity.get_boundary_points(num_points); @@ -439,6 +424,11 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des return; } + // exit immediately if no desired velocity + if (desired_vel_cms.is_zero()) { + return; + } + const AP_AHRS &_ahrs = AP::ahrs(); // do not adjust velocity if vehicle is outside the polygon fence