hostside/safety.c

   1 /*
   2  * realtime
   3  * safety core algorithm
   4  */
   5
   6 #include <stdio.h>
   7 #include <assert.h>
   8
   9 #include "realtime.h"
  10
  11 int n_trains;
  12 Train *trains;
  13 Segment *segments;
  14
  15 static void seg_clear_stale(Segment *seg) {
  16   if (!seg->tr_updated) {
  17     seg->owner= 0;
  18     seg->until_here= seg->until_detect= 0;
  19   }
  20 }
  21
  22 typedef struct {
  23   /* constant inputs */
  24   Train *tra;
  25   /* modified in place by lay_train_pass: */
  26   ErrorCode ec;
  27   int invert_count[2]; /* count of (switchable) segments,
  28                         * invert_count[0]: inverted from train's pov
  29                         *   iff train is backwards (ie, train not inverted)
  30                         * invert_count[1]: train is inverted
  31                         * set to -1 if any unswitchable is the other way */
  32   Segment *invert_forcer; /* the unswitchable which forces */
  33 } LayTrainState;
  34
  35 static void lay_train_pass(LayTrainState *l,
  36                            TrackLocation tloc, long advance,
  37                            long speed, unsigned backwards,
  38                            unsigned check_clash) {
  39   Segment *seg;
  40   long overall, remain, dist_until, time_until;
  41   int *invert_likehere, train_inverted_here;
  42   Train *tra= l->tra;
  43
  44   if (l->ec) return;
  45
  46   seg= tra->foredetect;
  47
  48   remain= overall= advance + speed * SPEED_CLEAR_MULT;
  49
  50   for (;;) {
  51     seg= tloc.seg;
  52
  53     if (check_clash) {
  54       if (seg->tr_updated) {
  55         l->ec= safety_problem(l->tra, tloc.seg, "self-collision");
  56         return;
  57       }
  58       if (seg->owner) {
  59         if (seg->owner != l->tra) {
  60           l->ec= safety_problem(l->tra, tloc.seg, "collision with %s",
  61                                 seg->owner->pname);
  62           return;
  63         }
  64         seg_clear_stale(seg);
  65       }
  66     }
  67
  68     seg->owner= l->tra;
  69     seg->tr_backwards= tloc.backwards ^ backwards;
  70     seg->tr_updated= 1;
  71
  72     train_inverted_here= seg->seg_inverted ^ seg->tr_backwards;
  73     invert_likehere= &l->invert_count[train_inverted_here];
  74
  75     if (seg->i->invertible) {
  76       if (*invert_likehere >= 0)
  77         (*invert_likehere)++;
  78     } else {
  79       if (*invert_likehere < 0) {
  80         l->ec= safety_problem(l->tra,0, "train requires"
  81                               " noninvertible segments with opposite polarity:"
  82                               " @%s, @%s", seg->i->pname,
  83                               l->invert_forcer->i->pname);
  84         return;
  85       }
  86       assert(!seg->seg_inverted);
  87       (*invert_likehere)++;
  88       l->invert_count[!train_inverted_here]= -1;
  89       l->invert_forcer= seg;
  90     }
  91
  92     dist_until= (overall - remain) - advance;
  93     time_until= !speed ? 0 : SPEED_CALC_TIME(speed, dist_until, DOWN);
  94     *(check_clash ? &seg->until_here : &seg->until_detect)= time_until;
  95
  96     if (!remain) break;
  97     trackloc_further(&tloc, &remain);
  98   }
  99 }
 100
 101 static void lay_train_inversions(LayTrainState *l) {
 102   SEG_IV;
 103   int train_be_inverted, seg_be_inverted;
 104
 105   if (l->ec) return;
 106
 107   train_be_inverted= l->invert_count[1] > l->invert_count[0];
 108   assert(l->invert_count[train_be_inverted] >= 0);
 109
 110   actual_inversions_start();
 111
 112   FOR_SEG {
 113     if (!seg->tr_updated) continue;
 114     assert(seg->owner == l->tra);
 115     seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
 116     assert(!(seg_be_inverted && !segi->invertible));
 117     seg->seg_inverted= seg_be_inverted;
 118     actual_inversions_segment(seg);
 119   }
 120   actual_inversions_done();
 121 }
 122
 123 static void lay_train_done(LayTrainState *l) {
 124   SEG_IV;
 125
 126   FOR_SEG {
 127     if (seg->owner == l->tra) {
 128       if (!seg->tr_updated) seg_clear_stale(seg);
 129       seg->tr_updated= 0;
 130     }
 131     assert(!seg->tr_updated);
 132     assert(seg->until_detect >= seg->until_here);
 133   }
 134 }
 135
 136 static ErrorCode lay_train(Train *tra, long added_slop) {
 137   Segment *seg;
 138   TrackLocation tloc;
 139   long head, headslop, tail, taildet;
 140   LayTrainState l;
 141
 142   seg= tra->foredetect;
 143
 144   tloc.seg= seg;
 145   tloc.into= tra->maxinto;
 146   tloc.backwards= seg->tr_backwards ^ tra->backwards;
 147
 148   l.tra= tra;
 149   l.ec= 0;
 150   l.invert_count[0]= l.invert_count[1]= 0;
 151
 152   head= tra->backwards ? tra->tail : tra->head;
 153   headslop= head + added_slop;
 154
 155   /* 1st pass:
 156    * update actual train location (train's head and future locations).
 157    *
 158    * 2nd pass:
 159    * update detection (future locations) only.
 160    *
 161    * 3rd pass:
 162    * train location (current location and tail)
 163    *
 164    * Clash checking is done in passes 1 and 3, including checking
 165    * whether the train clashes with itself (ie, fail if we find
 166    * segment with same train and tr_updated set).
 167    */
 168   lay_train_pass(&l, tloc,  headslop, tra->speed, 0, 1);
 169   lay_train_pass(&l, tloc,  0,        tra->speed, 0, 0);
 170
 171   trackloc_reverse(&tloc);
 172   seg->tr_updated= 0; /* we're about to do this one again */
 173   tail= tra->backwards ? tra->head : tra->tail;
 174   taildet= tra->detectable + tail;
 175
 176   lay_train_pass(&l, tloc, taildet,   0,          1, 1);
 177
 178   lay_train_inversions(&l);
 179   lay_train_done(&l);
 180
 181   return l.ec;
 182 }
 183
 184 void safety_notify_detection(Segment *seg) {
 185   Train *tra= seg->owner;
 186   ErrorCode ec;
 187   TrackLocation tloc;
 188
 189   if (!seg->owner)
 190     safety_panic(0,seg, "unexpected detection");
 191
 192   if (!seg->until_detect)
 193     return;
 194
 195   tloc.seg= seg;
 196   tloc.into= 0;
 197   tloc.backwards= seg->tr_backwards;
 198
 199   tra->foredetect= seg;
 200   tra->maxinto= trackloc_remaininseg(&tloc);
 201
 202   if (seg->cm_autostop) {
 203     seg->cm_autostop= 0;
 204     if (!tra->estopping) {
 205       speedmanager_autostop(tra);
 206       if (!tra->speed && tra->maxinto > AUTOSTOP_UNCERTAINTY)
 207         /* At some point we may need to allow more slop when the
 208          * next segment is points than when this is the last segment
 209          * (ie, just buffers next). */
 210         tra->maxinto= AUTOSTOP_UNCERTAINTY;
 211     }
 212   }
 213   tra->uncertainty= tra->maxinto;
 214
 215   if (!tra->estopping) {
 216     ec= lay_train(tra, 0);
 217     if (ec) {
 218       logmsg(ec,tra,seg->i, "emergency stop");
 219       safety_emergencystop(tra);
 220     }
 221   }
 222 }
 223
 224 void safety_emergencystop(Train *tra) {
 225   ErrorCode ec;
 226
 227   if (tra->estopping) return;
 228
 229   ec= lay_train(tra, ESTOP_UNCERTAINTY);
 230   if (ec) safety_panic(tra,0, "emergency stop forbidden!");
 231   speedmanager_emergencystop(tra);
 232 }
 233
 234 ErrorCode safety_requestspeed(Train *tra, long newspeed) {
 235   long oldspeed;
 236   ErrorCode ec;
 237
 238   oldspeed= tra->speed;
 239   tra->speed= newspeed;
 240
 241   ec= lay_train(tra, 0);
 242
 243   if (ec) {
 244     ErrorCode revert_ec;
 245
 246     if (oldspeed && oldspeed < newspeed) {
 247       logmsg(ec,tra,0, "countermanded acceleration"
 248              " from %ld to %ld", oldspeed, newspeed);
 249     } else if (oldspeed) {
 250       safety_panic(tra,0, "deceleration forbidden!"
 251                    " (from %ld to %ld", oldspeed, newspeed);
 252     } else {
 253       logmsg(ec,tra,0, "countermanded motion start");
 254     }
 255     oprintf(UPO, "countermanded %s %ld %ld\n",
 256             tra->pname, oldspeed, newspeed);
 257     revert_ec= lay_train(tra, 0);
 258     if (revert_ec)
 259       safety_panic(tra,0, "countermanding"
 260                    " speed change insufficient!");
 261   }
 262   return ec;
 263 }