Circle Fix

lthall · Sep 16, 2024 · 5c9fae9 · 5c9fae9
1 parent e42581a
commit 5c9fae9
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 1 deletion.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -474,6 +474,9 @@ void AC_PosControl::init_xy_controller_stopping_point()
     get_stopping_point_xy_cm(_pos_target.xy());
     _vel_desired.xy().zero();
     _accel_desired.xy().zero();
+
+    // add back offsets including terrain offset
+    add_offsets_xy();
 }
 
 // relax_velocity_controller_xy - initialise the position controller to the current position and velocity with decaying acceleration.

diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp
@@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms)
         target.y += - _radius * sinf(-_angle);
 
         // heading is from vehicle to center of circle
-        _yaw = get_bearing_cd(_inav.get_position_xy_cm() - _pos_control.get_pos_offset_cm().xy().tofloat(), _center.tofloat().xy());
+        _yaw = get_bearing_cd(_pos_control.get_pos_target_cm().xy().tofloat(), _center.tofloat().xy());
 
         if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
             _yaw += is_positive(_rate)?-9000.0f:9000.0f;