From: ian Date: Sun, 20 Apr 2008 10:48:51 +0000 (+0000) Subject: wip new safety X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?a=commitdiff_plain;h=77a2e0673dde048a48abe50d57c4563906029b14;p=trains.git wip new safety --- diff --git a/hostside/safety.c b/hostside/safety.c index d676e8a..64c54a2 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -27,18 +27,18 @@ Segment *segments; * 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 @@ -58,7 +58,7 @@ Segment *segments; * 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 @@ -89,8 +89,7 @@ Segment *segments; * 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: * @@ -140,7 +139,222 @@ Segment *segments; * 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; @@ -395,9 +609,6 @@ void safety_notify_detection(Segment *seg) { ErrorCode ec; TrackLocation tloc; - if (!seg->owner) - safety_panic(0,seg, "unexpected detection"); - if (!seg->until_detect) return; diff --git a/hostside/safety.h b/hostside/safety.h index 7b2820d..36481b7 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -38,7 +38,7 @@ struct Train { /* 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 */ @@ -171,7 +171,7 @@ movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r); * 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 @@ -228,16 +228,45 @@ void actual_inversions_done(void); 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 diff --git a/hostside/trackloc.c b/hostside/trackloc.c index 20c6770..2553511 100644 --- a/hostside/trackloc.c +++ b/hostside/trackloc.c @@ -5,21 +5,109 @@ #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; @@ -30,14 +118,25 @@ long trackloc_remaininseg(const TrackLocation *tloc) { 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; @@ -48,11 +147,11 @@ int trackloc_further(TrackLocation *tloc, long *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; @@ -61,8 +160,5 @@ int trackloc_further(TrackLocation *tloc, long *remain_io) { } } -void trackloc_reverse(TrackLocation *tloc) { - tloc->into= trackloc_remaininseg(tloc); - tloc->backwards ^= 1; -} +