on detection, when stopping, do not include an extra lump of uncertainty in the stopp...

diff --git a/hostside/safety.c b/hostside/safety.c
index 4c1dcd375152e764da8271660bc76d7b70f7bef8..b29971d7e39fd0c5ae4a8b31e55b539a29385897 100644 (file)
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -312,7 +312,7 @@ static void pred_callback_debug(const char *what, TrackLocation *t,
   PredictUserContext *u= c->u;
   
   oprintf(DUPO("safety") " predict   %s"
-         " %s%s dist=%-4d until=%-4ld %c%c%c.%c (was %s%s dist=%-4d)"
+         " %s%s dist=%-4d until=%-4ld %c%c%c.%c (was %s%s..%d dist=%-4d)"
          "  %c%c%c%c.%c%c%c%c"
          " elapsed=%ld nit=%d,%d\n",
          what,
@@ -330,6 +330,8 @@ static void pred_callback_debug(const char *what, TrackLocation *t,
 
          before && before->backwards?"-":"",
          before ? before->seg->i->pname : "-",
+         before ? before->remain : -1,
+
          u->was_distance,
          
          "-t"[ u->count_time ],
@@ -391,8 +393,8 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
   TimeInterval max_ms;
   ErrorCode ec;
 
-  if (!before) return 0;
   pred_callback_debug(" nose_nextseg",t,c,before);
+  if (!before) return 0;
 
   if (u->optimistic)
     advance_elapsed(u,calc_advanced(c));
@@ -494,8 +496,8 @@ static int tail_nextseg(TrackLocation *t, TrackAdvanceContext *c,
                        MovPosComb *mpc_io, const TrackLocation *before) {
   PredictUserContext *u= c->u;
 
-  if (!before) return 0;
   pred_callback_debug(" tail_nextseg",t,c,before);
+  if (!before) return 0;
 
   if (!before->seg->i->invertible)
     u->noninv_tally[before->backwards]--;
@@ -521,8 +523,8 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
    * the previous segment.
    */
 
-  if (!before) return 0;
   pred_callback_debug("fdet_nextseg",t,c,before);
+  if (!before) return 0;
 
   advanced= calc_advanced(c);
 
@@ -654,16 +656,23 @@ ErrorCode predict(Train *tra, int accelerate,
   u.problem_callback= ppc;
   u.problem_callback_u= ppcu;
   u.maxspeed= speedmanager_speed_maxestimate(u.train);
+  u.stopping= speedmanager_stopping(u.train);
   u.stopping_distance= speedmanager_stoppingdistance(u.train);
-  u.stopping= !accelerate && speedmanager_stopping(u.train);
 
-  oprintf(DUPO("safety") " predict starting %s%s maxspeed=%f"
-         " (speed %f try %f, step %d%s) stopdist=%d%s%s\n",
+  switch (accelerate) {
+  case  1:  u.stopping= 0;                                             break;
+  case  0:                                                             break;
+  case -1:  if (u.stopping) u.stopping_distance -= tra->uncertainty;   break;
+  default: abort();
+  }
+
+  oprintf(DUPO("safety") " predict ***starting*** %s%s maxspeed=%f"
+         " (speed %f try %f, step %d%s) stopdist=%d accelerate=%d %s\n",
          tra->backwards?"-":"",tra->pname,
          u.maxspeed, tra->speed.speed, tra->speed.try_speed, tra->speed.step,
          tra->speed.decel.running ? " decel" : "",
          u.stopping_distance,
-         accelerate ? " accel" : "",
+         accelerate,
          u.stopping ? " stopping" : "");
 
   FOR_SEG {
@@ -677,7 +686,6 @@ ErrorCode predict(Train *tra, int accelerate,
   if (!foredet)
     return predict_problem(&u,0,"train is not on layout");
 
-  u.accelerating= accelerate;
   u.walk_compute_polarise= 1;
   u.train_polarity_inverted= foredet->seg_inverted ^ foredet->tr_backwards;
   u.usecurrentposn= 1;
@@ -947,20 +955,19 @@ void safety_notify_detection(Segment *seg) {
   tra->uncertainty= tra->maxinto= stopdist;
   report_train_position(tra);
 
-  ec= predict(tra, 0, detection_report_problem, 0);
+  ec= predict(tra,-1, detection_report_problem, 0);
   if (!ec) return;
 
   assert(ec == EC_SignallingPredictedProblem);
 
-  if (tra->maxinto > stopdist) {
-    tra->uncertainty= tra->maxinto= stopdist;
-    report_train_position(tra);
-  }    
+  if (maxinto > stopdist) maxinto= stopdist;
+  tra->maxinto= tra->uncertainty= maxinto;
 
   report_train_position(tra);
+  speedmanager_safety_stop(tra);
+  ec= predict(tra,-1, 0,(char*)"safety commanding stop");
+  assert(!ec);
 
-  ec= speedmanager_speedchange_request(tra,0, 0,(char*)"detection sigstop");
-   /* that calls predict_confirm with our supplied arguments */
   assert(!ec);
 }
 

diff --git a/hostside/safety.h b/hostside/safety.h
index d256c4d90d9995a6b0fe9a3974c2b475aef0d300..689bd2031e03a2ff775d5365fdab5efade71bd41 100644 (file)
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -115,6 +115,13 @@ ErrorCode predict(Train *tra, int accelerate,
    * Caller must call this with different situations until it succeeds!
    * Caller may pass ppc=0 and ppcu=(char*)"some context" to
    * cause safety_panic if it fails.
+   *
+   * accelerate should be:
+   *   -1  if we have just detected the train entering its foredetect
+   *        so we can subtract the uncertainty from the stopping distance
+   *    0  if whatever we are doing will not disturb the existing plan
+   *    1  if we the existing plan is not necessarily sufficient as
+   *        we are intending to accelerate or something
    */
 
 void report_train_position(Train *tra);
@@ -210,6 +217,7 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step,
                            PredictionProblemCallback *ppc, void *ppcu);
   /* ppc and ppcu as for predict_confirm */
 
+void speedmanager_safety_stop(Train *tra);
 void speedmanager_reset_train(Train *tra);
 double speedmanager_speed_maxestimate(Train *tra);
 double speedmanager_stoppingdistance(Train *tra);
@@ -247,7 +255,7 @@ void actual_inversions_done(void);
 typedef struct TrackLocation TrackLocation;
 struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
   Segment *seg; /* current segment */
-  long remain; /* distance from end of segment as we look at it */
+  Distance remain; /* distance from end of segment as we look at it */
   unsigned backwards:1; /* if 1, into is positive and measured from end */
 };
 

diff --git a/hostside/speed.c b/hostside/speed.c
index 4f0a44c6978e845f4892cc759aca10915f0e81ec..534af0a141de30766380d876297d33e956d33875 100644 (file)
--- a/hostside/speed.c
+++ b/hostside/speed.c
@@ -98,8 +98,8 @@ int speedmanager_stopping(Train *tra) {
   return r;
 }  
 
-ErrorCode speedmanager_speedchange_request(Train *tra, int step,
-                           PredictionProblemCallback *ppc, void *ppcu) {
+static ErrorCode request_core(Train *tra, int step,
+                             PredictionProblemCallback *ppc, void *ppcu) {
   ErrorCode ec, ec2;
   struct timeval tnow;
   double vnow, vtarg;
@@ -123,8 +123,10 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step,
     tra->speed.decel.duration= stop_info(tra,vnow)->ts;
     toev_start(&tra->speed.decel);
     tra->speed.speed= vnow;
-    ec= predict(tra,0, 0,(char*)"deceleration forbidden");
-    assert(!ec);
+    if (ppc || ppcu) {
+      ec= predict(tra,0, 0,(char*)"deceleration forbidden");
+      assert(!ec);
+    }
     xmit(tra);
     return 0;
   }
@@ -147,6 +149,18 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step,
   return 0;
 }
 
+ErrorCode speedmanager_speedchange_request(Train *tra, int step,
+                           PredictionProblemCallback *ppc, void *ppcu) {
+  assert(ppc || ppcu);
+  return request_core(tra,step,ppc,ppcu);
+}
+
+void speedmanager_safety_stop(Train *tra) {
+  ErrorCode ec;
+  ec= request_core(tra,0,0,0);
+  assert(!ec);
+}
+
 void speedmanager_reset_train(Train *tra) {
   Nmra n;
   

diff --git a/hostside/xs.gdb b/hostside/xs.gdb
index c91a039a874b21b5493db0d66f1febe19301b879..5073fc8339c72937a5dd3be82b716de9c31f73e0 100644 (file)
--- a/hostside/xs.gdb
+++ b/hostside/xs.gdb
@@ -1,7 +1,7 @@
 file ./realtime
 break vdie
 break nmra_errchk_fail
-break predict_problem
+#break predict_problem
 break safety_panic
 break obc_error
 #break findhead_nextseg