get position right during safety detection-caused stop; much safety debugging output

diff --git a/hostside/safety.c b/hostside/safety.c
index dd817b9f55ed13b172e4dd785af4f6905020cb11..58bb0853a9016b341bbf9846816ba2961a2322f8 100644 (file)
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -239,7 +239,7 @@ typedef struct {
     walk_compute_polarise:1, /* nose_nextseg still needs to worry */
     need_polarise:1, /* when we commit */
     train_polarity_inverted:1, /* right now, or if know_best, the best */
-    know_best_polarity; /* longest-lasting into the future */
+    know_best_polarity:1; /* longest-lasting into the future */
   TrackLocation nose, fdet, tail;
   TrackAdvanceContext nosec, tailc, fdetc;
   TimeInterval elapsed; /* from now, minimum */
@@ -287,6 +287,37 @@ static ErrorCode predict_problem(PredictUserContext *u, Segment *seg,
   return ec;
 }
 
+static void pred_callback_debug(const char *what, TrackLocation *t,
+                               struct TrackAdvanceContext *c) {
+  PredictUserContext *u= c->u;
+  
+  oprintf(DUPO("safety") " predict   %s"
+         " %s%s until=%ld dist=%d (was %d) %c%c%c.%c  %c%c%c%c.%c%c%c%c"
+         " elapsed=%ld nit=%d,%d\n",
+         what,
+         t->backwards?"-":"", t->seg->i->pname, (long)t->seg->until,
+         c->distance, u->was_distance,
+         
+         "-N"[ t->seg->now_present ],
+         "-P"[ t->seg->pred_present ],
+         "-V"[ t->seg->pred_vacated ],
+         
+         "-#"[ t->seg->will_polarise ],
+         
+         "-t"[ u->count_time ],
+         "-a"[ u->accelerating ],
+         "-c"[ u->usecurrentposn ],
+         "-f"[ u->forceusemotions ],
+         
+         "-w"[ u->walk_compute_polarise ],
+         "-n"[ u->need_polarise ],
+         "-i"[ u->train_polarity_inverted ],
+         "-b"[ u->know_best_polarity ],
+
+         (long)u->elapsed,
+         u->noninv_tally[0], u->noninv_tally[1]);
+}
+
 /*---------- prediction trivial callbacks ----------*/
 
 static int pred_getmovpos(TrackLocation *t, TrackAdvanceContext *c,
@@ -329,6 +360,8 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
   TimeInterval max_ms;
   ErrorCode ec;
 
+  pred_callback_debug(" nose_nextseg",t,c);
+
   /* Is it empty ? */
 
   if (speedmanager_stopping(u->train) && t->seg->owner != u->train)
@@ -420,6 +453,8 @@ static int tail_nextseg(TrackLocation *t, TrackAdvanceContext *c,
                        MovPosComb *mpc_io, Segment *leaving) {
   PredictUserContext *u= c->u;
 
+  pred_callback_debug(" tail_nextseg",t,c);
+
   u->noninv_tally[leaving->tr_backwards]--;
 
   if (t->seg->pred_vacated) return 0; /* only vacate once */
@@ -443,6 +478,8 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
    * the previous segment.
    */
 
+  pred_callback_debug("fdet_nextseg",t,c);
+
   advanced= u->was_distance - c->distance;
 
   if (u->count_time) {
@@ -453,6 +490,7 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
   r= trackloc_advance(&u->nose,&u->nosec);
   if (r == EC_SignallingHorizonReached &&
       u->was_distance==TL_DIST_INF) {
+    oprintf(DUPO("safety") " predict   fdet_nextseg  horizon\n");
     /* Our very first `next segment' lookahead found the end.  So we
      * know that our nose hasn't left this segment because that's what
      * the stopping distance is supposed to prove. */
@@ -462,7 +500,10 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
     u->train->maxinto -= adjust;
     u->train->uncertainty += adjust;
   }
-  if (r) return r;
+  if (r) {
+    oprintf(DUPO("safety") " predict   fdet_nextseg  r=%s\n",ec2str(r));
+    return r;
+  }
 
   /* Now we have advanced the nose and have recorded any appropriate
    * motion(s).  But we advancing the nose has updated the segment's
@@ -520,8 +561,11 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
 
   /* Final adjustments, prepare for next iteration */
 
-  if (u->was_distance == TL_DIST_INF)
+  if (u->was_distance == TL_DIST_INF) {
     c->distance= speedmanager_stoppingdistance(u->train);
+    oprintf(DUPO("safety") " predict   stoppingdistance=%d\n",
+           c->distance);
+  }
 
   u->was_distance= c->distance;
   u->count_time= 1; /* we start counting time only after we've done the
@@ -619,7 +663,15 @@ ErrorCode predict(Train *tra, int accelerate,
   u.problem_callback= ppc;
   u.problem_callback_u= ppcu;
 
+  oprintf(DUPO("safety") " predict starting %s%s speed=%f (try %f, step %d%s)"
+         " accel=%d\n",
+         tra->backwards?"-":"",tra->pname,
+         tra->speed.speed, tra->speed.try_speed, tra->speed.step,
+         tra->speed.decel.running ? " decel" : "",
+         accelerate);
+
   FOR_SEG {
+    seg->det_expected= 0;
     seg->now_present= seg->pred_present=
       seg->pred_vacated= seg->will_polarise= 0;
   }
@@ -691,7 +743,7 @@ ErrorCode predict(Train *tra, int accelerate,
 
   FOR_SEG {
     if (seg->owner == tra) {
-      seg->det_ignore= seg->det_expected= 0;
+      seg->det_ignore= 0;
       seg->owner= 0;
     }
     if (seg->pred_present || seg->pred_vacated) {
@@ -721,6 +773,8 @@ ErrorCode predict(Train *tra, int accelerate,
   return 0;
 
  xproblem:
+  oprintf(DUPO("safety") " predict  returning %s\n", ec2str(ec));
+
   FOR_SEG {
     seg->now_present= seg->pred_present=
       seg->pred_vacated= seg->will_polarise= 0;
@@ -764,6 +818,8 @@ void safety_notify_detection(Segment *seg) {
 
   assert(ec == EC_SignallingPredictedProblem);
 
+  tra->maxinto= tra->uncertainty= 1;
+
   ec= speedmanager_speedchange_request(tra,0, 0,(char*)"detection sigstop");
    /* that calls predict_confirm with our supplied arguments */
   assert(!ec);

diff --git a/hostside/speed.c b/hostside/speed.c
index d29937895372ab51c6399d4bfddf670b758ce14c..d32e73ad418bbb4795190bae6f43bf640304353a 100644 (file)
--- a/hostside/speed.c
+++ b/hostside/speed.c
@@ -28,8 +28,13 @@ static const SpeedRange *stop_info(Train *tra, double speed) {
 static double current_speed(Train *tra, const struct timeval tnow) {
   double v1, v2;
   double v1sq, v2sq, vtsq;
-  double left_to_go, ts_v2;
+  double left_to_go, ts_v2, cs;
   
+  oprintf(DUPO("speed") "  current? %s try=%f decel.running=%d speed=%f"
+         " step=%d\n",
+         tra->pname, tra->speed.try_speed,
+         tra->speed.decel.running, tra->speed.speed, tra->speed.step);
+
   if (tra->speed.try_speed >= 0) return tra->speed.try_speed;
   if (!tra->speed.decel.running) return tra->speed.speed;
 
@@ -37,6 +42,7 @@ static double current_speed(Train *tra, const struct timeval tnow) {
               (tra->speed.decel.abs.tv_usec - tnow.tv_usec) * 0.001;
 
   if (left_to_go <= 0) {
+    oprintf(DUPO("speed") "  current?  decel-done\n");
     toev_stop(&tra->speed.decel);
     decel_done(&tra->speed.decel);
     return tra->speed.speed;
@@ -50,7 +56,11 @@ static double current_speed(Train *tra, const struct timeval tnow) {
   v1sq= v1*v1;
   v2sq= v2*v2;
   vtsq= v1sq + (v2sq-v1sq) * left_to_go / ts_v2;
-  return sqrt(vtsq);
+  cs= sqrt(vtsq);
+  oprintf(DUPO("speed") "  current?  decel-inprogress v2=%f v1=%f"
+         " ts_v2=%f ltg=%f  returning %f\n",
+         v2,v1, ts_v2, left_to_go, cs);
+  return cs;
 }
 
 double speedmanager_speed_maxestimate(Train *tra) {
@@ -80,7 +90,10 @@ double speedmanager_stoppingdistance(Train *tra) {
 }
 
 int speedmanager_stopping(Train *tra) {
-  return tra->speed.try_speed < 0 && !tra->speed.speed;
+  int r;
+  r= tra->speed.try_speed < 0 && !tra->speed.step;
+  oprintf(DUPO("speed") "  stopping? %s  returning %d\n", tra->pname, r);
+  return r;
 }  
 
 ErrorCode speedmanager_speedchange_request(Train *tra, int step,
@@ -89,6 +102,10 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step,
   struct timeval tnow;
   double vnow, vtarg;
 
+  oprintf(DUPO("speed") " request %s%s step%d\n",
+         tra->backwards?"-":"",tra->pname,
+         step);
+
   if (step == tra->speed.step)
     return 0;
 
@@ -96,6 +113,8 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step,
   vnow= current_speed(tra,tnow);
   vtarg= tra->speedcurve[step];
   
+  oprintf(DUPO("speed") " request  vnow=%f vtarg=%f\n", vnow,vtarg);
+
   if (vtarg <= vnow) {
     toev_stop(&tra->speed.decel);
     tra->speed.step= step;