*/
#include <stdio.h>
+#include <assert.h>
#include "layoutinfo.h"
#include "safety.h"
static void lay_train_pass(LayTrainState *l,
TrackLocation tloc, long advance,
- unsigned backwards, long speed,
+ long speed, unsigned backwards,
unsigned check_clash) {
+ State *s= &safety_state;
SegmentNum segn;
SegmentState *seg;
const SegmentInfo *segi;
- long overall, remain;
- int *invert_likehere, *invert_unlikehere, train_inverted_here;
- TrainState *tra= &s.trains[l->tran];
+ long overall, remain, dist_until, time_until;
+ int *invert_likehere, train_inverted_here;
+ TrainState *tra= &s->trains[l->tran];
if (l->ec) return;
segn= tra->foredetect;
- seg= &s.segments[segn];
+ seg= &s->segments[segn];
remain= overall= advance + speed * SPEED_CLEAR_MULT;
for (;;) {
segn= tloc.segn;
- seg= &s.segments[segn];
+ seg= &s->segments[segn];
segi= &info_segments[segn];
if (check_clash) {
if (seg->tr_updated) {
- l->ec= safety_problem(tloc.segn, l->tran, "self-collision");
+ l->ec= safety_problem(l->tran, tloc.segn, "self-collision");
return;
}
if (seg->owned) {
if (seg->owner != l->tran) {
- l->ec= safety_problem(tloc.segn, l->tran, seg->owner, "collision"
- " with %s", info_trains[seg->owner].pname);
+ l->ec= safety_problem(l->tran, tloc.segn, "collision with %s",
+ info_trains[seg->owner].pname);
return;
}
seg_clear_stale(seg);
(*invert_likehere)++;
} else {
if (*invert_likehere < 0) {
- l->ec= safety_problem(l->tran, NOTA(SegmentNum), "train requires"
+ l->ec= safety_problem(l->tran, NOTA(Segment), "train requires"
" noninvertible segments with opposite polarity:"
" @%s, @%s", segi->pname,
info_segments[l->invert_forcer].pname);
}
dist_until= (overall - remain) - advance;
- time_until= !speed ? 0 :
- (SPEED_FACTOR * dist_until) / speed;
+ time_until= !speed ? 0 : SPEED_CALC_TIME(speed, dist_until, DOWN);
*(check_clash ? &seg->until_here : &seg->until_detect)= time_until;
if (!remain) break;
}
static void lay_train_inversions(LayTrainState *l) {
+ State *s= &safety_state;
SegmentNum segn;
SegmentState *seg;
const SegmentInfo *segi;
actual_inversions_start();
- for (segn=0, seg=s->segs, segi=info_segments;
+ for (segn=0, seg=s->segments, segi=info_segments;
segn <= NUM_SEGMENTS;
segn++, seg++) {
if (!seg->tr_updated) continue;
seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
assert(!(seg_be_inverted && !segi->invertible));
seg->seg_inverted= seg_be_inverted;
- actual_inversions_segment();
+ actual_inversions_segment(segn);
}
actual_inversions_done();
}
static void lay_train_done(LayTrainState *l) {
+ State *s= &safety_state;
SegmentNum segn;
SegmentState *seg;
- for (segn=0, seg=s->segs;
+ for (segn=0, seg=s->segments;
segn <= NUM_SEGMENTS;
segn++, seg++) {
- if (seg->tran == l->tran) {
+ if (seg->owner == l->tran) {
if (!seg->tr_updated) seg_clear_stale(seg);
seg->tr_updated= 0;
}
assert(!seg->tr_updated);
- assert(seg->tr_here_future >= seg->tr_here_now);
- assert(seg->tr_detect_future >= seg->tr_detect_now);
- assert(seg->tr_detect_future >= seg->tr_here_future);
- assert(seg->tr_detect_now >= seg->tr_here_now);
+ assert(seg->until_detect >= seg->until_here);
}
}
static ErrorCode lay_train(TrainNum tran, long added_slop) {
State *s= &safety_state;
TrainState *tra= &s->trains[tran];
- TrainInfo *trai= info_trains[tran];
+ const TrainInfo *trai= &info_trains[tran];
+ SegmentNum segn;
SegmentState *seg;
TrackLocation tloc;
- long head, future;
+ long head, headslop, tail, taildet;
LayTrainState l;
segn= tra->foredetect;
tloc.segn= segn;
tloc.into= tra->maxinto;
- tloc.backwards= seg->tr_backwards ^ backwards;
+ tloc.backwards= seg->tr_backwards ^ tra->backwards;
l.tran= tran;
l.ec= 0;
* 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);
+ lay_train_pass(&l, tloc, headslop, tra->speed, 0, 1);
+ lay_train_pass(&l, tloc, 0, 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;
+ taildet= trai->detectable + tail;
- lay_train(&l, tloc, 0, taildet, 1);
+ lay_train_pass(&l, tloc, taildet, 0, 1, 1);
lay_train_inversions(&l);
lay_train_done(&l);
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];
+ State *s= &safety_state;
+ TrainState *tra= &s->trains[tran];
tra->speed= newspeed;
actual_setspeed(tran);
}
void safety_notify_detection(SegmentNum segn) {
- State *s = &safety_state;
- SegmentState *seg = s->segments[segn];
- TrainNum tran = segs->owner;
- TrainState *tra = s->trains[tran];
+ State *s= &safety_state;
+ SegmentState *seg= &s->segments[segn];
+ TrainNum tran= seg->owner;
+ TrainState *tra= &s->trains[tran];
+ ErrorCode ec;
TrackLocation tloc;
- if (seg->detectable_now) return;
- if (!seg->detectable_future)
- safety_panic(NOTA(TrainNum), segn, "unexpected detection");
+ if (!seg->owned)
+ safety_panic(NOTA(Train), segn, "unexpected detection");
+
+ if (!seg->until_detect)
+ return;
tloc.segn= segn;
tloc.into= 0;
ec= lay_train(tran, 0);
if (ec) {
- logmsg(tran, segn, "emergency stop");
+ logmsg(ec, tran, segn, "emergency stop");
safety_emergencystop(tran);
}
}
-void safety_emergencystop(TranNum) {
+void safety_emergencystop(TrainNum tran) {
+ State *s= &safety_state;
ErrorCode ec;
+ TrainState *tra= &s->trains[tran];
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);
+ if (ec) safety_panic(tran, NOTA(Segment), "emergency stop forbidden!");
+ speedmanager_speedchange_notify(tran);
}
void safety_requestspeed(TrainNum tran, long newspeed) {
+ State *s= &safety_state;
long oldspeed;
- TrainState *tra = s->trains[tran];
+ TrainState *tra= &s->trains[tran];
+ ErrorCode ec;
oldspeed= tra->speed;
tra->speed= newspeed;
if (ec) {
if (oldspeed && oldspeed < newspeed) {
- logmsg(tran, NOTA(SegmentNum), "countermanded acceleration",
- " from %l to %l", oldspeed, newspeed);
+ logmsg(ec, tran, NOTA(Segment), "countermanded acceleration"
+ " from %ld to %ld", oldspeed, newspeed);
} else if (oldspeed) {
- safety_panic(tran, NOTA(SegmentNum), "deceleration forbidden!"
- " (from %l to %l", oldspeed, newspeed);
+ safety_panic(tran, NOTA(Segment), "deceleration forbidden!"
+ " (from %ld to %ld", oldspeed, newspeed);
} else {
- logmsg(tran, NOTA(SegmentNum), "countermanded motion start");
+ logmsg(ec, tran, NOTA(Segment), "countermanded motion start");
}
setspeed(tran, oldspeed);
ec= lay_train(tran, 0);
if (ec)
- safety_panic(tran, NOTA(SegmentNum), "countermanding"
+ safety_panic(tran, NOTA(Segment), "countermanding"
" speed change insufficient!");
return;
}
/*========== more basic types etc. ==========*/
-typedef short TimeInterval;
+typedef short TimeInterval; /*ms*/
typedef int ErrorCode;
+/*---------- units and scales ----------*/
+
+/*
+ * Distances are in mm.
+ * Times are in ms.
+ * Speeds are in fixed point: unit is 1/SPEED_UNIT m/s
+ */
+
+#define SPEED_SHIFT 7
+#define SPEED_UNIT (1L<<SPEED_SHIFT) /* ms/mm */
+
/*========== state of the layout ==========*/
typedef struct {
SegmentState segments[NUM_SEGMENTS];
} State;
-extern State s;
+extern State safety_state;
+
+/*========== embed.c ==========*/
+/* surrounds the algorithms and machinery in a program: logging, error
+ * handling, arg parsing, etc.
+ */
+
+void logmsg(ErrorCode ec, TrainNum tran, SegmentNum segn, const char *fmt,...)
+ __attribute__((format(printf,4,5)));
+
+void safety_panic(TrainNum tran, SegmentNum segn, const char *fmt,...)
+ __attribute__((format(printf,3,4),noreturn));
+
+ErrorCode safety_problem(TrainNum tran, SegmentNum segn, const char *fmt,...)
+ __attribute__((format(printf,3,4)));
/*========== safety.c ==========*/
/*
*/
void actual_setspeed(TrainNum tran);
+void actual_emergencystop(TrainNum tran);
void actual_inversions_start(void);
void actual_inversions_segment(SegmentNum);
void actual_inversions_done(void);
/* safety.c will call these in this order: first start, then segment
- * for 0 or more segments (whose inversion may or may not have
- * changed), and finally done. At done, the PICs should be told to
- * (de)invert the segments as specified.
- */
+ * for 0 or more segments (whose s.segments[segn].seg_inverted may
+ * or may not have changed), and finally done. At done, the PICs
+ * should be told to (de)invert the segments as specified. */
/*
* Entrypoints are: Called from, and as a result of:
const SegmentLinkInfo *trackloc_segmentlink_near(const TrackLocation *tloc);
const SegmentLinkInfo *trackloc_segmentlink_far(const TrackLocation *tloc);
+/*========== useful macros and declarations ==========*/
+
+/*---------- calculations with fixed point speeds ----------*/
+
+#define DIVIDE_ROUNDING_UP(num,den) (((num) + (den) - 1) / (den))
+#define DIVIDE_ROUNDING_DOWN(num,den) ( (num) / (den))
+
+#define SPEED_CALC_TIME(speed,dist,round) \
+ DIVIDE_ROUNDING_##round((dist) * SPEED_UNIT, (speed))
+
+#define SPEED_CALC_DIST(speed,time,round) \
+ DIVIDE_ROUNDING_##round((time) * (speed), SPEED_UNIT)
+
+/*---------- safety margin parameters ----------*/
+
+#define CLEAR_FORESIGHT_TIME 500 /*ms*/
+#define AUTOSTOP_MAXSPEED ((50 * SPEED_UNIT)/1000) /* 50 mm/s */
+#define AUTOSTOP_UNCERTAINTY 20
+#define ESTOP_UNCERTAINTY 300
+
+#define SPEED_CLEAR_MULT SPEED_CALC_DIST(1,CLEAR_FORESIGHT_TIME,UP)
+
#endif /*SAFETY_H*/
~ijackson / trains.git / commitdiff