X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager_core.c;fp=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager_core.c;h=cc2a58af368ea9df97bf4597c99f1ea53a5c1842;hp=a7473c40668ad2da47532ac42cd80b7a87061b8d;hb=966d2c97e5fda37297568c5e8ed66271e986bcdd;hpb=dcb7cd39f9e3026ac926fa471056bf52f3f4023f

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index a7473c4..cc2a58a 100644
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -504,8 +504,10 @@ void trajectory_manager_circle_event(struct trajectory *traj)
 	 * if we are far, go in the center direction,
 	 * if we are at radius, we want to see the center at 90°
 	 * if we are nearer than radius, angle to center is > 90° */
-	if (v2pol_target.r > traj->target.circle.radius * 2)
-		angle_to_center_rad = 0;
+	if (v2pol_target.r > traj->target.circle.radius) {
+		angle_to_center_rad = traj->target.circle.radius / v2pol_target.r;
+		angle_to_center_rad *= (M_PI / 2);
+	}
 	else {
 		angle_to_center_rad = 1. - (v2pol_target.r /
 					    (2 * traj->target.circle.radius));