From 19cd78b1c227d2a95de014b98b303e719e897317 Mon Sep 17 00:00:00 2001 From: ian Date: Mon, 5 May 2008 10:36:25 +0000 Subject: [PATCH] extensive trackloc change (calls nextseg early, etc.) and bugfixes --- hostside/resolve.c | 7 +- hostside/runes | 4 +- hostside/safety.c | 278 ++++++++++++++++++++++++-------------------- hostside/safety.h | 16 +-- hostside/trackloc.c | 65 +++++++---- 5 files changed, 208 insertions(+), 162 deletions(-) diff --git a/hostside/resolve.c b/hostside/resolve.c index 35f6e76..1909396 100644 --- a/hostside/resolve.c +++ b/hostside/resolve.c @@ -381,7 +381,7 @@ static void ends_callback_debug(const char *what, TrackLocation *t, } static int findhead_nextseg(TrackLocation *t, struct TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before) { + MovPosComb *mpc_io, const TrackLocation *before) { FindEndUserContext *u= c->u; ends_callback_debug("findhead_nextseg",t,c); @@ -397,7 +397,7 @@ static int findhead_nextseg(TrackLocation *t, struct TrackAdvanceContext *c, } static int walkback_nextseg(TrackLocation *t, struct TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before) { + MovPosComb *mpc_io, const TrackLocation *before) { ends_callback_debug("walkback_nextseg",t,c); if (!t->seg->resfin_ours) return -1; @@ -411,7 +411,6 @@ static int resolve_complete_ends_train(Train *tra) { TrackLocation t; TrackAdvanceContext tc; FindEndUserContext u; - int r; Segment *startpoint; switch (tra->resolution) { @@ -455,7 +454,6 @@ static int resolve_complete_ends_train(Train *tra) { oprintf(DUPO("resolving") " ends start=%s%s es=%d\n", t.backwards?"-":"", t.seg->i->pname, u.extraspace); - r= findhead_nextseg(&t,&tc,0,0); assert(!r); trackloc_advance(&t,&tc); oprintf(DUPO("resolving") " ends es=%d furthest=%s fd=%s%s fd.dist=%d\n", @@ -478,7 +476,6 @@ static int resolve_complete_ends_train(Train *tra) { tc.distance= tra->detectable + tra->tail + MARGIN_NOSE; tc.nextseg= walkback_nextseg; - r= walkback_nextseg(&t,&tc,0,0); assert(!r); trackloc_advance(&t,&tc); oprintf(DUPO("resolving") " ends maxi=%d unc=%d;" diff --git a/hostside/runes b/hostside/runes index 408de55..dbf5ef2 100644 --- a/hostside/runes +++ b/hostside/runes @@ -1,3 +1,5 @@ -./realtime shinkansen.speeds.record homes.record + ./realtime shinkansen.speeds.record homes.record +gdb -x xs.gdb valgrind --db-attach=yes ./realtime -Lt -S+realtime.log shinkansen.speeds.record homes.record + diff -u +realtime.log t |less diff --git a/hostside/safety.c b/hostside/safety.c index 2d9b6b4..62044bd 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -245,6 +245,7 @@ typedef struct { TrackAdvanceContext nosec, tailc, fdetc; TimeInterval elapsed; /* from now, minimum */ Distance was_distance, autopoint_distance; + double maxspeed; Segment *hindmost, *furthest; PredictionProblemCallback *problem_callback; @@ -259,6 +260,13 @@ typedef struct { static int nose_length(Train *tra) { return MARGIN_NOSE + (tra->backwards ? tra->tail : tra->head); } +static void advance_elapsed(PredictUserContext *u, int advance_distance) { + u->elapsed += advance_distance / u->maxspeed; +} +static int calc_advanced(TrackAdvanceContext *c) { + PredictUserContext *u= c->u; + return u->was_distance - c->distance; +} /*---------- prediction problem reporting ----------*/ @@ -292,21 +300,30 @@ static ErrorCode predict_problem(PredictUserContext *u, Segment *seg, } static void pred_callback_debug(const char *what, TrackLocation *t, - struct TrackAdvanceContext *c) { + struct TrackAdvanceContext *c, + const TrackLocation *before) { PredictUserContext *u= c->u; oprintf(DUPO("safety") " predict %s" - " %s%s until=%ld dist=%d (was %d) %c%c%c.%c %c%c%c%c.%c%c%c%c" + " %s%s dist=%-4d until=%-4ld %c%c%c.%c (was %s%s dist=%-4d)" + " %c%c%c%c.%c%c%c%c" " elapsed=%ld nit=%d,%d\n", what, - t->backwards?"-":"", t->seg->i->pname, (long)t->seg->until, - c->distance, u->was_distance, - + + t->backwards?"-":"", + t->seg->i->pname, + c->distance, + (long)t->seg->until, + "-N"[ t->seg->now_present ], "-P"[ t->seg->pred_present ], "-V"[ t->seg->pred_vacated ], - "-#"[ t->seg->will_polarise ], + "-p"[ t->seg->will_polarise ], + + before && before->backwards?"-":"", + before ? before->seg->i->pname : "-", + u->was_distance, "-t"[ u->count_time ], "-a"[ u->accelerating ], @@ -345,7 +362,8 @@ static int pred_trackend_panic(TrackLocation *t, TrackAdvanceContext *c) { } static int initpresent_nextseg(TrackLocation *t, TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before) { + MovPosComb *mpc_io, + const TrackLocation *before) { PredictUserContext *u= c->u; t->seg->now_present= t->seg->pred_present= t->seg->will_polarise= 1; t->seg->until= 0; @@ -357,7 +375,7 @@ static int initpresent_nextseg(TrackLocation *t, TrackAdvanceContext *c, /*---------- prediction nose advancement ----------*/ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before) { + MovPosComb *mpc_io, const TrackLocation *before) { PredictUserContext *u= c->u; Segment *interferer; MovPosComb route_plan; @@ -365,7 +383,11 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, TimeInterval max_ms; ErrorCode ec; - pred_callback_debug(" nose_nextseg",t,c); + if (!before) return 0; + pred_callback_debug(" nose_nextseg",t,c,before); + + if (u->optimistic) + advance_elapsed(u,calc_advanced(c)); /* Is it empty ? */ @@ -382,14 +404,16 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, if (t->seg->pred_present) return predict_problem(u, t->seg, "will collide with itself!"); - interferer= segment_interferes(c,t->seg); - if (interferer) { - if (interferer->owner && interferer->owner != u->train) - return predict_problem(u, t->seg, "impeded by %s at %s", - interferer->owner->pname, interferer->i->pname); - if (interferer->pred_present) - return predict_problem(u, t->seg, "will collide with itself at %s!", - interferer->i->pname); + if (!u->optimistic) { + interferer= segment_interferes(c,t->seg); + if (interferer) { + if (interferer->owner && interferer->owner != u->train) + return predict_problem(u, t->seg, "impeded by %s at %s", + interferer->owner->pname, interferer->i->pname); + if (interferer->pred_present) + return predict_problem(u, t->seg, "will collide with itself at %s!", + interferer->i->pname); + } } /* What about the route ? */ @@ -416,7 +440,7 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, } } else { /* Extend the plan. */ - ec= movpos_findcomb_bysegs(before,t->seg,0,*mpc_io, &route_plan); + ec= movpos_findcomb_bysegs(before->seg,t->seg,0,*mpc_io, &route_plan); assert(!ec); /* there must be _some_ route since we're moving into t->seg */ } @@ -439,7 +463,7 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, } if (!u->train->autopoint || !t->seg->autopoint) - return predict_problem(u,t->seg,"will not automatically set point"); + return predict_problem(u,t->seg,"will not automatically set route"); ec= movpos_reserve(t->seg, max_ms, &route_reservation, route_plan, *mpc_io); if (ec) return predict_problem(u, t->seg, "cannot promise to" @@ -460,18 +484,19 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c, /*---------- prediction tail advancement ----------*/ static int tail_nextseg(TrackLocation *t, TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *leaving) { + MovPosComb *mpc_io, const TrackLocation *before) { PredictUserContext *u= c->u; - pred_callback_debug(" tail_nextseg",t,c); + if (!before) return 0; + pred_callback_debug(" tail_nextseg",t,c,before); if (!t->seg->i->invertible) - u->noninv_tally[leaving->tr_backwards]--; + u->noninv_tally[before->seg->tr_backwards]--; - if (leaving->pred_vacated) return 0; /* only vacate once */ - leaving->pred_present= 0; - leaving->pred_vacated= 1; - leaving->until= u->elapsed; + if (before->seg->pred_vacated) return 0; /* only vacate once */ + before->seg->pred_present= 0; + before->seg->pred_vacated= 1; + before->seg->until= u->elapsed; return 0; } @@ -479,7 +504,7 @@ static int tail_nextseg(TrackLocation *t, TrackAdvanceContext *c, /*---------- prediction foredetect advancement ----------*/ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before) { + MovPosComb *mpc_io, const TrackLocation *before) { PredictUserContext *u= c->u; int advanced, r; @@ -489,13 +514,13 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, * the previous segment. */ - pred_callback_debug("fdet_nextseg",t,c); + if (!before) return 0; + pred_callback_debug("fdet_nextseg",t,c,before); - advanced= u->was_distance - c->distance; + advanced= calc_advanced(c); - if (u->count_time) { - u->elapsed += advanced / speedmanager_speed_maxestimate(u->train); - } + if (u->count_time) + advance_elapsed(u,advanced); u->nosec.distance= advanced; r= trackloc_advance(&u->nose,&u->nosec); @@ -505,7 +530,7 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, /* Our very first `next segment' lookahead found the end. So we * know that our nose hasn't left this segment because that's what * the stopping distance is supposed to prove. */ - assert(before == u->train->foredetect); + assert(before->seg == u->train->foredetect); int adjust= nose_length(u->train); if (adjust > u->train->maxinto) adjust= u->train->maxinto; u->train->maxinto -= adjust; @@ -594,9 +619,11 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, /*---------- backtracking the optimistic nose ----------*/ static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before) { + MovPosComb *mpc_io, const TrackLocation *before) { PredictUserContext *u= c->u; + pred_callback_debug("optunwind_nextseg",t,c,before); + if (t->seg == u->furthest || t->seg->motion) return EC_SignallingHorizonReached; @@ -606,85 +633,7 @@ static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c, return 0; } -/*---------- reporting position and ownership ----------*/ - -void report_train_position(Train *tra) { - oprintf(UPO, "train %s %s at %s%s:%d+-%d\n", - tra->pname, tra->backwards ? "backwards" : "forwards", - tra->foredetect->tr_backwards?"-":"", - tra->foredetect->i->pname, tra->maxinto, tra->uncertainty); -} - -static int report_nextseg(TrackLocation *t, struct TrackAdvanceContext *c, - MovPosComb *mpc, Segment *before) { - PredictUserContext *u= c->u; - char flags[6]; - int r; - - if (t->seg->owner != u->train) return -1; - if (t->seg == u->hindmost && t->seg->tr_backwards == t->backwards) { - /* We've looped the loop. */ - oprintf(UPO, " ..."); - return -1; - } - - flags[0]= 0; - - if (t->seg->det_expected) - strcat(flags,"*"); - if (t->seg->det_ignore) - strcat(flags,"."); - if (t->seg == u->train->foredetect) { - strcat(flags,"!"); - u->usecurrentposn= 0; - } - - oprintf(UPO," %s%s", t->backwards?"-":"", t->seg->i->pname); - - if (t->seg->i->n_poscombs > 1) { - r= pred_getmovpos(t,c,mpc); assert(!r); assert(*mpc>=0); - oprintf(UPO,"/%s", t->seg->i->poscombs[*mpc].pname); - } - oprintf(UPO,"%s",flags); - - return 0; -} - -void report_train_ownerships(Train *tra, Segment *hindmost, - int always_use_motions) { - PredictUserContext u; - MovPosComb mpc; - int r; - - memset(&u,0,sizeof(u)); - u.train= tra; - u.hindmost= 0; - u.alwaysusemotions= always_use_motions; - u.usecurrentposn= !always_use_motions; - - /* Walk along the train printing its segments: */ - oprintf(UPO, "train %s has", tra->pname); - - u.nose.seg= hindmost; - u.nose.remain= 0; - u.nose.backwards= hindmost->tr_backwards; - - u.nosec.distance= TL_DIST_INF;; - u.nosec.nextseg= report_nextseg; - u.nosec.getmovpos= pred_getmovpos; - u.nosec.u= &u; - - mpc= u.nose.seg->movposcomb; - r= report_nextseg(&u.nose,&u.nosec,&mpc,0); assert(!r); - - u.hindmost= hindmost; - trackloc_advance(&u.nose,&u.nosec); - - oprintf(UPO, "\n"); -} - - -/*---------- prediction entrypoint ----------*/ +/*========== prediction entrypoint ==========*/ ErrorCode predict(Train *tra, int accelerate, PredictionProblemCallback *ppc, void *ppcu) { @@ -697,11 +646,12 @@ ErrorCode predict(Train *tra, int accelerate, u.train= tra; u.problem_callback= ppc; u.problem_callback_u= ppcu; + u.maxspeed= speedmanager_speed_maxestimate(u.train); - oprintf(DUPO("safety") " predict starting %s%s speed=%f (try %f, step %d%s)" - " accel=%d\n", + oprintf(DUPO("safety") " predict starting %s%s maxspeed=%f" + " (speed %f try %f, step %d%s) accel=%d\n", tra->backwards?"-":"",tra->pname, - tra->speed.speed, tra->speed.try_speed, tra->speed.step, + u.maxspeed, tra->speed.speed, tra->speed.try_speed, tra->speed.step, tra->speed.decel.running ? " decel" : "", accelerate); @@ -741,7 +691,6 @@ ErrorCode predict(Train *tra, int accelerate, u.tailc.nextseg= initpresent_nextseg; u.tailc.trackend= pred_trackend_panic; - ec= initpresent_nextseg(&u.tail,&u.tailc,0,0); assert(!ec); ec= trackloc_advance(&u.tail,&u.tailc); assert(!ec); trackloc_reverse_exact(&u.tail,0); @@ -774,19 +723,23 @@ ErrorCode predict(Train *tra, int accelerate, /* look ahead for autopoint */ - oprintf(DUPO("safety") " predict optimism %d\n", u.autopoint_distance); + oprintf(DUPO("safety") " predict optimism %d nose %s\n", + u.autopoint_distance, u.nose.seg->i->pname); if (u.autopoint_distance) { u.furthest= u.nose.seg; u.optimistic= 1; - u.nosec.distance= u.autopoint_distance; + u.was_distance= u.nosec.distance= u.autopoint_distance; trackloc_advance(&u.nose,&u.nosec); /* But actually we don't want to end up owning * segments unless we have to for motions. */ u.nose.backwards ^= 1; + u.nose.remain= 0; u.nosec.nextseg= optunwind_nextseg; + u.nosec.distance= TL_DIST_INF; + ec= trackloc_advance(&u.nose,&u.nosec); assert(ec == EC_SignallingHorizonReached); } @@ -806,19 +759,19 @@ ErrorCode predict(Train *tra, int accelerate, oprintf(UPO, " %s%s%s%c%c%c%c", seg->tr_backwards?"-":"", seg->i->pname, - seg->owner == tra ? "#" : "=", + seg->owner == tra ? "=" : + seg->owner ? "#" : "-" , "-N"[ seg->now_present ], "-P"[ seg->pred_present ], "-V"[ seg->pred_vacated ], - "-#"[ seg->will_polarise ]); + "-p"[ seg->will_polarise ]); - if (seg->owner == tra) { - seg->det_ignore= 0; - seg->owner= 0; - } if (seg->pred_present || seg->pred_vacated) { seg->owner= tra; seg->det_ignore= seg->now_present; + } else if (seg->owner == tra) { + seg->det_ignore= 0; + seg->owner= 0; } if (seg->motion && !seg->moving && !seg->now_present) { MovPosChange *reservation= seg->motion; @@ -857,6 +810,81 @@ ErrorCode predict(Train *tra, int accelerate, return ec; } +/*========== reporting position and ownership ==========*/ + +void report_train_position(Train *tra) { + oprintf(UPO, "train %s %s at %s%s:%d+-%d\n", + tra->pname, tra->backwards ? "backwards" : "forwards", + tra->foredetect->tr_backwards?"-":"", + tra->foredetect->i->pname, tra->maxinto, tra->uncertainty); +} + +static int report_nextseg(TrackLocation *t, struct TrackAdvanceContext *c, + MovPosComb *mpc, const TrackLocation *before) { + PredictUserContext *u= c->u; + char flags[6]; + int r; + + if (t->seg->owner != u->train) return -1; + if (before && + t->seg == u->hindmost && + t->seg->tr_backwards == t->backwards) { + /* We've looped the loop. */ + oprintf(UPO, " ..."); + return -1; + } + + flags[0]= 0; + + if (t->seg->det_expected) + strcat(flags,"*"); + if (t->seg->det_ignore) + strcat(flags,"."); + if (t->seg == u->train->foredetect) { + strcat(flags,"!"); + u->usecurrentposn= 0; + } + + oprintf(UPO," %s%s", t->backwards?"-":"", t->seg->i->pname); + + if (t->seg->i->n_poscombs > 1) { + r= pred_getmovpos(t,c,mpc); assert(!r); assert(*mpc>=0); + oprintf(UPO,"/%s", t->seg->i->poscombs[*mpc].pname); + } + oprintf(UPO,"%s",flags); + + return 0; +} + +void report_train_ownerships(Train *tra, Segment *hindmost, + int always_use_motions) { + PredictUserContext u; + + memset(&u,0,sizeof(u)); + u.train= tra; + u.hindmost= 0; + u.alwaysusemotions= always_use_motions; + u.usecurrentposn= !always_use_motions; + + /* Walk along the train printing its segments: */ + oprintf(UPO, "train %s has", tra->pname); + + u.nose.seg= hindmost; + u.nose.remain= 0; + u.nose.backwards= hindmost->tr_backwards; + + u.nosec.distance= TL_DIST_INF;; + u.nosec.nextseg= report_nextseg; + u.nosec.getmovpos= pred_getmovpos; + u.nosec.u= &u; + u.hindmost= hindmost; + + trackloc_advance(&u.nose,&u.nosec); + + oprintf(UPO, "\n"); +} + + /*========== entrypoints from rest of the program ==========*/ static void detection_report_problem(Train *tra, Segment *seg, diff --git a/hostside/safety.h b/hostside/safety.h index e6ab4c1..7ce4ffa 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -258,11 +258,12 @@ typedef struct TrackAdvanceContext { /* All event callbacks are optional; if not supplied we pretend * that they were a no-op which returned 0. */ int (*nextseg)(TrackLocation *t, struct TrackAdvanceContext *c, - MovPosComb *mpc_io, Segment *before); + MovPosComb *mpc_io, const TrackLocation *before); /* 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. - * t->remain is not valid on entry and should not be touched. */ + * t->remain is not valid on entry and should not be touched. + * Called once right at the start with before==0. */ int (*getmovpos)(TrackLocation *t, struct TrackAdvanceContext *c, MovPosComb *use_io); /* Will be called *use_io on entry is @@ -296,11 +297,12 @@ int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c); /* Advances t by dist until distance becomes 0, some callback * returns non-0, or sometimes 0 if we cannot continue and the * relevant callback is missing or returned 0. If we exhausted - * c->distance or reached the end of the track then *t is accurate; - * otherwise t is the segment which was problematic (passed to - * nextseg and perhaps getmovpos) and t->remain is set to -1. In - * any case c->distance is updated by the distance successfully - * traversed. */ + * c->distance then *t is the end of the counted distance, on the + * earlier of the two segments if there is a choice. Otherwise *t + * is the last location on the last sensible segment. (Ie, nextseg + * and getmovpos get a putative new segment and if they don't work + * trackloc_advance rewinds.) In any case c->distance is updated by + * the distance successfully traversed. */ int trackloc_reverse_exact(TrackLocation *t, TrackAdvanceContext *c); int trackloc_set_exactinto(TrackLocation *t, TrackAdvanceContext *c, diff --git a/hostside/trackloc.c b/hostside/trackloc.c index 19ee95c..4177c4f 100644 --- a/hostside/trackloc.c +++ b/hostside/trackloc.c @@ -64,16 +64,37 @@ int trackloc_getlink(TrackLocation *t, TrackAdvanceContext *c, /*---------- mutator ----------*/ +static int nextseg(TrackLocation *t, TrackAdvanceContext *c, + const SegPosCombInfo **pci_r, + const SegmentLinkInfo **link_r, + const TrackLocation *leaving) { + MovPosComb mpc_nego; + int r; + + mpc_nego= t->seg->movposcomb; + + if (c->nextseg) { + r= c->nextseg(t,c, &mpc_nego, leaving); + if (r) return r; + } + + r= trackloc_getlink(t,c, pci_r,link_r, mpc_nego); + if (r || !link) return r; + + return r; +} + int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c) { - Segment *next, *leaving; + Segment *next; SegmentNum nextnum; const SegPosCombInfo *pci; const SegmentLinkInfo *link; - MovPosComb mpc_nego; + TrackLocation leaving; int r; - r= trackloc_getlink(t,c, &pci,&link, t->seg->movposcomb); - if (r || !link) return r; + r= nextseg(t,c, &pci,&link, 0); + if (r) return r; + //fprintf(stderr,"advance start getlink %s%s/%s -> %s\n", // t->backwards?"-":"", t->seg->i->pname, // pci ? pci->pname : "?", @@ -86,35 +107,31 @@ int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c) { int use= t->remain < c->distance ? t->remain : c->distance; t->remain -= use; c->distance -= use; - } else { - nextnum= link->next; + continue; + } + + nextnum= link->next; - if (!SOMEP(nextnum)) { - if (c->trackend) return c->trackend(t,c); - return 0; - } - next= &segments[nextnum]; + if (!SOMEP(nextnum)) { + if (c->trackend) return c->trackend(t,c); + return 0; + } + next= &segments[nextnum]; - leaving= t->seg; - t->seg= next; - t->backwards ^= link->next_backwards; - t->remain= -1; - mpc_nego= next->movposcomb; + leaving= *t; - if (c->nextseg) { - r= c->nextseg(t,c, &mpc_nego,leaving); - if (r) return r; - } + t->seg= next; + t->backwards ^= link->next_backwards; + t->remain= -1; - r= trackloc_getlink(t,c, &pci,&link, mpc_nego); - if (r || !link) return r; + r= nextseg(t,c, &pci,&link, &leaving); + if (r) { *t= leaving; return r; } //fprintf(stderr,"advance ran getlink %s/%s -> %s\n",leaving->i->pname, // pci ? pci->pname : "?", // link && SOMEP(link->next) ? info_segments[link->next].pname : "?"); - t->remain= pci->dist; - } + t->remain= pci->dist; } } -- 2.30.2