From: ian Date: Wed, 14 May 2008 21:27:38 +0000 (+0000) Subject: redid safety prediction's handling of uncertainty X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?a=commitdiff_plain;h=3b746945b78eabf6bd149ee2218f0447b1fd355f;p=trains.git redid safety prediction's handling of uncertainty --- diff --git a/hostside/safety.c b/hostside/safety.c index b29971d..e84ffd9 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -237,17 +237,17 @@ Segment *segments; typedef struct { Train *train; unsigned - done_first_new_fdet:1, - count_time:1, accelerating:1, stopping:1, + done_first_new_fdet:1, + count_time:1, usecurrentposn:1, /* for pred_getmovpos */ - optimistic:1, /* for autopoint */ - alwaysusemotions:1, /* for report_train_ownerships only */ + optimistic:1, /* for autopoint */ walk_compute_polarise:1, /* nose_nextseg still needs to worry */ need_polarise:1, /* when we commit */ train_polarity_inverted:1, /* right now, or if know_best, the best */ - know_best_polarity:1; /* longest-lasting into the future */ + know_best_polarity:1, /* longest-lasting into the future */ + alwaysusemotions:1; /* for report_train_ownerships only */ TrackLocation nose, fdet, tail; TrackAdvanceContext nosec, tailc, fdetc; TimeInterval elapsed; /* from now, minimum */ @@ -264,8 +264,11 @@ typedef struct { int noninv_tally[2]; } PredictUserContext; +static int tail_length(Train *tra) { + return tra->backwards ? tra->head : tra->tail; +} static int nose_length(Train *tra) { - return MARGIN_NOSE + (tra->backwards ? tra->tail : tra->head); + return tra->backwards ? tra->tail : tra->head; } static void advance_elapsed(PredictUserContext *u, int advance_distance) { u->elapsed += advance_distance / u->maxspeed; @@ -312,12 +315,12 @@ static void pred_callback_debug(const char *what, TrackLocation *t, PredictUserContext *u= c->u; oprintf(DUPO("safety") " predict %s" - " %s%s dist=%-4d until=%-4ld %c%c%c.%c (was %s%s..%d dist=%-4d)" + " %c%s dist=%-4d until=%-4ld %c%c%c.%c (was %s%s..%d dist=%-4d)" " %c%c%c%c.%c%c%c%c" " elapsed=%ld nit=%d,%d\n", what, - t->backwards?"-":"", + " -"[t->backwards], t->seg->i->pname, c->distance, (long)t->seg->until, @@ -334,8 +337,8 @@ static void pred_callback_debug(const char *what, TrackLocation *t, u->was_distance, + "-2"[ u->done_first_new_fdet ], "-t"[ u->count_time ], - "-a"[ u->accelerating ], "-c"[ u->usecurrentposn ], "-o"[ u->optimistic ], @@ -533,18 +536,6 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, u->nosec.distance= advanced; r= trackloc_advance(&u->nose,&u->nosec); - if (r == EC_SignallingHorizonReached && - u->was_distance==TL_DIST_INF) { - oprintf(DUPO("safety") " predict fdet_nextseg horizon\n"); - /* 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->seg == u->train->foredetect); - int adjust= nose_length(u->train); - if (adjust > u->train->maxinto) adjust= u->train->maxinto; - u->train->maxinto -= adjust; - u->train->uncertainty += adjust; - } if (r) { oprintf(DUPO("safety") " predict fdet_nextseg r=%s\n",ec2str(r)); return r; @@ -574,12 +565,10 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, u->train_polarity_inverted); } - if (u->was_distance == TL_DIST_INF) { - if (u->train->autopoint) { - const SegPosCombInfo *pci; - r= trackloc_getlink(t,c,&pci,0,-1); assert(!r); - u->autopoint_distance= pci->dist; - } + if (u->train->autopoint && !u->autopoint_distance) { + const SegPosCombInfo *pci; + r= trackloc_getlink(t,c,&pci,0,-1); assert(!r); + u->autopoint_distance= pci->dist; } if (u->walk_compute_polarise) { @@ -615,9 +604,6 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c, /* Final adjustments, prepare for next iteration */ - if (u->was_distance == TL_DIST_INF) - c->distance= u->stopping_distance; - u->was_distance= c->distance; u->count_time= 1; /* we start counting time only after we've done the * whole foredetect segment as we may already have @@ -644,7 +630,7 @@ static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c, /*========== prediction entrypoint ==========*/ -ErrorCode predict(Train *tra, int accelerate, +ErrorCode predict(Train *tra, int accelerate, struct timeval tnow, PredictionProblemCallback *ppc, void *ppcu) { PredictUserContext u; Segment *foredet; @@ -655,16 +641,10 @@ 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); - u.stopping= speedmanager_stopping(u.train); - u.stopping_distance= speedmanager_stoppingdistance(u.train); - - switch (accelerate) { - case 1: u.stopping= 0; break; - case 0: break; - case -1: if (u.stopping) u.stopping_distance -= tra->uncertainty; break; - default: abort(); - } + u.accelerating= accelerate > 0; + u.maxspeed= speedmanager_speed_maxestimate(u.train, tnow); + u.stopping= accelerate<=0 && speedmanager_stopping(u.train); + u.stopping_distance= speedmanager_stoppingdistance(u.train, tnow); oprintf(DUPO("safety") " predict ***starting*** %s%s maxspeed=%f" " (speed %f try %f, step %d%s) stopdist=%d accelerate=%d %s\n", @@ -690,63 +670,90 @@ ErrorCode predict(Train *tra, int accelerate, u.train_polarity_inverted= foredet->seg_inverted ^ foredet->tr_backwards; u.usecurrentposn= 1; - u.fdetc.getmovpos= pred_getmovpos; + /*----- find the train's foredetect (rearmost possibility) -----*/ + /* + * If tra->uncertainty > tra->maxinto then in theory the foredetect + * might be in the previous segment. We don't take that into account, + * which extends the effective length of the train. + */ + + int effective_length= MARGIN_TAIL + tail_length(tra) + tra->detectable; + int effective_into_fdet= tra->maxinto - tra->uncertainty; + if (effective_into_fdet < 0) { + effective_length += -effective_into_fdet; + effective_into_fdet += -effective_into_fdet; + assert(!effective_into_fdet); + } + u.fdetc.u= &u; + u.fdetc.getmovpos= pred_getmovpos; trackloc_set_exactinto(&u.fdet, &u.fdetc, foredet, foredet->tr_backwards, - tra->maxinto); + effective_into_fdet); - u.tail= u.fdet; + /*----- find the train's tail (rearmost possibility) -----*/ + /* + * We walk backwards, also marking the train present. + */ - u.tailc.getmovpos= pred_getmovpos; u.tailc.u= &u; - - ec= trackloc_reverse_exact(&u.tail, &u.tailc); assert(!ec); - - /* find the train's tail and mark it present */ - - u.tailc.distance= tra->detectable + (tra->backwards ? tra->head : tra->tail) - + tra->uncertainty + MARGIN_TAIL; + u.tailc.getmovpos= pred_getmovpos; u.tailc.nextseg= initpresent_nextseg; u.tailc.trackend= pred_trackend_panic; + u.tailc.distance= effective_length; - ec= trackloc_advance(&u.tail,&u.tailc); assert(!ec); - trackloc_reverse_exact(&u.tail,0); + u.tail= u.fdet; + + ec= trackloc_reverse_exact(&u.tail,&u.tailc); assert(!ec); + ec= trackloc_advance(&u.tail,&u.tailc); assert(!ec); + ec= trackloc_reverse_exact(&u.tail,&u.tailc); assert(!ec); u.hindmost= u.tail.seg; - /* find the train's nose */ + /*----- find the train's nose (rearmost possibility) -----*/ - u.nose= u.fdet; - u.nosec.distance= nose_length(tra); - u.nosec.nextseg= nose_nextseg; + u.nosec.u= &u; u.nosec.getmovpos= pred_getmovpos; + u.nosec.nextseg= nose_nextseg; u.nosec.trackend= pred_trackend; - u.nosec.u= &u; + u.nosec.distance= nose_length(tra); u.count_time= 0; + u.nose= u.fdet; + ec= trackloc_advance(&u.nose,&u.nosec); if (ec && ec != EC_SignallingHorizonReached) goto xproblem; - /* predict the future */ + /*----- predict the future - (including the present uncertainty) -----*/ - u.fdetc.distance= u.was_distance= - u.stopping ? u.stopping_distance : TL_DIST_INF; u.fdetc.nextseg= fdet_nextseg; u.fdetc.getmovpos= pred_getmovpos; u.fdetc.trackend= pred_trackend; - u.tailc.nextseg= tail_nextseg; + if (accelerate<0 && u.stopping) { + /* we actually know exactly where we are */ + u.fdetc.distance= u.stopping_distance; + } else { + u.fdetc.distance= tra->uncertainty + u.stopping_distance; + } + u.fdetc.distance += MARGIN_NOSE; + u.was_distance= u.fdetc.distance; + ec= trackloc_advance(&u.fdet,&u.fdetc); if (ec && ec != EC_SignallingHorizonReached) goto xproblem; - /* look ahead for autopoint */ + /*----- look ahead for autopoint -----*/ + + if (tra->autopoint) { + Distance min_autopoint_dist= MARGIN_AUTOPOINTTIME * u.maxspeed; - oprintf(DUPO("safety") " predict optimism %d nose %s\n", - u.autopoint_distance, u.nose.seg->i->pname); + oprintf(DUPO("safety") " predict optimism %d (min %d) nose %s\n", + u.autopoint_distance, min_autopoint_dist, u.nose.seg->i->pname); + + if (u.autopoint_distance < min_autopoint_dist) + u.autopoint_distance= min_autopoint_dist; - if (u.autopoint_distance) { u.furthest= u.nose.seg; u.optimistic= 1; u.was_distance= u.nosec.distance= u.autopoint_distance; @@ -929,6 +936,7 @@ void safety_notify_detection(Segment *seg) { ErrorCode ec; Segment *interferer; int stopdist, maxinto; + struct timeval tnow; if (seg->det_ignore) return; if (!seg->det_expected) { @@ -947,15 +955,17 @@ void safety_notify_detection(Segment *seg) { if (seg->movposcomb < 0) safety_panic(tra,seg, "track route not set and train has arrived"); - stopdist= speedmanager_stoppingdistance(tra); + mgettimeofday(&tnow); + + stopdist= speedmanager_stoppingdistance(tra,tnow); maxinto= seg->i->poscombs[seg->movposcomb].dist; if (speedmanager_stopping(tra) && maxinto > stopdist) maxinto= stopdist; tra->foredetect= seg; - tra->uncertainty= tra->maxinto= stopdist; + tra->uncertainty= tra->maxinto= maxinto; report_train_position(tra); - ec= predict(tra,-1, detection_report_problem, 0); + ec= predict(tra,-1,tnow, detection_report_problem, 0); if (!ec) return; assert(ec == EC_SignallingPredictedProblem); @@ -964,8 +974,8 @@ void safety_notify_detection(Segment *seg) { tra->maxinto= tra->uncertainty= maxinto; report_train_position(tra); - speedmanager_safety_stop(tra); - ec= predict(tra,-1, 0,(char*)"safety commanding stop"); + speedmanager_safety_stop(tra,tnow); + ec= predict(tra,-1,tnow, 0,(char*)"safety commanding stop"); assert(!ec); assert(!ec); diff --git a/hostside/safety.h b/hostside/safety.h index 689bd20..96dfec8 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -109,7 +109,7 @@ ErrorCode safety_check_movposchange(Segment *seg, PredictionProblemCallback *ppc, void *ppcu); /* If this check success, caller may call movpos_change */ -ErrorCode predict(Train *tra, int accelerate, +ErrorCode predict(Train *tra, int accelerate, struct timeval tnow, PredictionProblemCallback *ppc, void *ppcu); /* Lower-level interface for speedmanager etc. * Caller must call this with different situations until it succeeds! @@ -217,10 +217,10 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step, PredictionProblemCallback *ppc, void *ppcu); /* ppc and ppcu as for predict_confirm */ -void speedmanager_safety_stop(Train *tra); +void speedmanager_safety_stop(Train *tra, struct timeval tnow); void speedmanager_reset_train(Train *tra); -double speedmanager_speed_maxestimate(Train *tra); -double speedmanager_stoppingdistance(Train *tra); +double speedmanager_speed_maxestimate(Train *tra, struct timeval tnow); +double speedmanager_stoppingdistance(Train *tra, struct timeval tnow); int speedmanager_stopping(Train *tra); /*========== actual.c ==========*/ @@ -357,19 +357,10 @@ int trackloc_set_exactinto(TrackLocation *t, TrackAdvanceContext *c, /*---------- safety margin parameters ----------*/ -#define MARGIN_NOSE 6 -#define MARGIN_TAIL 6 -#define MARGIN_SPEED 1.2 - -#if 0 -#define CLEAR_FORESIGHT_TIME 500 /*ms*/ -#define AUTOSTOP_MAXSPEED 0.050 /*m/s*/ -#define AUTOSTOP_UNCERTAINTY 20 /*mm*/ -#define ESTOP_UNCERTAINTY 300 /*mm*/ -#define ESTOP_DEADTIME 2000 /*ms*/ - -#define TIMEINTERVAL2MS(ti) (ti) -#define SPEED_CLEAR_MULT SPEED_CALC_DIST(1,CLEAR_FORESIGHT_TIME,UP) -#endif +#define MARGIN_NOSE 6 /*mm*/ +#define MARGIN_TAIL 6 /*mm*/ +#define MARGIN_SPEED 1.2 /*ratio*/ +#define MARGIN_STOPTIME 50 /*ms*/ +#define MARGIN_AUTOPOINTTIME 500 /*ms*/ #endif /*SAFETY_H*/ diff --git a/hostside/speed.c b/hostside/speed.c index 534af0a..197ea95 100644 --- a/hostside/speed.c +++ b/hostside/speed.c @@ -65,18 +65,14 @@ static double current_speed(Train *tra, const struct timeval tnow) { return cs; } -double speedmanager_speed_maxestimate(Train *tra) { - struct timeval tnow; - mgettimeofday(&tnow); +double speedmanager_speed_maxestimate(Train *tra, struct timeval tnow) { return current_speed(tra,tnow) * MARGIN_SPEED; } -double speedmanager_stoppingdistance(Train *tra) { - struct timeval tnow; +double speedmanager_stoppingdistance(Train *tra, struct timeval tnow) { double v, xs; const SpeedRange *regime; - mgettimeofday(&tnow); v= current_speed(tra,tnow); if (v==0) return 0; @@ -88,7 +84,7 @@ double speedmanager_stoppingdistance(Train *tra) { double v_ts_vcrit= v * regime->ts; if (xs > v_ts_vcrit) xs= v_ts_vcrit; double xs_vcrit= regime->xs; if (xs > xs_vcrit) xs= xs_vcrit; - return xs; + return xs + v * MARGIN_STOPTIME; } int speedmanager_stopping(Train *tra) { @@ -98,10 +94,9 @@ int speedmanager_stopping(Train *tra) { return r; } -static ErrorCode request_core(Train *tra, int step, +static ErrorCode request_core(Train *tra, int step, struct timeval tnow, PredictionProblemCallback *ppc, void *ppcu) { ErrorCode ec, ec2; - struct timeval tnow; double vnow, vtarg; oprintf(DUPO("speed") " request %s%s step %d\n", @@ -111,7 +106,6 @@ static ErrorCode request_core(Train *tra, int step, if (step == tra->speed.step) return 0; - mgettimeofday(&tnow); vnow= current_speed(tra,tnow); vtarg= tra->speedcurve[step]; @@ -124,7 +118,7 @@ static ErrorCode request_core(Train *tra, int step, toev_start(&tra->speed.decel); tra->speed.speed= vnow; if (ppc || ppcu) { - ec= predict(tra,0, 0,(char*)"deceleration forbidden"); + ec= predict(tra,0,tnow, 0,(char*)"deceleration forbidden"); assert(!ec); } xmit(tra); @@ -132,11 +126,11 @@ static ErrorCode request_core(Train *tra, int step, } tra->speed.try_speed= vtarg; - ec= predict(tra,1, ppc,ppcu); + ec= predict(tra,1,tnow, ppc,ppcu); if (ec) { tra->speed.try_speed= -1; - ec2= predict(tra,0, 0,(char*)"abandoned acceleration"); + ec2= predict(tra,0,tnow, 0,(char*)"abandoned acceleration"); assert(!ec2); return ec; } @@ -151,13 +145,15 @@ static ErrorCode request_core(Train *tra, int step, ErrorCode speedmanager_speedchange_request(Train *tra, int step, PredictionProblemCallback *ppc, void *ppcu) { + struct timeval tnow; assert(ppc || ppcu); - return request_core(tra,step,ppc,ppcu); + mgettimeofday(&tnow); + return request_core(tra,step,tnow,ppc,ppcu); } -void speedmanager_safety_stop(Train *tra) { +void speedmanager_safety_stop(Train *tra, struct timeval tnow) { ErrorCode ec; - ec= request_core(tra,0,0,0); + ec= request_core(tra,0,tnow,0,0); assert(!ec); }