diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
index fb4495dfd8..684dd0bcda 100644
--- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
@@ -245,15 +245,8 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_po
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
-		if (_r > 0) {
-			_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F / 2.f;
-
-		} else {
-			_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) - M_PI_F / 2.f;
-		}
-
+		_yaw_setpoint = wrap_pi(atan2f(center_to_position(1), center_to_position(0)) + (sign(_v) * M_PI_F / 2.f));
 		_yawspeed_setpoint = _v / _r;
-
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
@@ -262,7 +255,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_po
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
 	default:
-		_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+		_yaw_setpoint = wrap_pi(atan2f(center_to_position(1), center_to_position(0)) + M_PI_F);
 		// yawspeed feed-forward because we know the necessary angular rate
 		_yawspeed_setpoint = _v / _r;
 		break;