-/**/
+/*
+ */
#include <stdio.h>
#include "safety.h"
-State safety_actual;
-
-typedef struct TrackLocation TrackLocation;
-struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
- SegmentNum segn; /* current segment */
- long into; /* distance from start of segment */
- unsigned backwards:1; /* if 1, into is positive and measured from end */
-};
-
-const SegmentLinkInfo *trackloc_segmentlink_near(const TrackLocation *tloc) {
- SegmentState *seg = s->seg[tloc->segn];
- const SegmentInfo *segi= &safety_segis[tloc->segn];
-
- return (tloc->backwards ? &segi->forwards :
- seg->pt_sideways ? &segi->sideways :
- &segi->backwards);
-}
-
-const SegmentLinkInfo *trackloc_segmentlink_far(const TrackLocation *tloc) {
- SegmentState *seg = s->seg[tloc->segn];
- const SegmentInfo *segi= &safety_segis[tloc->segn];
-
- return (tloc->backwards ? &segi->backwards :
- seg->pt_sideways ? &segi->sideways :
- &segi->forwards);
-}
-
-long trackloc_remaininseg(const TrackLocation *tloc) {
- /* Returns dist that tloc can advance before it goes into next segment. */
- State *s = &safety_state;
- SegmentState *seg = s->seg[tloc->segn];
- const SegmentInfo *segi= &safety_segis[tloc->segn];
- const SegmentLinkInfo *lnki_near, *lnki_far;
- long segment_len;
-
- lnki_near= trackloc_segmentlink_near(tloc);
- lnki_far= trackloc_segmentlink_far(tloc);
- segment_len= linki_near->dist + link_far->dist;
- assert(tloc->into <= segment_len);
- return segment_len - tloc->info;
-}
-
-void trackloc_further(TrackLocation *tloc, long *remain_io) {
- /* Advances tloc, decrementing *remain_io, until either
- * *remain_io becomes zero, or tloc->segn changes. */
- State *s = &safety_state;
- SegmentState *seg = s->seg[tloc->segn];
- const SegmentInfo *segi= &safety_segis[tloc->segn];
- const SegmentLinkInfo *lnki_far;
-
- segment_remain= trackloc_remaininseg(tloc);
-
- if (*remain_io <= segment_remain) {
- tloc->into += *remain_io;
- *remain_io= 0;
- } else {
- lnki_far= trackloc_segmentlink_far(tloc);
- *remain_io -= segment_remain;
- tloc->segn= lnki_far->next;
- tloc->into= 0;
- tloc->backwards ^= lnki_far->next_backwards;
- }
-}
-
-static voi seg_clear_stale(SegmentState *seg) {
+static void seg_clear_stale(SegmentState *seg) {
if (!seg->tr_updated) {
- seg->tr_here_now= seg->tr_here_future=
- seg->tr_detect_now= seg->tr_detect_future= 0;
+ seg->owned= 0;
+ seg->until_here= seg->until_detect= NOTA(TimeInterval);
}
}
-void lay_train_checkclash(ErrorCode *ec, SegmentLinkInfo *lnki
- TrainNum tran,) {
- SegmentNum clash_segn;
- SegmentState *clash_seg;
- TrainNum clash_tran;
- TrainState *clash_tra;
-
- clash_segn= lnki->clashing;
- if (clash_segn == NOTA(Segment)) return;
-
- clash_seg= &s->segs[lnki->clashing];
- if (!clash_seg->owned) return;
-
- clash_tran= clash->owner;
- clash_tra= &s->tras[clash_tran];
-
- if (clash_tra->justarrived) {
- TrackLocation clash_loc;
- clash_loc.segn= clash_tra->foredetect;
- clash_loc.
-
-
-= &s->segs[lnki->clashing];
- if (clash->
-
- lay_train_checkclash2(
-
-static void lay_train(ErrorCode *ec, TrainNum tran,
- TrackLocation tloc, long into,
- unsigned backwards, long speed) {
- /* pass 0: update actual train location, check for train clashing
- * with itself (ie, fail if we find segment with same train and
- * tr_updated set.
- * pass 1: update detection only. Clashingness checking is
- * done where it is convenient not to avoid it, but is not
- * necessary. */
+typedef struct {
+ /* constant inputs */
+ TrainNum tran;
+ /* modified in place by lay_train_pass: */
+ ErrorCode ec;
+ int invert_count[1]; /* 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 */
+} LayTrainState;
+
+static void lay_train_pass(LayTrainState l,
+ TrackLocation tloc, long advance,
+ unsigned backwards, long speed,
+ unsigned check_clash) {
SegmentState *seg;
const SegmentInfo *segi;
long overall, remain;
+ int *invert_likehere, *invert_unlikehere;
- if (*ec) return;
+ if (l->ec) return;
segn= tra->segn;
seg= &s->segs[segn];
- tloc.segn= segn;
- tloc.into= into;
- tloc.backwards= seg->tr_backwards ^ backwards;
remain= overall= advance + speed * SPEED_CLEAR_MULT;
if (check_clash) {
if (seg->tr_updated) {
- *ec= safety_problem(tloc.segn, tran, tran, "collision with itself");
+ *ec= safety_problem(tloc.segn, l->tran, "self-collision");
return;
}
if (seg->owned) {
if (seg->owner != tran) {
- *ec= safety_problem(tloc.segn, tran, seg->owner, "collision");
+ l->ec= safety_problem(tloc.segn, l->tran, seg->owner, "collision"
+ " with %s", info_trans[seg->owner].pname);
return;
}
seg_clear_stale(seg);
seg->owned= 1;
seg->owner_backwards= tloc.backwards ^ backwards;
seg->tr_updated= 1;
- seg->tran= tran;
+ seg->tran= l->tran;
+
+ train_inverted_here= seg->seg_inverted ^ seg->tr_backwards;
+ invert_likehere= &l->invert_count[train_inverted_here];
+
+ if (segi->invertible) {
+ if (*invert_likehere >= 0)
+ (*invert_likehere)++;
+ } else {
+ if (*invert_likehere < 0) {
+ l->ec= safety_problem(l->tran, NOTA(SegmentNum), "train requires"
+ " noninvertible segments with opposite polarity:"
+ " @%s, @%s", segi->pname,
+ info_segments[l->invert_forcer].pname);
+ return;
+ }
+ assert(!seg->seg_inverted);
+ (*invert_likehere)++;
+ l->invert_count[!train_inverted_here]= -1;
+ l->invert_forcer= segn;
+ }
dist_until= (overall - remain) - advance;
- time_until= (SPEED_FACTOR * dist_until) / tra->speed;
- *(pass==0 ? &seg->until_here : &seg->until_detect)= time_until;
+ time_until= !speed ? 0 :
+ (SPEED_FACTOR * dist_until) / speed;
+ *(check_clash ? &seg->until_here : &seg->until_detect)= time_until;
if (!remain) break;
trackloc_further(&tloc, &remain);
}
}
-static void lay_train_done() {
+static void lay_train_inversions(LayTrainState *l) {
+ SegmentNum segn;
+ SegmentState *seg;
+ int train_be_inverted, seg_be_inverted;
+
+ if (l->ec) return;
+
+ train_be_inverted= l->invert_count[1] > l->invert_count[0];
+ assert(l->invert_count[train_be_inverted] >= 0);
+
for (segn=0, seg=s->segs;
segn <= NUM_SEGMENTS;
segn++, seg++) {
- if (seg->tr_here_future && seg->tran == tran) {
- seg_clear_stale(seg);
+ if (!seg->tr_updated) continue;
+ assert(seg->owner == l->tran);
+ seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
+
+}
+
+static void lay_train_done(LayTrainState *l) {
+ SegmentNum segn;
+ SegmentState *seg;
+
+ for (segn=0, seg=s->segs;
+ segn <= NUM_SEGMENTS;
+ segn++, seg++) {
+ if (seg->tran == l->tran) {
+ if (!seg->tr_updated) seg_clear_stale(seg);
seg->tr_updated= 0;
}
assert(!seg->tr_updated);
}
}
-void safety_train_changed(TrainNum tran) {
+static ErrorCode lay_train(TrainNum tran, long added_slop) {
State *s= &safety_state;
- TrainState *tra= &s->tras[tran];
- TrainInfo *trai= info_train[tran];
+ TrainState *tra= &s->trains[tran];
+ TrainInfo *trai= info_trains[tran];
SegmentState *seg;
TrackLocation tloc;
long head, future;
+ LayTrainState l;
segn= tra->foredetect;
- seg= &s->seg[segn];
-
- head= tra->backwards ? trai->tail : trai->head;
- lay_train(tran, &ec, tloc, head, 0,1);
-
- ec= 0;
- lay_train(&ec, tran, tloc, head, 0);
- lay_train(&ec, tran, tloc, 0, 1);
- lay_train_done(tran);
- if (ec) return ec;
-
+ seg= &s->segments[segn];
tloc.segn= segn;
- tloc.into=
-
-
-
+ tloc.into= tra->maxinto;
+ tloc.backwards= seg->tr_backwards ^ backwards;
+ l.tran= tran;
+ l.ec= 0;
+ l.invert_count[0]= l.invert_count[1]= 0;
+
+ head= tra->backwards ? trai->tail : trai->head;
+ headslop= head + added_slop;
+
+ /* 1st pass:
+ * update actual train location (train's head and future locations).
+ *
+ * 2nd pass:
+ * update detection (future locations) only.
+ *
+ * 3rd pass:
+ * train location (current location and tail)
+ *
+ * Clash checking is done in passes 1 and 3, including checking
+ * whether the train clashes with itself (ie, fail if we find
+ * segment with same train and tr_updated set).
+ */
+ lay_train(&l, tloc, tra->speed, headslop, 1);
+ lay_train(&l, tloc, tra->speed, 0, 0);
+
+ trackloc_reverse(&tloc);
+ seg->tr_updated= 0; /* we're about to do this one again */
+ tail= tra->backwards ? trai->head : trai->tail;
+ taildet= tra->detectable + tail;
+
+ lay_train(&l, tloc, 0, taildet, 1);
+
+ lay_train_inversions(&l);
+ lay_train_done(&l);
+
+ 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). */
+ TrainState *tra = s->trains[tran];
- for (segn =
-s->tras[tran]
- SegmentState *seg = s->seg[segn];
+ tra->speed= newspeed;
+ actual_setspeed(tran);
+ speedmanager_speedchange_notify(tran);
+}
void safety_notify_detection(SegmentNum segn) {
State *s = &safety_state;
- SegmentState *seg = s->seg[segn];
+ SegmentState *seg = s->segments[segn];
TrainNum tran = segs->owner;
- TrainState *tra = s->tras[tran];
+ TrainState *tra = s->trains[tran];
TrackLocation tloc;
if (seg->detectable_now) return;
if (!seg->detectable_future)
- safety_panic(segn, NONE, "unexpected detection");
+ safety_panic(NOTA(TrainNum), segn, "unexpected detection");
tloc.segn= segn;
tloc.into= 0;
tra->foredetect= segn;
tra->maxinto= trackloc_remaininseg(&tloc);
- if (seg->autostop) {
- seg->autostop= 0;
- speedmanager_setspeed(tran, 0);
- if (tra->maxinto > AUTOSTOP_UNCERTAINTY)
+ 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;
+
+ seg->cm_autostop= 0;
+ setspeed(tran, 0);
}
tra->uncertainty= tra->maxinto;
- ec= safety_train_changed(tran);
+ ec= lay_train(tran, 0);
if (ec) {
logmsg(tran, segn, "emergency stop");
- actual_emergencystop(tran);
- tra->justarrived= 0;
- ec= safety_train_changed(s, tran);
- if (ec) panic(segn, tran, "emergency stop insufficient!");
+ safety_emergencystop(tran);
}
}
+void safety_emergencystop(TranNum) {
+ ErrorCode ec;
-
-
-
+ tra->speed= 0;
+ actual_emergencystop(tran);
+ ec= lay_train(tran, ESTOP_UNCERTAINTY);
+ if (ec) safety_panic(tran, segn, "emergency stop forbidden!");
+ speedmanager_speedchange_notify(tran, tra->backwards);
+}
+
+void safety_requestspeed(TrainNum tran, long newspeed) {
+ long oldspeed;
+ TrainState *tra = s->trains[tran];
- own
+ oldspeed= tra->speed;
+ tra->speed= newspeed;
+
+ ec= lay_train(tran, 0);
+
+ if (ec) {
+ if (oldspeed && oldspeed < newspeed) {
+ logmsg(tran, NOTA(SegmentNum), "countermanded acceleration",
+ " from %l to %l", oldspeed, newspeed);
+ } else if (oldspeed) {
+ safety_panic(tran, NOTA(SegmentNum), "deceleration forbidden!"
+ " (from %l to %l", oldspeed, newspeed);
+ } else {
+ logmsg(tran, NOTA(SegmentNum), "countermanded motion start");
+ }
+ setspeed(tran, oldspeed);
+ ec= lay_train(tran, 0);
+ if (ec)
+ safety_panic(tran, NOTA(SegmentNum), "countermanding"
+ " speed change insufficient!");
+ return;
+ }
+
+ setspeed(tran, newspeed);
+}
int main(void) {
#ifndef SAFETY_H
#define SAFETY_H
+/*========== basic types etc. ==========*/
+
typedef unsigned short TrainNum;
typedef unsigned short SegmentNum;
typedef unsigned short LocationNum;
typedef short Distance;
typedef char Speed; /* non-negative, units of 4mm/s */
+/*========== state of the layout ==========*/
+
typedef struct {
- SegmentNum foredetect; /* train's detectable part is at most maxinto */
- Distance maxinto, uncertainty; /* into foredetect but train may be */
- unsigned /* uncertainty less far advanced */
+ SegmentNum foredetect; /* train's detectable part is at most maxinto */
+ Distance maxinto, uncertainty; /* into foredetect but train may be */
+ unsigned /* uncertainty less far advanced */
backwards:1; /* train is moving backwards wrt its own front and back */
Speed speed;
} TrainState;
owned:1, /* this track segment is reserved for a train */
tr_backwards:1, /* train's motion is (would be) backwards wrt track */
pt_sideways:1, /* points are set to `alternative'. (no points?: 0) */
- pt_moving:1, /* points have been told to change, sideways is new state */
- autostop:1, /* owning train is slow and wants to stop on detection */
- tr_updated:1; /* for use by safety_train_changed etc.; otherwise 0 */
+ pt_moving:1, /* points have been told to change to pt_sideways */
+ cm_autostop:1, /* train should stop on detection */
+ cm_pointchange:1, /* points should change when they can */
+ seg_inverted:1, /* polarity is inverted! */
+ tr_updated:1; /* for use by safety.c:lay_train etc.; otherwise 0 */
TimeInterval until_here, /* ) nonnegative; */ /* ) always valid but */
until_detect; /* ) 0 if already */ /* ) only meaningful */
TrainNum owner; /* ) iff owned */
} SegmentLinkInfo;
typedef struct {
+ unsigned invertible:1;
SegmentLink backwards, forwards, sideways;
+ const char *pname;
} SegmentInfo;
typedef struct {
Speed maxspeed;
Distance tail, detectable, head;
+ const char *pname;
} TrainInfo;
-extern const TrainInfo info_train[NUM_TRAINS];
-extern const SegmentInfo info_segment[NUM_SEGMENTS];
+extern const TrainInfo info_trains[NUM_TRAINS];
+extern const SegmentInfo info_segments[NUM_SEGMENTS];
typedef struct {
- TrainState tras[NUM_TRAINS];
- SegmentState segs[NUM_SEGMENTS];
+ TrainState trains[NUM_TRAINS];
+ SegmentState segments[NUM_SEGMENTS];
} State;
+extern State s;
+
+/*========== safety.c ==========*/
+/*
+ * safety.c is responsible for ensuring that things don't go
+ * physically wrong (eg, collisions, derailments, short circuits,
+ * etc.).
+ */
+
+void safety_emergencystop(TranNum);
+ /* Callable directly in response to application command. */
+
+void safety_requestspeed(TrainNum tran, long newspeed);
+ /* To be called only by the speed manager, thus indirectly from
+ * user request.
+ * Will result in a call to speedmanager_speedchange_notify (and of
+ * course to actual_setspeed. Speed manager must apply accel/decel
+ * curve so that if safety.c agrees the command, the actual speed of
+ * the train changes straight away (at least for decel).
+ */
+
+void safety_notify_detection(SegmentNum segn);
+ /* To be called by actual.c when new train detection occurs. */
+
+/*========== speedmgr.c ==========*/
+
+void speedmanager_speedchange_notify(TrainNum tran);
+ /* To be called only by safety.c, whenever speed is actually set.
+ * New speed has already been recorded in State. */
+
+/*========== actual.c ==========*/
+/* actual.c should only be called from safety.c.
+ * It is responsible for communicating with the PICs, including
+ * repeating the NMRA commands and redacting detection information.
+ */
+
+void actual_setspeed(TrainNum tran);
+
+/*
+ *
+ * Entrypoints are: Called from, and as a result of:
+ * actual_setspeed safety.c
+ * actual_emergencystop safety.c
+
+
+/*========== utils.c ==========*/
+
+typedef struct TrackLocation TrackLocation;
+struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
+ SegmentNum segn; /* current segment */
+ long into; /* distance from start of segment */
+ unsigned backwards:1; /* if 1, into is positive and measured from end */
+};
+
+long trackloc_remaininseg(const TrackLocation *tloc);
+ /* Returns dist that tloc can advance before it goes into next segment. */
+
+void trackloc_further(TrackLocation *tloc, long *remain_io);
+ /* Advances tloc, decrementing *remain_io, until either
+ * *remain_io becomes zero, or tloc->segn changes. */
+
+void trackloc_reverse(TrackLocation *tloc);
+ /* Reverses tloc without changing its actual location. */
+
+const SegmentLinkInfo *trackloc_segmentlink_near(const TrackLocation *tloc);
+const SegmentLinkInfo *trackloc_segmentlink_far(const TrackLocation *tloc);
+
#endif /*SAFETY_H*/
~ijackson / trains.git / commitdiff