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