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 */
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;
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,
u->was_distance,
+ "-2"[ u->done_first_new_fdet ],
"-t"[ u->count_time ],
- "-a"[ u->accelerating ],
"-c"[ u->usecurrentposn ],
"-o"[ u->optimistic ],
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;
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) {
/* 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
/*========== 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;
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",
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;
ErrorCode ec;
Segment *interferer;
int stopdist, maxinto;
+ struct timeval tnow;
if (seg->det_ignore) return;
if (!seg->det_expected) {
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);
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);
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;
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) {
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",
if (step == tra->speed.step)
return 0;
- mgettimeofday(&tnow);
vnow= current_speed(tra,tnow);
vtarg= tra->speedcurve[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);
}
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;
}
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);
}
~ijackson / trains.git / commitdiff