hostside/speed.c

   1 /*
   2  * realtime
   3  * speed manager
   4  */
   5
   6 #include "realtime.h"
   7 #include "nmra.h"
   8
   9 static void changereq_internal(Train *tra, int newcommanded, int inautostop);
  10 static void accel_more(TimeoutEvent *toev);
  11
  12 void speedmanager_reset_train(Train *tra) {
  13   Nmra n;
  14   ErrorCode ec;
  15
  16   tra->estopping= 0;
  17   tra->speed= 0;
  18   tra->accel.target= tra->accel.commanded= 0;
  19   toev_init(&tra->accel.more);
  20   enco_nmra_speed126(&n, tra->addr, 0, tra->backwards);
  21   retransmit_urgent_queue_relaxed(&tra->accel.rn, &n);
  22   ec= safety_requestspeed(tra, 0);
  23   if (ec)
  24     safety_panic(tra, 0, "position reset impossible!");
  25 }
  26
  27 static void xmit_speed(Train *tra) {
  28   Nmra n;
  29   enco_nmra_speed126(&n, tra->addr,
  30                      tra->accel.curve[tra->accel.commanded].step,
  31                      tra->backwards);
  32   retransmit_urgent_requeue(&tra->accel.rn, &n);
  33 }
  34
  35 void speedmanager_autostop(Train *tra) {
  36   if (tra->speed < AUTOSTOP_MAXSPEED) {
  37     toev_stop(&tra->accel.more);
  38     tra->accel.commanded= tra->accel.target= tra->speed= 0;
  39     xmit_speed(tra);
  40   } else {
  41     changereq_internal(tra, 0, 1);
  42   }
  43 }
  44
  45 static void estop_done(TimeoutEvent *toev) {
  46   Train *tra= (void*)((char*)toev - offsetof(Train, accel.more));
  47   retransmit_urgent_cancel(&tra->accel.rn);
  48   speedmanager_reset_train(tra);
  49 }
  50
  51 void speedmanager_emergencystop(Train *tra) {
  52   Nmra n;
  53   tra->estopping= 1;
  54   toev_stop(&tra->accel.more);
  55   enco_nmra_estop1(&n, tra->addr);
  56   retransmit_urgent_requeue(&tra->accel.rn, &n);
  57
  58   toev_stop(&tra->accel.more);
  59   tra->accel.more.callback= estop_done;
  60   tra->accel.more.duration= ESTOP_DEADTIME;
  61   toev_start(&tra->accel.more);
  62 }
  63
  64 static void adjust_next(Train *tra, int inautostop) {
  65   long newspeed;
  66
  67   if (tra->accel.target > tra->accel.commanded) {
  68     tra->accel.commanded++;
  69     tra->accel.more.duration= tra->accel.curve[tra->accel.commanded].upwait;
  70     newspeed= tra->accel.curve[tra->accel.commanded].speed;
  71   } else if (tra->accel.target < tra->accel.commanded) {
  72     newspeed= tra->accel.curve[tra->accel.commanded].speed;
  73     tra->accel.commanded--;
  74     tra->accel.more.duration= tra->accel.curve[tra->accel.commanded].downwait;
  75   } else {
  76     return;
  77   }
  78
  79   if (!inautostop) {
  80     ErrorCode ec= safety_requestspeed(tra, newspeed);
  81     if (ec) {
  82       assert(newspeed > tra->speed);
  83       assert(tra->accel.target >= tra->accel.commanded);
  84       tra->accel.target= --tra->accel.commanded;
  85       toev_stop(&tra->accel.more);
  86       return;
  87     }
  88   } else {
  89     tra->speed= newspeed;
  90   }
  91
  92   toev_stop(&tra->accel.more);
  93   tra->accel.more.callback= accel_more;
  94   toev_start(&tra->accel.more);
  95   xmit_speed(tra);
  96 }
  97
  98 static void accel_more(TimeoutEvent *toev) {
  99   Train *tra= (void*)((char*)toev - offsetof(Train, accel.more));
 100   adjust_next(tra, 0);
 101 }
 102
 103 static void changereq_internal(Train *tra, int newcommanded, int inautostop) {
 104   int reverse, newold;
 105
 106   if (!tra->accel.more.running) {
 107     assert(tra->accel.commanded == tra->accel.target);
 108     adjust_next(tra, 0);
 109   } else {
 110     if (tra->accel.target > tra->accel.commanded) {
 111       newold= tra->accel.commanded+1;
 112       reverse= newcommanded < newold;
 113     } else if (tra->accel.target < tra->accel.commanded) {
 114       newold= tra->accel.commanded-1;
 115       reverse= newcommanded > newold;
 116     } else {
 117       abort();
 118     }
 119     tra->accel.target= newcommanded;
 120
 121     if (reverse) {
 122       /* switch from accel to decel or vice versa */
 123       toev_stop(&tra->accel.more);
 124       tra->accel.commanded= newold;
 125       adjust_next(tra, 0);
 126     }
 127   }
 128 }
 129
 130 void speedmanager_speedchange_request(Train *tra, long speed) {
 131   changereq_internal(tra, speed, 0);
 132   int a, b, try, found;
 133   const SpeedCurveEntry *curve= tra->accel.curve;
 134
 135   if (tra->estopping) {
 136     logmsg(EC_Invalid, tra, 0, "speed request ignored during emergency stop");
 137     return;
 138   }
 139
 140   for (a=0, b=tra->accel.curvesz;
 141        a < b;
 142        ) {
 143     try= (a + b) >> 2;
 144     if (curve[try].speed > speed) b= try; else a= try+1;
 145   }
 146   /* Loop postconditions:
 147    *   a==b
 148    *   either   a=try+1 never executed ie all curve[].speed > speed
 149    *             hence a=0
 150    *       or   curve[a-1].speed <= speed
 151    *   either   b=try never executed ie all curve[].speed <= speed
 152    *             hence b=trai->nsteps
 153    *       or   curve[b].speed > speed
 154    */
 155   found= a>1 ? a-1 : speed ? 1 : 0;
 156
 157   if (curve[found].speed < speed && speed != INT_MAX)
 158     logmsg(EC_Invalid, tra,0,
 159            "requested speed %ld excessive; capping at %ld",
 160            speed, curve[found].speed);
 161
 162   changereq_internal(tra, found, 0);
 163 }