From 6af705d4554defc27aa475dab99f918b26de3ce1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 16 Feb 2014 11:44:30 +0900 Subject: [PATCH] AC_Circle: bug fix for pos target when radius is zero --- libraries/AC_WPNav/AC_Circle.cpp | 18 ++++++++++++------ libraries/AC_WPNav/AC_Circle.h | 2 ++ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index a9cdb99e00..3f92b7ffca 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -111,9 +111,6 @@ void AC_Circle::update() _angle = wrap_PI(_angle); _angle_total += angle_change; - // heading is 180 deg from vehicles target position around circle - _yaw = wrap_PI(_angle-PI); - // if the circle_radius is zero we are doing panorama so no need to update loiter target if (_radius != 0.0) { // calculate target position @@ -126,10 +123,19 @@ void AC_Circle::update() _pos_control.set_pos_target(target); // heading is 180 deg from vehicles target position around circle - _yaw = wrap_PI(_angle-PI); + _yaw = wrap_PI(_angle-PI) * AC_CIRCLE_DEGX100; }else{ - // heading is 180 deg from vehicles target position around circle - _yaw = wrap_PI(_angle-PI); + // set target position to center + Vector3f target; + target.x = _center.x; + target.y = _center.y; + target.z = _pos_control.get_alt_target(); + + // update position controller target + _pos_control.set_pos_target(target); + + // heading is same as _angle but converted to centi-degrees + _yaw = _angle * AC_CIRCLE_DEGX100; } // trigger position controller on next update diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 6760eb0982..af11227e73 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -12,6 +12,8 @@ #define AC_CIRCLE_RADIUS_DEFAULT 1000.0f // radius of the circle in cm that the vehicle will fly #define AC_CIRCLE_RATE_DEFAULT 20.0f // turn rate in deg/sec. Positive to turn clockwise, negative for counter clockwise +#define AC_CIRCLE_DEGX100 5729.57795f // constant to convert from radians to centi-degrees + class AC_Circle { public: