From 37a7635c66233f6eafe13862a27405cc74db3f60 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 3 Oct 2024 10:07:52 +0900 Subject: [PATCH] AC_Loiter: updates to offset handling --- libraries/AC_WPNav/AC_Loiter.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 6e00781855..3b7f9690a8 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -107,7 +107,7 @@ void AC_Loiter::init_target(const Vector2f& position) _brake_accel = 0.0f; // set target position - _pos_control.set_pos_target_xy_cm(position.x, position.y); + _pos_control.set_pos_desired_xy_cm(position); } /// initialize's position and feed-forward velocity from current pos and velocity @@ -221,12 +221,10 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) } // get loiters desired velocity from the position controller where it is being stored. - const Vector3f &desired_vel_3d = _pos_control.get_vel_desired_cms(); - Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y}; + Vector2f desired_vel = _pos_control.get_vel_desired_cms().xy(); // update the desired velocity using our predicted acceleration - desired_vel.x += _predicted_accel.x * dt; - desired_vel.y += _predicted_accel.y * dt; + desired_vel += _predicted_accel * dt; Vector2f loiter_accel_brake; float desired_speed = desired_vel.length(); @@ -261,8 +259,7 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) // Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing) float horizSpdDem = desired_vel.length(); if (horizSpdDem > gnd_speed_limit_cms) { - desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem; - desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem; + desired_vel = desired_vel * gnd_speed_limit_cms / horizSpdDem; } #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -279,11 +276,11 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) #endif // !APM_BUILD_ArduPlane // get loiters desired velocity from the position controller where it is being stored. - Vector2p target_pos = _pos_control.get_pos_target_cm().xy(); + Vector2p desired_pos = _pos_control.get_pos_desired_cm().xy(); - // update the target position using our predicted velocity - target_pos += (desired_vel * dt).topostype(); + // update the desired position using our desired velocity and acceleration + desired_pos += (desired_vel * dt).topostype(); // send adjusted feed forward acceleration and velocity back to the Position Controller - _pos_control.set_pos_vel_accel_xy(target_pos, desired_vel, _desired_accel); + _pos_control.set_pos_vel_accel_xy(desired_pos, desired_vel, _desired_accel); }