From 9ca47cf465c132174bb3864b7e4b4cb048128876 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 2 Sep 2024 14:07:05 +0900 Subject: [PATCH] AC_Circle: integrate pos vel accel offsets Co-authored-by: Leonard Hall --- libraries/AC_WPNav/AC_Circle.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 082ec55dc2..cc24c78ad6 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -86,7 +86,7 @@ void AC_Circle::init() _pos_control.init_z_controller_stopping_point(); // get stopping point - const Vector3p& stopping_point = _pos_control.get_pos_target_cm(); + const Vector3p& stopping_point = _pos_control.get_pos_desired_cm(); // set circle center to circle_radius ahead of stopping point _center = stopping_point; @@ -185,7 +185,7 @@ bool AC_Circle::update(float climb_rate_cms) if (_terrain_alt) { target_z_cm = _center.z + terr_offset; } else { - target_z_cm = _pos_control.get_pos_target_z_cm(); + target_z_cm = _pos_control.get_pos_desired_z_cm(); } // if the circle_radius is zero we are doing panorama so no need to update loiter target @@ -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(), _center.tofloat().xy()); + _yaw = get_bearing_cd(_pos_control.get_pos_desired_cm().xy().tofloat(), _center.tofloat().xy()); if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { _yaw += is_positive(_rate)?-9000.0f:9000.0f; @@ -313,12 +313,13 @@ void AC_Circle::init_start_angle(bool use_heading) _angle = wrap_PI(_ahrs.yaw-M_PI); } else { // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading) - const Vector3f &curr_pos = _inav.get_position_neu_cm(); - if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) { + // curr_pos_desired is the position before we add offsets and terrain + const Vector3f &curr_pos_desired= _pos_control.get_pos_desired_cm().tofloat(); + if (is_equal(curr_pos_desired.x,float(_center.x)) && is_equal(curr_pos_desired.y,float(_center.y))) { _angle = wrap_PI(_ahrs.yaw-M_PI); } else { // get bearing from circle center to vehicle in radians - float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x); + float bearing_rad = atan2f(curr_pos_desired.y-_center.y, curr_pos_desired.x-_center.x); _angle = wrap_PI(bearing_rad); } }