hostside/speed.c

   1 /*
   2  * realtime
   3  * speed manager
   4  */
   5
   6 #include "realtime.h"
   7 #include "nmra.h"
   8
   9 void actual_speed(Train *tra, int step) {
  10   Nmra n;
  11   enco_nmra_speed126(&n, tra->addr, step, tra->backwards);
  12   ouprintf("train %s speed commanding %d\n", tra->pname, step);
  13   retransmit_urgent_requeue(&tra->speed.rn, &n);
  14 }
  15
  16 static void xmit(Train *tra) {
  17   actual_speed(tra,tra->speed.step);
  18 }
  19
  20 static void decel_done(TimeoutEvent *toev) {
  21   Train *tra= (void*)((char*)toev - offsetof(Train, speed.decel));
  22   struct timeval tnow;
  23   ErrorCode ec;
  24
  25   tra->speed.speed= tra->speedcurve[tra->speed.step];
  26
  27   DPRINTF(speed,core, "decel_done %s step %d speed %f\n",
  28           tra->pname, tra->speed.step, tra->speed.speed);
  29   mgettimeofday(&tnow);
  30
  31   ec= predict(tra,tnow, PREDF_OLDPLAN, 0,0, 0, 0,(char*)"deceleration done");
  32   assert(!ec);
  33 }
  34
  35 static const SpeedRange *stop_info(Train *tra, double speed) {
  36   int i;
  37   for (i=0; i<tra->n_speedregimes; i++)
  38     if (speed <= tra->speedregimes[i].speed + UNMARGIN_ROUNDING)
  39       return &tra->speedregimes[i];
  40   abort();
  41 }
  42
  43 static double current_speed(Train *tra, const struct timeval tnow) {
  44   double v1, v2;
  45   double v1sq, v2sq, vtsq;
  46   double left_to_go, ts_v2, cs;
  47
  48   DPRINTF(speed,query, "current? %s decel.running=%d speed=%f"
  49           " step=%d\n", tra->pname,
  50           tra->speed.decel.running, tra->speed.speed, tra->speed.step);
  51
  52   if (!tra->speed.decel.running) return tra->speed.speed;
  53
  54   left_to_go= (tra->speed.decel.abs.tv_sec  - tnow.tv_sec) * 1000.0 +
  55               (tra->speed.decel.abs.tv_usec - tnow.tv_usec) * 0.001;
  56
  57   if (left_to_go <= 0) {
  58     DPRINTF(speed,query, "current?  decel-done\n");
  59     /* we don't cancel the timeout because we need the callback to lay out
  60      * the train again at the lower speed */
  61     return tra->speed.speed;
  62   }
  63
  64   v2= tra->speed.speed;
  65   v1= tra->speedcurve[tra->speed.step];
  66   assert(v2 >= v1);
  67
  68   ts_v2= stop_info(tra,v2)->ts;
  69   v1sq= v1*v1;
  70   v2sq= v2*v2;
  71   vtsq= v1sq + (v2sq-v1sq) * left_to_go / ts_v2;
  72   cs= sqrt(vtsq);
  73   DPRINTF(speed,query, "current?  decel-inprogress v2=%f v1=%f"
  74           " ts_v2=%f ltg=%f  returning %f\n",
  75           v2,v1, ts_v2, left_to_go, cs);
  76   return cs;
  77 }
  78
  79 static double stoppingdistance(Train *tra, struct timeval tnow,
  80                                const SpeedRange *regime, double v) {
  81   double xs;
  82
  83   if (v==0) return 0;
  84
  85   xs= tra->speedregimes[tra->n_speedregimes-1].xs;
  86
  87   double v_ts_vcrit= v * regime->ts;  if (xs > v_ts_vcrit) xs= v_ts_vcrit;
  88   double xs_vcrit= regime->xs;        if (xs > xs_vcrit)   xs= xs_vcrit;
  89
  90   return xs + v * MARGIN_STOPTIME;
  91 }
  92
  93 void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo *ir) {
  94   double vnow= current_speed(tra,tnow);
  95
  96   ir->max_speed_estimate= vnow * MARGIN_SPEED;
  97   ir->stopping_distance= stoppingdistance(tra,tnow, stop_info(tra,vnow), vnow);
  98   ir->stopping= !tra->speed.step;
  99
 100   DPRINTF(speed,query, "getinfo? %s  max=%f stopdist=%d %c\n", tra->pname,
 101           ir->max_speed_estimate, ir->stopping_distance,
 102           "-s"[ir->stopping]);
 103 }
 104
 105 int speedmanager_stopped(Train *tra) {
 106   return tra->speed.speed == 0;
 107 }
 108
 109 static ErrorCode request_core(Train *tra, int step, struct timeval tnow,
 110                               PredictionProblemCallback *ppc, void *ppcu) {
 111   ErrorCode ec, ec2;
 112   double vnow, vtarg, vmax;
 113   SpeedInfo try;
 114   const SpeedRange *regime;
 115   Distance oldmaxinto;
 116
 117   DPRINTF(speed,core, "request %s%s step %d\n",
 118           tra->backwards?"-":"",tra->pname,
 119           step);
 120
 121   if (step == tra->speed.step)
 122     return 0;
 123
 124   vnow= current_speed(tra,tnow);
 125   vtarg= tra->speedcurve[step];
 126   oldmaxinto= tra->maxinto;
 127
 128   DPRINTF(speed,core, " request  vnow=%f vtarg=%f\n", vnow,vtarg);
 129
 130   vmax= MAX(vtarg,vnow);
 131   try.max_speed_estimate= vmax * MARGIN_SPEED;
 132   regime= stop_info(tra,vmax);
 133   try.stopping_distance= stoppingdistance(tra,tnow, regime, vmax);
 134   try.stopping= !step;
 135
 136   if (!try.stopping) {
 137     Segment *fdet= tra->foredetect;
 138     tra->maxinto= fdet->i->poscombs[fdet->movposcomb].dist;
 139     assert(tra->maxinto >= oldmaxinto);
 140     tra->uncertainty += tra->maxinto - oldmaxinto;
 141   }
 142
 143   ec= predict(tra,tnow,
 144               step <= tra->speed.step ? PREDF_OLDPLAN : 0,
 145               0,0, &try, ppc,ppcu);
 146   if (ec) {
 147     tra->uncertainty -= tra->maxinto - oldmaxinto;
 148     tra->maxinto= oldmaxinto;
 149     ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,
 150                  0,(char*)"abandoned acceleration");
 151     assert(!ec2);
 152     return ec;
 153   }
 154
 155   toev_stop(&tra->speed.decel);
 156   tra->speed.step= step;
 157   tra->speed.speed= vmax;
 158
 159   if (vtarg <= vnow) {
 160     tra->speed.decel.duration= regime->ts;
 161     toev_start(&tra->speed.decel);
 162   }
 163   report_train_position(tra);
 164   xmit(tra);
 165   return 0;
 166 }
 167
 168 ErrorCode speedmanager_speedchange_request(Train *tra, int step,
 169                            PredictionProblemCallback *ppc, void *ppcu) {
 170   struct timeval tnow;
 171   assert(ppc || ppcu);
 172
 173   MUSTECR( safety_checktrain(tra,ppc,ppcu) );
 174   mgettimeofday(&tnow);
 175   return request_core(tra,step,tnow,ppc,ppcu);
 176 }
 177
 178 void speedmanager_safety_stop(Train *tra, struct timeval tnow) {
 179   ErrorCode ec;
 180   ec= request_core(tra,0,tnow,0,(char*)"emergency stop");
 181   assert(!ec);
 182 }
 183
 184 void speedmanager_reset_train(Train *tra) {
 185   tra->speed.step= 0;
 186   toev_init(&tra->speed.decel);
 187   tra->speed.decel.callback= decel_done;
 188   tra->speed.decel.pclass= "decel";
 189   tra->speed.decel.pinst= tra->pname;
 190   tra->speed.speed= 0;
 191   if (tra->addr < 0)
 192     return;
 193
 194   actual_speed(tra, 0);
 195 }