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   SegmentNum segn;
 103   Segment *seg;
 104   const SegmentInfo *segi;
 105   int train_be_inverted, seg_be_inverted;
 106
 107   if (l->ec) return;
 108
 109   train_be_inverted= l->invert_count[1] > l->invert_count[0];
 110   assert(l->invert_count[train_be_inverted] >= 0);
 111
 112   actual_inversions_start();
 113
 114   for (segn=0, seg=segments, segi=info_segments;
 115        segn <= NUM_SEGMENTS;
 116        segn++, seg++, segi++) {
 117     if (!seg->tr_updated) continue;
 118     assert(seg->owner == l->tra);
 119     seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
 120     assert(!(seg_be_inverted && !segi->invertible));
 121     seg->seg_inverted= seg_be_inverted;
 122     actual_inversions_segment(seg);
 123   }
 124   actual_inversions_done();
 125 }
 126
 127 static void lay_train_done(LayTrainState *l) {
 128   SegmentNum segn;
 129   Segment *seg;
 130
 131   for (segn=0, seg=segments;
 132        segn <= NUM_SEGMENTS;
 133        segn++, seg++) {
 134     if (seg->owner == l->tra) {
 135       if (!seg->tr_updated) seg_clear_stale(seg);
 136       seg->tr_updated= 0;
 137     }
 138     assert(!seg->tr_updated);
 139     assert(seg->until_detect >= seg->until_here);
 140   }
 141 }
 142
 143 static ErrorCode lay_train(Train *tra, long added_slop) {
 144   Segment *seg;
 145   TrackLocation tloc;
 146   long head, headslop, tail, taildet;
 147   LayTrainState l;
 148
 149   seg= tra->foredetect;
 150
 151   tloc.seg= seg;
 152   tloc.into= tra->maxinto;
 153   tloc.backwards= seg->tr_backwards ^ tra->backwards;
 154
 155   l.tra= tra;
 156   l.ec= 0;
 157   l.invert_count[0]= l.invert_count[1]= 0;
 158
 159   head= tra->backwards ? tra->tail : tra->head;
 160   headslop= head + added_slop;
 161
 162   /* 1st pass:
 163    * update actual train location (train's head and future locations).
 164    *
 165    * 2nd pass:
 166    * update detection (future locations) only.
 167    *
 168    * 3rd pass:
 169    * train location (current location and tail)
 170    *
 171    * Clash checking is done in passes 1 and 3, including checking
 172    * whether the train clashes with itself (ie, fail if we find
 173    * segment with same train and tr_updated set).
 174    */
 175   lay_train_pass(&l, tloc,  headslop, tra->speed, 0, 1);
 176   lay_train_pass(&l, tloc,  0,        tra->speed, 0, 0);
 177
 178   trackloc_reverse(&tloc);
 179   seg->tr_updated= 0; /* we're about to do this one again */
 180   tail= tra->backwards ? tra->head : tra->tail;
 181   taildet= tra->detectable + tail;
 182
 183   lay_train_pass(&l, tloc, taildet,   0,          1, 1);
 184
 185   lay_train_inversions(&l);
 186   lay_train_done(&l);
 187
 188   return l.ec;
 189 }
 190
 191 void safety_notify_detection(Segment *seg) {
 192   Train *tra= seg->owner;
 193   ErrorCode ec;
 194   TrackLocation tloc;
 195
 196   if (!seg->owner)
 197     safety_panic(0,seg, "unexpected detection");
 198
 199   if (!seg->until_detect)
 200     return;
 201
 202   tloc.seg= seg;
 203   tloc.into= 0;
 204   tloc.backwards= seg->tr_backwards;
 205
 206   tra->foredetect= seg;
 207   tra->maxinto= trackloc_remaininseg(&tloc);
 208
 209   if (seg->cm_autostop) {
 210     seg->cm_autostop= 0;
 211     if (!tra->estopping) {
 212       speedmanager_autostop(tra);
 213       if (!tra->speed && tra->maxinto > AUTOSTOP_UNCERTAINTY)
 214         /* At some point we may need to allow more slop when the
 215          * next segment is points than when this is the last segment
 216          * (ie, just buffers next). */
 217         tra->maxinto= AUTOSTOP_UNCERTAINTY;
 218     }
 219   }
 220   tra->uncertainty= tra->maxinto;
 221
 222   if (!tra->estopping) {
 223     ec= lay_train(tra, 0);
 224     if (ec) {
 225       logmsg(ec,tra,seg->i, "emergency stop");
 226       safety_emergencystop(tra);
 227     }
 228   }
 229 }
 230
 231 void safety_emergencystop(Train *tra) {
 232   ErrorCode ec;
 233
 234   if (tra->estopping) return;
 235
 236   ec= lay_train(tra, ESTOP_UNCERTAINTY);
 237   if (ec) safety_panic(tra,0, "emergency stop forbidden!");
 238   speedmanager_emergencystop(tra);
 239 }
 240
 241 ErrorCode safety_requestspeed(Train *tra, long newspeed) {
 242   long oldspeed;
 243   ErrorCode ec;
 244
 245   oldspeed= tra->speed;
 246   tra->speed= newspeed;
 247
 248   ec= lay_train(tra, 0);
 249
 250   if (ec) {
 251     ErrorCode revert_ec;
 252
 253     if (oldspeed && oldspeed < newspeed) {
 254       logmsg(ec,tra,0, "countermanded acceleration"
 255              " from %ld to %ld", oldspeed, newspeed);
 256     } else if (oldspeed) {
 257       safety_panic(tra,0, "deceleration forbidden!"
 258                    " (from %ld to %ld", oldspeed, newspeed);
 259     } else {
 260       logmsg(ec,tra,0, "countermanded motion start");
 261     }
 262     oprintf(UPO, "countermanded %s %ld %ld\n",
 263             tra->pname, oldspeed, newspeed);
 264     revert_ec= lay_train(tra, 0);
 265     if (revert_ec)
 266       safety_panic(tra,0, "countermanding"
 267                    " speed change insufficient!");
 268   }
 269   return ec;
 270 }