* emergency stop). If this fails we issue the emergency stop command
* now. This prediction is done as follows:
*
- * We use three tracklocs which move in step - one for the foredetect
- * and one for each end of our train. This represents a predicted
- * train position. (We use `predicted' to refer to these future
- * situations and `current' to refer to that prevailing at the time we
- * are making the prediction.) When we advance these we advance
- * the head and foredetect in lockstep; when the foredetect enters
- * a new segment we chase up the with the tail.
+ * We use three tracklocs which move roughly together - one for the
+ * foredetect and one for each end of our train. This represents a
+ * predicted train position. (We use `predicted' to refer to these
+ * future situations and `current' to refer to that prevailing at the
+ * time we are making the prediction.) The advancement is driving
+ * using the foredetect; when it enters a new segment we advance first
+ * the head and then the tail by the same distance.
*
- * We advance these tracklocs firstly exactly one segment and then
- * over the current stopping distance. At each advancement we record
- * the time from the currently-next detection at which this prediction
- * will happen (computed from the current speed). (We don't take into
+ * We advance foredetect firstly exactly one segment and then over the
+ * current stopping distance. At each advancement we record the time
+ * from the currently-next detection at which this prediction will
+ * happen (computed from the current speed). (We don't take into
* account any already commanded but not necessarily happened
* deceleration; however if we are attempting to increase our speed,
* current speed refers to the new speed.) If we're already emergency
* prediction then that's fine we can just claim it.
*
* If we _have_ claimed it then it had better be predicted to be
- * vacant by the time we get there (pred_vacant set, see below).
+ * vacant by the time we get there (pred_present clear, see below).
*
* If it is currently moving then that must be OK because we checked
* it before (except that if we're trying to increase our speed that
* the new ownership.
*
* If not then we issue an signal stop command (we'd better not be
- * doing that already!) or countermand the speed increase or whatever
- */
+ * doing that already!) or countermand the speed increase or whatever */
/*
* Here is how we use the stuff in the Segment:
*
* to own the segment when we're finished
*/
-/* ============ OLD CODE ========== */
+#define pred_present mark0
+#define pred_vacated mark1
+
+void update_head(Train *tra, Segment *seg) {
+ tra->foredetect= seg;
+ if (tra->foredetect->movposcomb < 0)
+ safety_panic(tra,seg, "track route not set and train has arrived");
+}
+
+typedef struct {
+ unsigned
+ accelerating:1;
+ TrackLocation nose, fdet, tail;
+ TrackAdvanceContext nosec, tailc, fdetc;
+ TimeInterval elapsed;
+ Distance was_distance;
+
+ /* tally counts segments which have been entered in fdet_nextseg but
+ * not left in tail_nextseg (ie not the same ones as pred_present),
+ * in each direction (according to tr_backwards) */
+ int noninv_tally[2];
+} PredictUserContext;
+
+static int initpresent_nextseg(TrackLocation *t, TrackAdvanceContext *c,
+ MovPosComb *mpc_io, Segment *before) {
+ PredictUserContext *u= c->u;
+ next->pred_present= 1;
+ adjust_tally(t,c,1);
+}
+
+static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
+ MovPosComb *mpc_io, Segment *before) {
+ PredictUserContext *u= c->u;
+
+ /* Is it empty ? */
+
+ if (t->seg->owner) {
+ if (t->seg->owner != u->train)
+ return safety_problem(u->train, t->seg, "impeded by %s",
+ t->seg->owner->pname);
+ }
+ if (t->seg->pred_present)
+ return safety_problem(u->train, t->seg, "will collide with itself!");
+
+ /* Is it set for us ? */
+
+ if (t->seg->i->n_poscombs==1)
+ goto movement_ok;
+
+ if (t->seg->moving) {
+ if (!t->seg->owner)
+ return safety_problem(u->train, t->seg, "route not yet set");
+no good if we are trying to accelerate - may get there too soon
+ goto movement_ok;
+ }
+
+ /* Maybe we can set it apropriately, or plan to. */
+
+ if (t->seg->pred_vacated) {
+ /* We're just predicting that we'll have it vacated. */
+ MovPosChange *newres;
+
+ if (t->seg->move_reservation && !u->accelerating)
+ goto movement_ok;
+
+need to calculate new intended movposcomb and remember it
+so we get right answer next time
+ ec= movpos_reserve(t->seg, u->elapsed - t->seg->until, &newres);
+ if (ec) return safety_problem(u->train, t->seg, "cannot promise to"
+ " set route in time: %s", ec2str(ec));
+ movpos_unreserve(t->seg->move_reservation);
+ t->seg->move_reservation= newres;
+ } else {
+ /* Hah! We can change it! */
+ /* This also copes with the case where in fact it was set right
+ * all along. In that case movpos's minimisation algorithm will
+ * chose to implement our request without any actual motions. */
+ ec= movpos_change_bysegs(before,t->seg,0, t->seg->until,
+ t->seg->move_reservation);
+stash intended movposcomb in new field in Segment
+need to make us owner now as well
+ownership should last until it stops moving at least
+ no actually this is still wrong
+ t->seg->move_reservation= 0;
+ if (ec) return safety_problem(u->train, t->seg, "cannot"
+ " set route in time: %s", ec2str(ec));
+ }
+
+ movement_ok:
+ t->seg->pred_present= 1;
+ t->seg->until= u->elapsed;
+
+ adjust
+
+ &
+
+
+ const MovPosCombInfo *pci;
+ const SegmentLinkInfo *link;
+
+ r= trackloc_getlink(t,0,0,&link,-1,1);
+ if (!r && link->next==before)
+ goto movement_ok;
+
+
+
+ pci= t->seg->i->poscombs[
+ link= t->backwards ? &tryi->forwards : &tryi->backwards;
+
+ MovPosComb only_ok= -1, try;
+ for (try=0, tryi=t->seg->i->poscombs;
+ try < t->seg->i->n_poscombs; try++, tryi++) {
+ link= t->backwards ? &tryi->forwards : &tryi->backwards;
+ if (link->next != before) continue;
+ if (only_ok >= 0) { only_ok=-1; goto several_combs_ok; }
+ only_ok= try;
+ }
+ assert(only_ok >= 0); /* otherwise graph is full of lies */
+ several_combs_ok:
+
+ if (t->seg->movposcomb < 0 || t->seg->movposcomb != only_ok) {
+
+ else if (t->seg->movposcomb < 0) {
+
+
+ return safety_problem(
+
+ t->seg->pred_present= 1;
+
+
+ safety_problem
+
+static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
+ Segment *next, MovPosComb *mpc_io) {
+ PredictUserContext *u= c->u;
+ int advanced;
+
+ advanced= c->u->was_distance - c->distance;
+
+ u->nosec.distance= advanced;
+ r= trackloc_advance(&u->nose,&u->nosec); if (r) return r;
+
+tally add new segment
+
+ u->tailc.distance= advanced;
+ r=
+
+void predict_and_provide(Train *tra) {
+ PredictUserContext u;
+
+ memset(&u,0,sizeof(u));
+
+ trackloc_set_maxinto(&u.fdet, tra->foredetect,
+ tra->foredetect->tr_backwards);
+ trackloc_set_maxinto(&u.tail, tra->foredetect,
+ !tra->foredetect->tr_backwards);
+
+ /* find the train's tail and mark it present */
+
+ u.tail.seg->pred_present= 1;
+
+ u.tailc.distance= tra->detectable + (tra->backwards ? tra->head : tra->tail);
+ u.tailc.nextseg= initpresent_nextseg;
+ u.tailc.trackend= 0;
+ u.tailc.getmovpos= 0;
+ u.tailc.u= &u;
+
+ r= trackloc_advance(&u.tail,&u.tailc); assert(!r);
+ trackloc_reverse_exact(&u.tail,0);
+
+ /* find the train's nose */
+
+ u.nose= fdet;
+ u.nosec.distance= tra->detectable + (tra->backwards ? tra->tail : tra->head);
+ u.nosec.nextseg= nose_nextseg;
+ u.nosec.trackend= pred_trackend;
+ u.nosec.getmovpos= pred_getmovpos;
+ u.nosec.u= &u;
+
+ r= trackloc_advance(&u.nose,&u.nosec);
+ if (r) return r;
+
+ /* predict the future */
+
+ fdetc.distance= u.was_distance= INT_MAX;
+ fdetc.nextseg= fdet_nextseg;
+ fdetc.trackend= pred_trackend;
+ fdetc.getmovpos= pred_getmovpos;
+ fdetc.u= &u;
+
+ r= trackloc_advance(&fdet,&fdetc);
+
+
+predict the future
+ fdet.seg= tra->foredetect;
+
+ fdet.into= 0;
+
+ tail.seg= tra->foredetect;
+ tail.into= 0;
+ tail.backwards=
+
+void safety_notify_detection(Segment *seg) {
+ if (seg->det_ignore) return;
+ if (!seg->det_expected)
+ safety_panic(0,seg, "unexpected detection");
+
+ Train *tra= seg->owner;
+
+ seg_clear_marks();
+ update_head(tra,seg);
+ predict_and_provide(tra);
+
+
+
+ /* ============ OLD CODE ========== */
static void seg_clear_stale(Segment *seg) {
seg->owner= 0;
ErrorCode ec;
TrackLocation tloc;
- if (!seg->owner)
- safety_panic(0,seg, "unexpected detection");
-
if (!seg->until_detect)
return;
/* Location: */
struct Segment *foredetect; /* train's detectable part is at most maxinto */
- Distance maxinto, uncertainty; /* into foredetect but train may be */
+ // 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 */
* points_all_abandon is called (see startup.c);
*/
-void movpos_unreserve(MovPosChange *reservation);
+void movpos_unreserve(MovPosChange *reservation /* 0 ok */);
MovPosComb movpos_poscomb_actual(Segment *seg);
/* gives actual current position as published by movfeatkind
typedef struct TrackLocation TrackLocation;
struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
Segment *seg; /* current segment */
- Segment *err; /* segment where error happens (eg, movposcomb wrong) */
- long into; /* distance from start of segment */
+ long remain; /* distance from end of segment as we look at it */
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.
- * Returns -1 if current track log has unknown movposcomb.
+void trackloc_set_mininto(TrackLocation *tloc, Segment *seg, int backwards);
+void trackloc_set_maxinto(TrackLocation *tloc, Segment *seg, int backwards);
+
+typedef struct { /* all set by caller, only distance modified by trackloc: */
+ int distance;
+ int nextseg(TrackLocation *t, TrackAdvanceContext *c,
+ Segment *next, MovPosComb *mpc_io);
+ /* On entry *mpc_io is from next->movposcomb. nextseg may modify
+ * it if it feels like it in which case the modified value will
+ * be used instead. If on 0-return it is still -1, getmovpos is used */
+ int trackend(TrackLocation *t, TrackAdvanceContext *c);
+ int getmovpos(TrackLocation *t, TrackAdvanceContext *c, MovPosComb *use_r);
+ /* May be called
+ * - at the start of trackloc_advance if current segment position unknown
+ * - after nextseg has failed to determine position of next segment
+ * - by trackloc_getlink
+ * it _must_ either return non-0 or set *use_r to a valid value
+ */
+ void *u;
+} TrackAdvanceContext;
+
+int trackloc_getlink(TrackLocation *t, TrackAdvanceContext *c,
+ const SegPosCombInfo **pci_r /* 0 ok */,
+ const SegmentLinkInfo **link_r /* 0 ok */,
+ MovPosComb mpc /* -1: get from segment or getmovpos */);
+ /* Of c, only c->getmovpos is used. c may be 0 in which case we
+ * pretend getmovpos returned -1. */
+
+int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c);
+ /* Advances t by dist until either some callback returns non-0
+ * or distance becomes 0. nextseg and getmovpos callbacks may be 0.
+ * If trackend callback is 0
*/
+
int trackloc_further(TrackLocation *tloc, long *remain_io);
/* Advances tloc, decrementing *remain_io, until one of the
* following happens: err set to
#include "realtime.h"
-const SegPosCombInfo *trackloc_segposcomb(const TrackLocation *tloc) {
- Segment *seg= tloc->seg;
+typedef TrackLocationAdvanceCallbacks Calls;
- if (seg->movposcomb<0) return 0;
- assert(seg->movposcomb < seg->i->n_poscombs);
- return &seg->i->poscombs[seg->movposcomb];
+/*---------- constructors/initialisers ----------*/
+
+void trackloc_set_mininto(TrackLocation *tloc, Segment *seg, int backwards) {
+ const SegPosCombInfo *pci;
+
+ tloc->seg= seg;
+ tloc->backwards= backwards;
+ pci= trackloc_segposcomb(tloc);
+ tloc->remain= pci->dist;
}
-const SegmentLinkInfo *trackloc_segmentlink(const TrackLocation *tloc,
- const SegPosCombInfo *pci,
- unsigned far) {
- return (tloc->backwards ^ far) ? &pci->forwards : &pci->backwards;
+void trackloc_set_maxinto(TrackLocation *tloc, Segment *seg, int backwards) {
+ tloc->seg= seg;
+ tloc->backwards= backwards;
+ tloc->into= 0;
}
-long trackloc_remaininseg(const TrackLocation *tloc) {
+/*---------- enquirers ----------*/
+
+int trackloc_getlink(TrackLocation *t, TrackAdvanceContext *c,
+ const SegPosCombInfo **pci_r,
+ const SegmentLinkInfo **link_r,
+ MovPosComb mpc, int backend) {
+ const SegPosCombInfo *pci;
+ MovPosComb mpc;
+
+ if (mpc<0) {
+ mpc= t->movposcomb;
+ }
+ if (mpc<0 && c) {
+ if (c) {
+ if (!c->getmovpos)
+ c->panic(t,c,"unexpected lack of definite track route");
+
+ r= c->getmovpos(t,c, &mpc);
+ if (r) return r;
+ }
+ }
+ if (mpc<0) {
+ *pci_r=0;
+ *link_r=0;
+ return -1;
+ }
+ assert(mpc < seg->i->n_poscombs);
+ pci= &seg->i->poscombs[seg->movposcomb];
+ if (*pci_r) *pci_r= pci;
+ if (*link_r) *link_r= (tloc->backwards ^ backend)
+ ? &pci->backwards : &pci->forwards;
+ return 0;
+}
+
+/*---------- mutator ----------*/
+
+int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c) {
+ Segment *next, *save;
+ const SegPosCombInfo *pci;
+ const SegmentLinkInfo *link;
+ MovPosComb mpc_nego;
+
+ r= getsegmentlink(t,c, &pci,&link, t->seg.movposcomb);
+ if (r) return r;
+
+ for (;;) {
+ if (!c->distance) return 0;
+
+ if (t->remain) {
+ use= t->remain < *distance ? t->remain : *distance;
+ t->remain -= use;
+ *distance -= use;
+ } else {
+ next= link->next;
+
+ if (!SOMEP(next)) {
+ if (!c->trackend) c->panic(t,c,"unexpected end of track");
+ return c->trackend(t,c);
+ }
+
+ leaving= t->seg;
+ t->seg= next;
+ t->backwards ^= link->next_backwards;
+ mpc_nego= next->movposcomb;
+
+ if (c->nextseg) {
+ r= calls->nextseg(t,c, &mpc_nego,leaving);
+ if (r) return r;
+ }
+
+ r= getsegmentlink(t,c, &pci,&link, mpc_nego);
+ if (r) { t->seg=save; return r; }
+
+ t->remain= link->dist;
+ }
+ }
+}
+
+/* ============ OLD CODE ============= */
+
+
+
+
+xxx long trackloc_remaininseg(const TrackLocation *tloc) {
const SegPosCombInfo *pci;
long segment_len;
return segment_len - tloc->into;
}
-int trackloc_further(TrackLocation *tloc, long *remain_io) {
+xxx long trackloc_remaininseg_poscomb(const SegPosCombInfo *pci) {
+ segment_len= pci->dist;
+ assert(tloc->into <= segment_len);
+ return segment_len - tloc->into;
+}
+
+
+void trackloc_reverse_inexact(TrackLocation *tloc) {
+ tloc->remain= trackloc_segmentlink(tloc) - tloc->remain;
+ tloc->backwards ^= 1;
+}
+
+
+int trackloc_further(TrackLocation *tloc, long *remain_io, Segment *err_r) {
const SegPosCombInfo *pci;
const SegmentLinkInfo *lnki_far;
Segment *nextseg;
long segment_remain;
- segment_remain= trackloc_remaininseg(tloc);
- if (segment_remain<0) { tloc->err=tloc->seg; return -3; }
if (*remain_io <= segment_remain) {
tloc->into += *remain_io;
tloc->into += segment_remain;
pci= trackloc_segposcomb(tloc);
- lnki_far= trackloc_segmentlink(tloc, pci, 1);
- if (!SOMEP(lnki_far->next)) { tloc->err=tloc->seg; return -2; }
+ lnki_far= trackloc_segmentlink(tloc, pci, 0);
+ if (!SOMEP(lnki_far->next)) { *err_r=tloc->seg; return -2; }
nextseg= &segments[lnki_far->next];
- if (nextseg->movposcomb<0) { tloc->err=nextseg; return -1; }
+ if (nextseg->movposcomb<0) { *err_r=nextseg; return -1; }
tloc->seg= nextseg;
tloc->into= 0;
}
}
-void trackloc_reverse(TrackLocation *tloc) {
- tloc->into= trackloc_remaininseg(tloc);
- tloc->backwards ^= 1;
-}
+
~ijackson / trains.git / commitdiff