fix multi movpos

diff --git a/hostside/TODO b/hostside/TODO
index 8b4c4ca22dafe6b4a21e4b9d0ae09c01d8999d54..4859f14cf71900dd35568dd943434039feb98af4 100644 (file)
--- a/hostside/TODO
+++ b/hostside/TODO
@@ -1,8 +1,5 @@
 bugs
        does not set serial port mode etc.
-       multiple motions handling is wrong: deadline miscalculated etc.
-               deadline should be start deadline and we should sub length
-
 
 want to
        document commands [!]movfeat and speed

diff --git a/hostside/movpos.c b/hostside/movpos.c
index 24f9143c89dee7a4551e1ddeab2d281e6c533f62..4079f5bbb454e671c7df65307c110121730ce149 100644 (file)
--- a/hostside/movpos.c
+++ b/hostside/movpos.c
@@ -199,9 +199,9 @@ static PointQueue pt_confirmed, pt_reserved;
 static void pt_check_action(void);
 static ErrorCode pt_check_plan(void);
 
-static PtSlot pt_maxdelay_reldeadline(int maxdelay_ms) {
+static PtSlotSigned pt_maxdelay_reldeadline(int maxdelay_ms, int n_motions) {
   if (maxdelay_ms==-1) return PT_MAX_QUEUE*16;
-  return (maxdelay_ms - POINT_MOVEMENT + CDU_RECHARGE) / CDU_RECHARGE;
+  return (maxdelay_ms - POINT_MOVEMENT) / CDU_RECHARGE - n_motions;
 }
 
 static void pt_queue_remove_index(PointQueue *q, int index) {
@@ -332,8 +332,11 @@ static Change *point_allocate(int alloc_motions) {
 static ErrorCode point_reserve(Change *chg, Segment *move,
                               int maxdelay_ms) {
   PointReq *r= (PointReq*)chg;
-  r->deadline= pt_maxdelay_reldeadline(maxdelay_ms);
-  if (!r->deadline) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
+  PtSlotSigned reldeadline;
+
+  reldeadline= pt_maxdelay_reldeadline(maxdelay_ms, r->n_motions);
+  if (reldeadline <= 0) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
+  r->deadline= reldeadline;
   return pt_enqueue(&pt_reserved, r);
 }
 
@@ -341,7 +344,7 @@ static ErrorCode point_confirm(Change *chg, Segment *move,
                               int n_motions, const Motion *motions,
                               int maxdelay_ms) {
   PointReq *r= (PointReq*)chg;
-  PtSlot newdeadline;
+  PtSlotSigned reldeadline;
   int allow_failure;
   ErrorCode ec;
 
@@ -360,15 +363,14 @@ static ErrorCode point_confirm(Change *chg, Segment *move,
     return EC_MovFeatReservationInapplicable;
   assert(n_motions <= r->n_motions);
   if (maxdelay_ms == -1) {
-    newdeadline= r->deadline;
-    if (!~newdeadline) newdeadline= pt_maxdelay_reldeadline(-1);
+    reldeadline= r->deadline;
+    if (!~r->deadline) reldeadline= pt_maxdelay_reldeadline(-1, n_motions);
   } else {
-    newdeadline= pt_maxdelay_reldeadline(maxdelay_ms);
+    reldeadline= pt_maxdelay_reldeadline(maxdelay_ms, n_motions);
   }
-  allow_failure= newdeadline < r->deadline;
-  oprintf(DUPO("movpos/point") " newdeadline=[%d@t+%d] allow_failure=%d\n",
-         n_motions, newdeadline, allow_failure);
-  newdeadline += pt_cslot;
+  allow_failure= reldeadline < (PtSlotSigned)r->deadline;
+  oprintf(DUPO("movpos/point") " reldeadline=[%d@t+%d] allow_failure=%d\n",
+         n_motions, reldeadline, allow_failure);
 
   /* state A or R */
   pt_dequeue(r);
@@ -389,7 +391,7 @@ static ErrorCode point_confirm(Change *chg, Segment *move,
   if (!n_motions) r->motions[0].i= move->i->movfeats;
   assert(r->motions[0].i);
   r->n_motions= n_motions;
-  r->deadline= newdeadline + pt_cslot;
+  r->deadline= reldeadline + pt_cslot;
 
   if (n_motions == move->i->n_movfeats)
     r->actual= 0;