From e9c23b538e27fa20c74ec9ed250c668ff1007507 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 9 Mar 2018 11:40:56 +0900
Subject: [PATCH] AC_Circle: run horizontal position control at main loop rate

---
 libraries/AC_WPNav/AC_Circle.cpp | 102 +++++++++++++++----------------
 1 file changed, 48 insertions(+), 54 deletions(-)

diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp
index 5feefc8310..20f3764358 100644
--- a/libraries/AC_WPNav/AC_Circle.cpp
+++ b/libraries/AC_WPNav/AC_Circle.cpp
@@ -111,61 +111,55 @@ void AC_Circle::update()
 {
     // calculate dt
     float dt = _pos_control.time_since_last_xy_update();
-
-    // update circle position at poscontrol update rate
-    if (dt >= _pos_control.get_dt_xy()) {
-
-        // double check dt is reasonable
-        if (dt >= 0.2f) {
-            dt = 0.0f;
-        }
-
-        // ramp angular velocity to maximum
-        if (_angular_vel < _angular_vel_max) {
-            _angular_vel += fabsf(_angular_accel) * dt;
-            _angular_vel = MIN(_angular_vel, _angular_vel_max);
-        }
-        if (_angular_vel > _angular_vel_max) {
-            _angular_vel -= fabsf(_angular_accel) * dt;
-            _angular_vel = MAX(_angular_vel, _angular_vel_max);
-        }
-
-        // update the target angle and total angle traveled
-        float angle_change = _angular_vel * dt;
-        _angle += angle_change;
-        _angle = wrap_PI(_angle);
-        _angle_total += angle_change;
-
-        // if the circle_radius is zero we are doing panorama so no need to update loiter target
-        if (!is_zero(_radius)) {
-            // calculate target position
-            Vector3f target;
-            target.x = _center.x + _radius * cosf(-_angle);
-            target.y = _center.y - _radius * sinf(-_angle);
-            target.z = _pos_control.get_alt_target();
-
-            // update position controller target
-            _pos_control.set_xy_target(target.x, target.y);
-
-            // heading is 180 deg from vehicles target position around circle
-            _yaw = wrap_PI(_angle-M_PI) * DEGX100;
-        }else{
-            // 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_xy_target(target.x, target.y);
-
-            // heading is same as _angle but converted to centi-degrees
-            _yaw = _angle * DEGX100;
-        }
-
-        // update position controller
-        _pos_control.update_xy_controller(1.0f);
+    if (dt >= 0.2f) {
+        dt = 0.0f;
     }
+
+    // ramp angular velocity to maximum
+    if (_angular_vel < _angular_vel_max) {
+        _angular_vel += fabsf(_angular_accel) * dt;
+        _angular_vel = MIN(_angular_vel, _angular_vel_max);
+    }
+    if (_angular_vel > _angular_vel_max) {
+        _angular_vel -= fabsf(_angular_accel) * dt;
+        _angular_vel = MAX(_angular_vel, _angular_vel_max);
+    }
+
+    // update the target angle and total angle traveled
+    float angle_change = _angular_vel * dt;
+    _angle += angle_change;
+    _angle = wrap_PI(_angle);
+    _angle_total += angle_change;
+
+    // if the circle_radius is zero we are doing panorama so no need to update loiter target
+    if (!is_zero(_radius)) {
+        // calculate target position
+        Vector3f target;
+        target.x = _center.x + _radius * cosf(-_angle);
+        target.y = _center.y - _radius * sinf(-_angle);
+        target.z = _pos_control.get_alt_target();
+
+        // update position controller target
+        _pos_control.set_xy_target(target.x, target.y);
+
+        // heading is 180 deg from vehicles target position around circle
+        _yaw = wrap_PI(_angle-M_PI) * DEGX100;
+    } else {
+        // 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_xy_target(target.x, target.y);
+
+        // heading is same as _angle but converted to centi-degrees
+        _yaw = _angle * DEGX100;
+    }
+
+    // update position controller
+    _pos_control.update_xy_controller(1.0f);
 }
 
 // get_closest_point_on_circle - returns closest point on the circle