vt100: include pgmspace.h as we use PROGMEM macro

[aversive.git] / modules / devices / robot / trajectory_manager / trajectory_manager_core.c

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index c71def0..6b3c172 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -191,8 +191,6 @@ void trajectory_stop(struct trajectory *traj)
                                  UPDATE_A | UPDATE_D | RESET_D | RESET_A);
 }
 
-//#include <stackdump.h>
-
 /** set relative angle and distance consign to 0, and break any
  * deceleration ramp in quadramp filter */
 void trajectory_hardstop(struct trajectory *traj)
@@ -320,8 +318,25 @@ uint8_t trajectory_distance_finished(struct trajectory *traj)
  * distance. */
 uint8_t trajectory_finished(struct trajectory *traj)
 {
-       return trajectory_angle_finished(traj) &&
-               trajectory_distance_finished(traj);
+       uint8_t flags, ret;
+       //      uint8_t ret2;
+
+       IRQ_LOCK(flags);
+       ret = trajectory_distance_finished(traj) &&
+               trajectory_angle_finished(traj);
+       IRQ_UNLOCK(flags);
+
+#if 0
+       /* XXX THIS IS A VERY BAD WORKAROUND (fix a race) */
+       IRQ_LOCK(flags);
+       ret2 = trajectory_distance_finished(traj) &&
+               trajectory_angle_finished(traj);
+       IRQ_UNLOCK(flags);
+
+       return ret && ret2;
+#else
+       return ret;
+#endif
 }
 
 /** return true if traj is nearly finished */
@@ -1057,7 +1072,8 @@ int8_t trajectory_clitoid(struct trajectory *traj,
        if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm,
                         Vd, traj->a_acc, d_inter_mm,
                         &Aa, &Va, &remain) < 0) {
-               DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error");
+               DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error",
+                     __FUNCTION__);
                return -1;
        }