/*
+ * realtime
+ * safety core algorithm
*/
#include <stdio.h>
#include <assert.h>
-#include "layoutinfo.h"
-#include "safety.h"
+#include "realtime.h"
-static void seg_clear_stale(SegmentState *seg) {
+int n_trains;
+Train *trains;
+Segment *segments;
+
+static void seg_clear_stale(Segment *seg) {
if (!seg->tr_updated) {
- seg->owned= 0;
+ seg->owner= 0;
seg->until_here= seg->until_detect= 0;
}
}
typedef struct {
/* constant inputs */
- TrainNum tran;
+ Train *tra;
/* modified in place by lay_train_pass: */
ErrorCode ec;
- int invert_count[1]; /* count of (switchable) segments,
+ int invert_count[2]; /* count of (switchable) segments,
* invert_count[0]: inverted from train's pov
* iff train is backwards (ie, train not inverted)
* invert_count[1]: train is inverted
* set to -1 if any unswitchable is the other way */
- SegmentNum invert_forcer; /* the unswitchable which forces */
+ Segment *invert_forcer; /* the unswitchable which forces */
} LayTrainState;
static void lay_train_pass(LayTrainState *l,
TrackLocation tloc, long advance,
long speed, unsigned backwards,
unsigned check_clash) {
- State *s= &safety_state;
- SegmentNum segn;
- SegmentState *seg;
- const SegmentInfo *segi;
+ Segment *seg;
long overall, remain, dist_until, time_until;
int *invert_likehere, train_inverted_here;
- TrainState *tra= &s->trains[l->tran];
+ Train *tra= l->tra;
if (l->ec) return;
- segn= tra->foredetect;
- seg= &s->segments[segn];
+ seg= tra->foredetect;
remain= overall= advance + speed * SPEED_CLEAR_MULT;
for (;;) {
- segn= tloc.segn;
- seg= &s->segments[segn];
- segi= &info_segments[segn];
+ seg= tloc.seg;
if (check_clash) {
if (seg->tr_updated) {
- l->ec= safety_problem(l->tran, tloc.segn, "self-collision");
+ l->ec= safety_problem(l->tra, tloc.seg, "self-collision");
return;
}
- if (seg->owned) {
- if (seg->owner != l->tran) {
- l->ec= safety_problem(l->tran, tloc.segn, "collision with %s",
- info_trains[seg->owner].pname);
+ if (seg->owner) {
+ if (seg->owner != l->tra) {
+ l->ec= safety_problem(l->tra, tloc.seg, "collision with %s",
+ seg->owner->pname);
return;
}
seg_clear_stale(seg);
}
}
- seg->owned= 1;
+ seg->owner= l->tra;
seg->tr_backwards= tloc.backwards ^ backwards;
seg->tr_updated= 1;
- seg->owner= l->tran;
train_inverted_here= seg->seg_inverted ^ seg->tr_backwards;
invert_likehere= &l->invert_count[train_inverted_here];
- if (segi->invertible) {
+ if (seg->i->invertible) {
if (*invert_likehere >= 0)
(*invert_likehere)++;
} else {
if (*invert_likehere < 0) {
- l->ec= safety_problem(l->tran, NOTA(Segment), "train requires"
+ l->ec= safety_problem(l->tra,0, "train requires"
" noninvertible segments with opposite polarity:"
- " @%s, @%s", segi->pname,
- info_segments[l->invert_forcer].pname);
+ " @%s, @%s", seg->i->pname,
+ l->invert_forcer->i->pname);
return;
}
assert(!seg->seg_inverted);
(*invert_likehere)++;
l->invert_count[!train_inverted_here]= -1;
- l->invert_forcer= segn;
+ l->invert_forcer= seg;
}
dist_until= (overall - remain) - advance;
}
static void lay_train_inversions(LayTrainState *l) {
- State *s= &safety_state;
SegmentNum segn;
- SegmentState *seg;
+ Segment *seg;
const SegmentInfo *segi;
int train_be_inverted, seg_be_inverted;
actual_inversions_start();
- for (segn=0, seg=s->segments, segi=info_segments;
+ for (segn=0, seg=segments, segi=info_segments;
segn <= NUM_SEGMENTS;
- segn++, seg++) {
+ segn++, seg++, segi++) {
if (!seg->tr_updated) continue;
- assert(seg->owner == l->tran);
+ assert(seg->owner == l->tra);
seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
assert(!(seg_be_inverted && !segi->invertible));
seg->seg_inverted= seg_be_inverted;
- actual_inversions_segment(segn);
+ actual_inversions_segment(seg);
}
actual_inversions_done();
}
static void lay_train_done(LayTrainState *l) {
- State *s= &safety_state;
SegmentNum segn;
- SegmentState *seg;
+ Segment *seg;
- for (segn=0, seg=s->segments;
+ for (segn=0, seg=segments;
segn <= NUM_SEGMENTS;
segn++, seg++) {
- if (seg->owner == l->tran) {
+ if (seg->owner == l->tra) {
if (!seg->tr_updated) seg_clear_stale(seg);
seg->tr_updated= 0;
}
}
}
-static ErrorCode lay_train(TrainNum tran, long added_slop) {
- State *s= &safety_state;
- TrainState *tra= &s->trains[tran];
- const TrainInfo *trai= &info_trains[tran];
- SegmentNum segn;
- SegmentState *seg;
+static ErrorCode lay_train(Train *tra, long added_slop) {
+ Segment *seg;
TrackLocation tloc;
long head, headslop, tail, taildet;
LayTrainState l;
- segn= tra->foredetect;
- seg= &s->segments[segn];
+ seg= tra->foredetect;
- tloc.segn= segn;
+ tloc.seg= seg;
tloc.into= tra->maxinto;
tloc.backwards= seg->tr_backwards ^ tra->backwards;
- l.tran= tran;
+ l.tra= tra;
l.ec= 0;
l.invert_count[0]= l.invert_count[1]= 0;
- head= tra->backwards ? trai->tail : trai->head;
+ head= tra->backwards ? tra->tail : tra->head;
headslop= head + added_slop;
/* 1st pass:
trackloc_reverse(&tloc);
seg->tr_updated= 0; /* we're about to do this one again */
- tail= tra->backwards ? trai->head : trai->tail;
- taildet= trai->detectable + tail;
+ tail= tra->backwards ? tra->head : tra->tail;
+ taildet= tra->detectable + tail;
lay_train_pass(&l, tloc, taildet, 0, 1, 1);
return l.ec;
}
-static void setspeed(TrainNum tran, Speed newspeed) {
- /* does not lay the train, caller must do that (or have done it,
- * in which case they should already have set tra->speed). */
- State *s= &safety_state;
- TrainState *tra= &s->trains[tran];
-
- tra->speed= newspeed;
- actual_setspeed(tran);
- speedmanager_speedchange_notify(tran);
-}
-
-void safety_notify_detection(SegmentNum segn) {
- State *s= &safety_state;
- SegmentState *seg= &s->segments[segn];
- TrainNum tran= seg->owner;
- TrainState *tra= &s->trains[tran];
+void safety_notify_detection(Segment *seg) {
+ Train *tra= seg->owner;
ErrorCode ec;
TrackLocation tloc;
- if (!seg->owned)
- safety_panic(NOTA(Train), segn, "unexpected detection");
+ if (!seg->owner)
+ safety_panic(0,seg, "unexpected detection");
if (!seg->until_detect)
return;
- tloc.segn= segn;
+ tloc.seg= seg;
tloc.into= 0;
tloc.backwards= seg->tr_backwards;
- tra->foredetect= segn;
+ tra->foredetect= seg;
tra->maxinto= trackloc_remaininseg(&tloc);
-
- if (seg->cm_autostop) {
- if (tra->speed < AUTOSTOP_MAXSPEED &&
- tra->maxinto > AUTOSTOP_UNCERTAINTY)
- /* At some point we may need to allow more slop when the
- * next segment is points than when this is the last segment
- * (ie, just buffers next). */
- tra->maxinto= AUTOSTOP_UNCERTAINTY;
+ if (seg->cm_autostop) {
seg->cm_autostop= 0;
- setspeed(tran, 0);
+ if (!tra->estopping) {
+ speedmanager_autostop(tra);
+ if (!tra->speed && tra->maxinto > AUTOSTOP_UNCERTAINTY)
+ /* At some point we may need to allow more slop when the
+ * next segment is points than when this is the last segment
+ * (ie, just buffers next). */
+ tra->maxinto= AUTOSTOP_UNCERTAINTY;
+ }
}
tra->uncertainty= tra->maxinto;
-
- ec= lay_train(tran, 0);
- if (ec) {
- logmsg(ec, tran, segn, "emergency stop");
- safety_emergencystop(tran);
+
+ if (!tra->estopping) {
+ ec= lay_train(tra, 0);
+ if (ec) {
+ logmsg(ec,tra,seg->i, "emergency stop");
+ safety_emergencystop(tra);
+ }
}
}
-void safety_emergencystop(TrainNum tran) {
- State *s= &safety_state;
+void safety_emergencystop(Train *tra) {
ErrorCode ec;
- TrainState *tra= &s->trains[tran];
- tra->speed= 0;
- actual_emergencystop(tran);
- ec= lay_train(tran, ESTOP_UNCERTAINTY);
- if (ec) safety_panic(tran, NOTA(Segment), "emergency stop forbidden!");
- speedmanager_speedchange_notify(tran);
+ if (tra->estopping) return;
+
+ ec= lay_train(tra, ESTOP_UNCERTAINTY);
+ if (ec) safety_panic(tra,0, "emergency stop forbidden!");
+ speedmanager_emergencystop(tra);
}
-void safety_requestspeed(TrainNum tran, long newspeed) {
- State *s= &safety_state;
+ErrorCode safety_requestspeed(Train *tra, long newspeed) {
long oldspeed;
- TrainState *tra= &s->trains[tran];
ErrorCode ec;
oldspeed= tra->speed;
tra->speed= newspeed;
- ec= lay_train(tran, 0);
+ ec= lay_train(tra, 0);
if (ec) {
+ ErrorCode revert_ec;
+
if (oldspeed && oldspeed < newspeed) {
- logmsg(ec, tran, NOTA(Segment), "countermanded acceleration"
+ logmsg(ec,tra,0, "countermanded acceleration"
" from %ld to %ld", oldspeed, newspeed);
} else if (oldspeed) {
- safety_panic(tran, NOTA(Segment), "deceleration forbidden!"
+ safety_panic(tra,0, "deceleration forbidden!"
" (from %ld to %ld", oldspeed, newspeed);
} else {
- logmsg(ec, tran, NOTA(Segment), "countermanded motion start");
+ logmsg(ec,tra,0, "countermanded motion start");
}
- setspeed(tran, oldspeed);
- ec= lay_train(tran, 0);
- if (ec)
- safety_panic(tran, NOTA(Segment), "countermanding"
+ oprintf(UPO, "countermanded %s %ld %ld\n",
+ tra->pname, oldspeed, newspeed);
+ revert_ec= lay_train(tra, 0);
+ if (revert_ec)
+ safety_panic(tra,0, "countermanding"
" speed change insufficient!");
- return;
}
-
- setspeed(tran, newspeed);
-}
-
-
-int main(void) {
- printf("%d\n",(int)sizeof(State));
- return 0;
+ return ec;
}