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                               unsigned predictflags,
 111                               PredictionProblemCallback *ppc, void *ppcu) {
 112   /* We ignore the value of PREDF_OLDPLAN in predictflags, as we can tell
 113    * whether the old plan is good by the change in speed */
 114   ErrorCode ec, ec2;
 115   double vnow, vtarg, vmax;
 116   SpeedInfo try;
 117   const SpeedRange *regime;
 118   Distance oldmaxinto;
 119
 120   DPRINTF(speed,core, "request %s%s step %d\n",
 121           tra->backwards?"-":"",tra->pname,
 122           step);
 123
 124   if (step == tra->speed.step)
 125     return 0;
 126
 127   vnow= current_speed(tra,tnow);
 128   vtarg= tra->speedcurve[step];
 129   oldmaxinto= tra->maxinto;
 130
 131   DPRINTF(speed,core, " request  vnow=%f vtarg=%f\n", vnow,vtarg);
 132
 133   vmax= MAX(vtarg,vnow);
 134   try.max_speed_estimate= vmax * MARGIN_SPEED;
 135   regime= stop_info(tra,vmax);
 136   try.stopping_distance= stoppingdistance(tra,tnow, regime, vmax);
 137   try.stopping= !step;
 138
 139   if (!try.stopping) {
 140     Segment *fdet= tra->foredetect;
 141     tra->maxinto= fdet->i->poscombs[fdet->movposcomb].dist;
 142     assert(tra->maxinto >= oldmaxinto);
 143     tra->uncertainty += tra->maxinto - oldmaxinto;
 144   }
 145
 146   predictflags &= ~PREDF_OLDPLAN;
 147
 148   ec= predict(tra,tnow,
 149               predictflags | (step <= tra->speed.step ? PREDF_OLDPLAN : 0),
 150               0,0, &try, ppc,ppcu);
 151   if (ec) {
 152     tra->uncertainty -= tra->maxinto - oldmaxinto;
 153     tra->maxinto= oldmaxinto;
 154     ec2= predict(tra,tnow, predictflags | PREDF_OLDPLAN, 0,0, 0,
 155                  0,(char*)"abandoned acceleration");
 156     assert(!ec2);
 157     return ec;
 158   }
 159
 160   toev_stop(&tra->speed.decel);
 161   tra->speed.step= step;
 162   tra->speed.speed= vmax;
 163
 164   if (vtarg <= vnow) {
 165     tra->speed.decel.duration= regime->ts;
 166     toev_start(&tra->speed.decel);
 167   }
 168   report_train_position(tra);
 169   xmit(tra);
 170   return 0;
 171 }
 172
 173 ErrorCode speedmanager_speedchange_request(Train *tra, int step,
 174                            PredictionProblemCallback *ppc, void *ppcu) {
 175   struct timeval tnow;
 176   assert(ppc || ppcu);
 177
 178   MUSTECR( safety_checktrain(tra,ppc,ppcu) );
 179   mgettimeofday(&tnow);
 180   return request_core(tra,step, tnow,0, ppc,ppcu);
 181 }
 182
 183 void speedmanager_safety_stop(Train *tra, struct timeval tnow,
 184                               unsigned predictflags) {
 185   ErrorCode ec;
 186   ec= request_core(tra,0,tnow,predictflags, 0,(char*)"emergency stop");
 187   assert(!ec);
 188 }
 189
 190 void speedmanager_reset_train(Train *tra) {
 191   tra->speed.step= 0;
 192   toev_init(&tra->speed.decel);
 193   tra->speed.decel.callback= decel_done;
 194   tra->speed.decel.pclass= "decel";
 195   tra->speed.decel.pinst= tra->pname;
 196   tra->speed.speed= 0;
 197   if (tra->addr < 0)
 198     return;
 199
 200   actual_speed(tra, 0);
 201 }
 202
 203 void speedmanager_init(void) {
 204   Nmra n;
 205   TRA_IV;
 206   FOR_TRA {
 207     enco_nmra_idle(&n);
 208     retransmit_urgent_queue_relaxed(&tra->speed.rn, &n);
 209   }
 210 }