From 44606ca8725469ea8845555e158f3390b1a5b06c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 16 Dec 2020 14:58:35 +0100 Subject: [PATCH] FlightTaskOrbit: further simplify circling yaw setpoint calculation The whole angle adding and pi wrap was just necessary because the correct atan2 arguments were never completely thought through. --- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 4035e4b5e6..723eed1069 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -250,7 +250,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f ¢er_to_po break; case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: - _yaw_setpoint = wrap_pi(atan2f(center_to_position(1), center_to_position(0)) + (sign(_v) * M_PI_F / 2.f)); + _yaw_setpoint = atan2f(sign(_v) * center_to_position(0), -sign(_v) * center_to_position(1)); _yawspeed_setpoint = _v / _r; break; @@ -260,7 +260,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f ¢er_to_po case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER: default: - _yaw_setpoint = wrap_pi(atan2f(center_to_position(1), center_to_position(0)) + M_PI_F); + _yaw_setpoint = atan2f(-center_to_position(1), -center_to_position(0)); // yawspeed feed-forward because we know the necessary angular rate _yawspeed_setpoint = _v / _r; break;