From 09dfe1d5a14818aac6e42af9f5610efe16a2b208 Mon Sep 17 00:00:00 2001 From: ian Date: Sun, 20 Apr 2008 19:33:24 +0000 Subject: [PATCH] wip new safety; getting confused about polarity --- hostside/movpos.c | 7 +- hostside/safety.c | 201 +++++++++++++++++++++++++++++++++++----------- hostside/safety.h | 3 + 3 files changed, 165 insertions(+), 46 deletions(-) diff --git a/hostside/movpos.c b/hostside/movpos.c index 973d583..5d2235b 100644 --- a/hostside/movpos.c +++ b/hostside/movpos.c @@ -315,8 +315,13 @@ fprintf(stderr," point confirm\n"); if (n_motions >= r->n_motions) return EC_MovFeatReservationInapplicable; assert(n_motions <= r->n_motions); - newdeadline= pt_maxdelay_reldeadline(maxdelay_ms) + pt_cslot; + if (maxdelay_ms == -1) { + newdeadline= r->deadline; + } else { + newdeadline= pt_maxdelay_reldeadline(maxdelay_ms); + } allow_failure= newdeadline < r->deadline; + newdeadline += pt_cslot; /* state A or R */ pt_dequeue(r); diff --git a/hostside/safety.c b/hostside/safety.c index ee93426..2d6a35a 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -50,8 +50,7 @@ Segment *segments; * us. (We check this before updating our tail trackloc as below.) * In that case we update the segment's until.) (We use the value * `elapsed' which is the minimum time until the current nose - * prediction; if we've only just detected the train at foredetect - * we can count the time for the + * prediction.) * * However it may be a segment which we currently own, or perhaps it * may be set against us. (If it's owned by anyone else or is not @@ -103,7 +102,7 @@ Segment *segments; * * owner train which owns this for signalling purposes * no other train may interfere - * + * * physical in an unowned segment, this has no particular * feature relationship to anything * & @@ -122,20 +121,20 @@ Segment *segments; * If we succeed we will end up * owning the segment, and if we fail * we garbage collect it - * + * * det_ignore this is part of the body of a train which we've * already detected here - * + * * det_expected this is the next place we expect owner train * to arrive * (it is not valid to have both det_ignore * and _expected) - * + * * VALID DURING PREDICTION ETC. ONLY - * + * * pred_present in our prediction, the train physically occupies this * until_here tells us when will be - * + * * pred_vacated in our prediction, the train does not occupy this * although it did in previous predictions * until_here tells us when it will have been vacated @@ -146,23 +145,57 @@ Segment *segments; * iff either of these is set, this train is going * to own the segment when we're finished * + * now_present train is in this segment now (in which we include + * the segment just before foredetect) + * * until time until the we will arrive (pred_present), or * depart (pred_vacated), valid only if * one of those is set * value is as for u->elapsed - */ + * + * Valid combinations during prediction: + * + * now_present + * | pred_present + * |/ vacated + * ||/ motion && + * ||| !moving + * NPV until means + * + * has never been here as far as we know --- undef + * here now, prediction has it still here NP- arrival = 0 old, defer + * here now, but predicted to have departed N-V departure old, defer + * pred. to have arrived here -P- arrival to do now + * pred. to have arrived and departed --V departure to do now + * here now, pred. to have dep'd and ret'd NPV re-arrival new, defer + * now absent, pred. to have arr, dep & ret'd -PV re-arrival forbidden + * + * invalid/impossible combination: + * train here now but never in prediction N-- + * + * `old' means a motion reservation which is part of the path we + * committed to in an earlier prediction but which we have not yet + * reached in this prediction. + * + * (These are the values when not in the middle of nose_nextseg.) */ /*========== prediction machinery ==========*/ -#define pred_present mark0 -#define pred_vacated mark1 +#define now_present mark0 +#define pred_present mark1 +#define pred_vacated mark2 +#define will_polarise mark3 typedef void PredictionProblemCallback(void *pu, Train *tra, TrackSegment *seg, const char *fmt, va_list al); typedef struct { unsigned - accelerating:1; + accelerating:1, + walk_compute_polarise:1, /* nose_nextseg still needs to worry */ + need_polarise:1, /* when we commit */ + train_polarity_inverted:1, /* right now */ + know_best_polarity; /* longest-lasting into the future */ TrackLocation nose, fdet, tail; TrackAdvanceContext nosec, tailc, fdetc; TimeInterval elapsed; /* from now, minimum */ @@ -170,7 +203,7 @@ typedef struct { PredictionProblemCallback *problem_callback; void *problem_callback_u; - + /* 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) */ @@ -200,7 +233,8 @@ static int pred_getmovpos(TrackLocation *t, TrackAdvanceContext *c, MovPosComb *use_io) { PredictUserContext *u= c->u; if (t->seg->motion) *use_io= movpos_change_intent(t->seg->motion); - if (*use_io<0) return panic(tra,seg, "track route unexpectedly not known"); + if (*use_io<0) safety_panic(tra,seg, "track route unexpectedly not known"); + return 0; } static int pred_trackend(TrackLocation *t, TrackAdvanceContext *c) { @@ -215,9 +249,9 @@ static int pred_trackend_panic(TrackLocation *t, TrackAdvanceContext *c) { static int initpresent_nextseg(TrackLocation *t, TrackAdvanceContext *c, MovPosComb *mpc_io, Segment *before) { PredictUserContext *u= c->u; - next->pred_present= 1; + next->now_present= next->pred_present= next->will_polarise= 1; next->until= 0; - adjust_tally(t,c,1); + u->noninv_tally[!t->backwards]++; /* ! since going backwards along train */ } /*---------- prediction nose advancement ----------*/ @@ -227,6 +261,7 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, PredictUserContext *u= c->u; MovPosComb route_plan; MovPosChange *route_change; + TimeInterval max_ms; /* Is it empty ? */ @@ -251,12 +286,40 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, interferer->i->pname); } + /* Check polarity */ + + if (u->walk_compute_polarise) { + if (!u->need_polarise) { + /* this is the very next segment as we only do this once: */ + u->walk_compute_polarise= u->need_polarise= + (t->seg->seg_inverted ^ t->seg->tr_backwards ^ + u->train_polarity_inverted); + } + if (t->seg->will_polarise) + /* this is our 2nd visit, stop now */ + u->walk_compute_polarise= 0; + + if (u->walk_compute_polarise) + seg->will_polarise= 1; + + if (!u->know_best_polarity && !t->seg->i->invertible) { + u->know_best_polarity= 1; + u-> + + + if ( + + int train_inverted_here= + + } + /* What about the route ? */ if (t->seg->i->n_poscombs==1) goto movement_ok; - + if (t->seg->moving) { + assert(!t->seg->now_present); if (!t->seg->owner) return predict_problem(u, t->seg, "route not yet set"); if (!u->accelerating) { @@ -280,15 +343,22 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, } if (route_plan == *mpc_io && !t->seg->moving) { /* Well, that's all fine then. */ - movpos_unreserve(t->seg->motion); /* we don't need this */ + movpos_unreserve(t->seg->motion); /* we don't need this whatever it is */ t->seg->motion= 0; goto movement_ok; } /* We have a plan but it involves some motion: */ - ec= movpos_reserve(t->seg, - !t->seg->pred_vacated ? u->elapsed - : u->elapsed - t->seg->until, - &route_reservation, route_plan, *mpc_io); + if (t->seg->pred_vacated) { + if (!t->seg->now_present) + /* And we're not even there yet! This is too hard because we + * are too stupid not to change it straight away. */ + return predict_problem(u, t->seg, "dynamic route too complicated" + " - would need multiple queued changes"); + max_ms= u->elapsed - t->seg->until; + } else { + max_ms= u->elapsed; + } + ec= movpos_reserve(t->seg, max_ms, &route_reservation, route_plan, *mpc_io); if (ec) return predict_problem(u, t->seg, "cannot promise to" " set route: %s", ec2str(ec)); @@ -327,6 +397,12 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, PredictUserContext *u= c->u; int advanced; + /* NB, on entry the movposcomb of the segment we're entering (and + * other similar details) may not be known or sufficient because + * when fdet has only just entered a new segment, nose is still in + * the previous segment. + */ + advanced= u->was_distance - c->distance; if (u->count_time) { @@ -344,6 +420,13 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, if (r) return r; + /* Now we have advanced the nose and have recoreded any appropriate + * motion(s). But we advancing the nose has updated the segment's + * notion of the motion but not trackloc_advance's cached version of + * the movposcomb in *mpc_io. That doesn't matter because we don't + * actually use it ourselves and trackloc_advance will call + * getmovpos. */ + if (!t->seg->invertible) { u->noninv_tally[t->backwards]++; if (u->noninv_tally[0] && u->noninv_tally[1]) @@ -357,31 +440,37 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, c->distance= speedmanager_stoppingdistance(u->train); u->was_distance= c->distance; - u->count_time= 1; + u->count_time= 1; /* we start counting time only after we've done the + * whole foredetect segment as we may already have + * nearly finished it */ return 0; } /*---------- prediction entrypoint ----------*/ -static ErrorCode predict_confirm(Train *tra, int accelerate, int justdetected, +static ErrorCode predict_confirm(Train *tra, int accelerate, PredictionProblemCallback *ppc, void *ppcu) { /* Caller must call this with different situations until it succeeds! */ PredictUserContext u; + Segment *foredet; FOR_SEG { - seg->pred_present= seg->pred_vacated= 0; + seg->now_preset= seg->pred_present= + seg->pred_vacated= seg->choose_invert= 0; } + foredet= tra->foredetect; + memset(&u,0,sizeof(u)); u.accelerating= accelerate; - u.count_time= justdetected; /* if just detected we have whole fdet segment */ + u.walk_will_polarise= 1; + u.need_polarise= 0; + u.train_polarity_inverted= foredet->seg_inverted ^ foredet->tr_backwards; u.problem_callback= ppc; u.problem_callback_u= ppcu; - trackloc_set_maxinto(&u.fdet, tra->foredetect, - tra->foredetect->tr_backwards); - trackloc_set_maxinto(&u.tail, tra->foredetect, - !tra->foredetect->tr_backwards); + trackloc_set_maxinto(&u.fdet, foredet, foredet->tr_backwards); + trackloc_set_maxinto(&u.tail, foredet, !foredet->tr_backwards); /* find the train's tail and mark it present */ @@ -398,11 +487,13 @@ static ErrorCode predict_confirm(Train *tra, int accelerate, int justdetected, /* find the train's nose */ u.nose= fdet; - u.nosec.distance= tra->detectable + (tra->backwards ? tra->tail : tra->head); + u.nosec.distance= speedmanager_nosesafetmargin(u->train) + + (tra->backwards ? tra->tail : tra->head); u.nosec.nextseg= nose_nextseg; u.nosec.getmovpos= pred_getmovpos; u.nosec.trackend= pred_trackend; u.nosec.u= &u; + u.count_time= 0; r= trackloc_advance(&u.nose,&u.nosec); if (r) return r; @@ -422,32 +513,52 @@ static ErrorCode predict_confirm(Train *tra, int accelerate, int justdetected, /*----- commit to the plan -----*/ + trackloc_set_maxinto(&u.fdet, foredet, + foredet->tr_backwards); + u.fdetc.distance= INT_MAX; + u.fdetc.nextseg= polarise_nextseg; + u.fdetc.trackend= polarise_trackend; + + r= trackloc_advance(&fdet,&fdetc); + FOR_SEG { if (seg->owner == tra) { - seg->det_ignore= seg->det_expect= 0; + seg->det_ignore= seg->det_expected= 0; seg->owner= 0; } if (seg->pred_present || seg->pred_vacated) { seg->owner= tra; - if (!seg->until) - seg->det_ignore= 1; + seg->det_ignore= seg->now_present; + } + if (seg->motion && !seg->moving && !seg->now_present) { + MovPosChange *reservation= seg->motion; + MovPosComb target= movpos_change_intent(reservation); + seg->motion= 0; + ec= movpos_change(seg, target, -1, reservation); + assert(!ec); } - + movpos_unreserve(seg->motion); + seg->motion= 0; + } else if (seg->pred_vacated + + } +} + predict the future - fdet.seg= tra->foredetect; + fdet.seg= foredet; fdet.into= 0; - tail.seg= tra->foredetect; + tail.seg= foredet; tail.into= 0; - tail.backwards= + tail.backwards= 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"); -} +} void safety_notify_detection(Segment *seg) { if (seg->det_ignore) return; @@ -458,7 +569,7 @@ void safety_notify_detection(Segment *seg) { update_head(tra,seg); predict_and_provide(tra); - + /* ============ OLD CODE ========== */ @@ -485,7 +596,7 @@ static void lay_train_check_clash(LayTrainState *l, Segment *check, Segment *report) { const char *exi1="", *exi2=""; if (check!=report) { exi1= " at "; exi2= check->i->pname; } - + if (check->tr_updated) { l->ec= safety_problem(l->tra, report, "self-collision%s%s", exi1,exi2); return; @@ -598,7 +709,7 @@ fprintf(stderr,"lay_train_pass further=%d tloc=@%s+%ld remain=%ld\n", static void lay_train_inversions(LayTrainState *l) { SEG_IV; int train_be_inverted, seg_be_inverted; - + if (l->ec) return; train_be_inverted= l->invert_count[1] > l->invert_count[0]; @@ -621,7 +732,7 @@ static void lay_train_inversions(LayTrainState *l) { static void lay_train_done(LayTrainState *l) { SEG_IV; - + FOR_SEG { if (seg->owner == l->tra) { if (!seg->tr_updated) seg_clear_stale(seg); @@ -652,7 +763,7 @@ fprintf(stderr,"lay_train %s @%s added_slop=%ld maxinto=%ld " l.tra= tra; l.ec= 0; l.invert_count[0]= l.invert_count[1]= 0; - + head= tra->backwards ? tra->tail : tra->head; headslop= head + added_slop; @@ -753,7 +864,7 @@ ErrorCode safety_requestspeed(Train *tra, long newspeed) { if (ec) { ErrorCode revert_ec; - + if (oldspeed && oldspeed < newspeed) { logmsg(ec,tra,0, "countermanded acceleration" " from %ld to %ld", oldspeed, newspeed); diff --git a/hostside/safety.h b/hostside/safety.h index d5edc66..777b356 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -149,6 +149,9 @@ movpos_change(Segment *tomove, MovPosComb target, * allowed to know implementation details and may know what * movposcomb means for certain segments depending on the movement * requested.) + * + * If a reservation is supplied, maxdelay_ms may be -1 to use the + * same value as was given to movpos_reserve. */ ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd, -- 2.30.2