Fix pretty much wholly buggy speed change logic

diff --git a/hostside/safety.c b/hostside/safety.c
index 9838a22f31666e4046c2f6481febd16d2e8b3235..bdcdbfe35c7c6c0c4385e5b7ab6b061c72640821 100644 (file)
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -263,6 +263,7 @@ Segment *segments;
 typedef struct {
   Train *train;
   unsigned
+    speed_info_specified:1,
     neednewplan:1,
     stopping:1,
     done_first_new_fdet:1,
@@ -708,6 +709,7 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
   u.problem_callback= ppc;
   u.problem_callback_u= ppcu;
   u.neednewplan= !(flags & PREDF_OLDPLAN);
+  u.speed_info_specified= !!speed_info;
 
   if (!speed_info) {
     speedmanager_getinfo(tra,tnow,&speed_info_buf);
@@ -720,13 +722,15 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
   u.desire_move= desire_move;
   u.desire_movposcomb= desire_movposcomb;
 
-  DPRINTF(safety,core," ***predicting*** %s%s maxspeed=%f"
-         " (speed %f try %f, step %d%s) stopdist=%d flags=%c%c%s"
+  DPRINTF(safety,core," ***predicting*** %s%s %smaxspeed=%f%s stopdist=%d"
+         " (speed %f, step %d%s) flags=%c%c%s"
          " desire=%s/%s\n",
          tra->backwards?"-":"",tra->pname,
-         u.maxspeed, tra->speed.speed, tra->speed.try_speed, tra->speed.step,
-         tra->speed.decel.running ? " decel" : "",
+         u.speed_info_specified ? "specified " : "",
+         u.maxspeed, u.stopping ? "(stopping)" : "",
          u.stopping_distance,
+         tra->speed.speed, tra->speed.step,
+         tra->speed.decel.running ? " decel" : "",
          "-j"[ !!(flags & PREDF_JUSTDET) ],
          "no"[ !!(flags & PREDF_OLDPLAN) ],
          u.stopping ? " stopping" : "",
@@ -1224,9 +1228,6 @@ void safety_notify_detection(Segment *seg) {
 
   report_train_position(tra);
   speedmanager_safety_stop(tra,tnow);
-  ec= predict(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN,0,0,
-             &speed_info, 0,(char*)"safety commanding stop");
-  assert(!ec);
 }
 
 ErrorCode safety_movposchange(Segment *seg, MovPosComb comb,

diff --git a/hostside/safety.h b/hostside/safety.h
index fe48cd9e031950d21a32649ddbb773d4c7545869..abb3bacb565bb714e1cdd5077846481883789029 100644 (file)
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -54,7 +54,6 @@ struct Train {
     int step; /* commanded */
     TimeoutEvent decel; /* running iff decelerating */
     double speed; /* iff decerating, at last change; otherwise accurate */
-    double try_speed; /* during speedchange_request only, otherwise < 0 */
     RetransmitUrgentNode rn;
   } speed;
 

diff --git a/hostside/speed.c b/hostside/speed.c
index ae42bd7ee58fbefbf0348ee50f755802dfe74048..e365f6f5119d2f0d114c07b1a2ab2cc75755820b 100644 (file)
--- a/hostside/speed.c
+++ b/hostside/speed.c
@@ -37,12 +37,10 @@ static double current_speed(Train *tra, const struct timeval tnow) {
   double v1sq, v2sq, vtsq;
   double left_to_go, ts_v2, cs;
   
-  DPRINTF(speed,query, "current? %s try=%f decel.running=%d speed=%f"
-         " step=%d\n",
-         tra->pname, tra->speed.try_speed,
+  DPRINTF(speed,query, "current? %s decel.running=%d speed=%f"
+         " step=%d\n", tra->pname, 
          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;
 
   left_to_go= (tra->speed.decel.abs.tv_sec  - tnow.tv_sec) * 1000.0 +
@@ -70,14 +68,12 @@ static double current_speed(Train *tra, const struct timeval tnow) {
   return cs;
 }
 
-static double stoppingdistance(Train *tra, struct timeval tnow, double v) {
+static double stoppingdistance(Train *tra, struct timeval tnow,
+                              const SpeedRange *regime, double v) {
   double xs;
-  const SpeedRange *regime;
 
   if (v==0) return 0;
 
-  regime= stop_info(tra,v);
-
   xs= tra->speedregimes[tra->n_speedregimes-1].xs;
 
   double v_ts_vcrit= v * regime->ts;  if (xs > v_ts_vcrit) xs= v_ts_vcrit;
@@ -90,8 +86,8 @@ void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo *ir) {
   double vnow= current_speed(tra,tnow);
 
   ir->max_speed_estimate= vnow * MARGIN_SPEED;
-  ir->stopping_distance= stoppingdistance(tra,tnow,vnow);
-  ir->stopping= tra->speed.try_speed < 0 && !tra->speed.step;
+  ir->stopping_distance= stoppingdistance(tra,tnow, stop_info(tra,vnow), vnow);
+  ir->stopping= !tra->speed.step;
 
   DPRINTF(speed,query, "getinfo? %s  max=%f stopdist=%d %c\n", tra->pname,
          ir->max_speed_estimate, ir->stopping_distance,
@@ -106,6 +102,8 @@ static ErrorCode request_core(Train *tra, int step, struct timeval tnow,
                              PredictionProblemCallback *ppc, void *ppcu) {
   ErrorCode ec, ec2;
   double vnow, vtarg;
+  SpeedInfo try;
+  const SpeedRange *regime;
 
   DPRINTF(speed,core, "request %s%s step %d\n",
          tra->backwards?"-":"",tra->pname,
@@ -119,36 +117,30 @@ static ErrorCode request_core(Train *tra, int step, struct timeval tnow,
   
   DPRINTF(speed,core, " request  vnow=%f vtarg=%f\n", vnow,vtarg);
 
-  if (vtarg <= vnow) {
-    toev_stop(&tra->speed.decel);
-    tra->speed.step= step;
-    tra->speed.decel.duration= stop_info(tra,vnow)->ts;
-    toev_start(&tra->speed.decel);
-    tra->speed.speed= vnow;
-    if (ppc || ppcu) {
-      ec= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,
-                 0,(char*)"deceleration forbidden");
-      assert(!ec);
-    }
-    xmit(tra);
-    return 0;
-  }
-
-  tra->speed.try_speed= vtarg;
-  ec= predict(tra,tnow, 0, 0,0, 0, ppc,ppcu);
+  try.max_speed_estimate= MAX(vtarg,vnow);
+  regime= stop_info(tra,try.max_speed_estimate);
+  try.stopping_distance= stoppingdistance(tra,tnow, regime,
+                                         try.max_speed_estimate);
+  try.stopping= !step;
 
+  ec= predict(tra,tnow,
+             step <= tra->speed.step ? PREDF_OLDPLAN : 0,
+             0,0, &try, ppc,ppcu);
   if (ec) {
-    tra->speed.try_speed= -1;
     ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,
                 0,(char*)"abandoned acceleration");
     assert(!ec2);
     return ec;
   }
 
-  tra->speed.step= step;
   toev_stop(&tra->speed.decel);
-  tra->speed.speed= vtarg;
-  tra->speed.try_speed= -1;
+  tra->speed.step= step;
+  tra->speed.speed= try.max_speed_estimate;
+
+  if (vtarg <= vnow) {
+    tra->speed.decel.duration= regime->ts;
+    toev_start(&tra->speed.decel);
+  }
   xmit(tra);
   return 0;
 }
@@ -165,7 +157,7 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step,
 
 void speedmanager_safety_stop(Train *tra, struct timeval tnow) {
   ErrorCode ec;
-  ec= request_core(tra,0,tnow,0,0);
+  ec= request_core(tra,0,tnow,0,(char*)"emergency stop");
   assert(!ec);
 }
 
@@ -176,7 +168,6 @@ void speedmanager_reset_train(Train *tra) {
   tra->speed.decel.pclass= "decel";
   tra->speed.decel.pinst= tra->pname;
   tra->speed.speed= 0;
-  tra->speed.try_speed= -1;
   if (tra->addr < 0)
     return;