3 * safety core algorithm
18 * New signalling/safety algorithm:
20 * When we detect a train at the relevant det_expected we firstly
21 * update our knowledge of its current position. We don't change
22 * any ownerships just yet, but we do set pred_present for the parts
23 * where the train is right now.
25 * We then check to see that when we reach the _next_ block boundary,
26 * we'll still be able to stop safely (unless we're already doing an
27 * emergency stop). If this fails we issue the emergency stop command
28 * now. This prediction is done as follows:
30 * We use three tracklocs which move roughly together - one for the
31 * foredetect and one for each end of our train. This represents a
32 * predicted train position. (We use `predicted' to refer to these
33 * future situations and `current' to refer to that prevailing at the
34 * time we are making the prediction.) The advancement is driven
35 * using the foredetect; when it enters a new segment we advance first
36 * the head and then the tail by the same distance.
38 * We advance foredetect by the total of the uncertainty (which takes
39 * it into the next segment) and the current stopping distance. At
40 * each advancement we record the earliest time from now at which this
41 * prediction will happen (computed from the current speed). (We
42 * don't take into account any already commanded but not necessarily
43 * happened deceleration; however if we are attempting to increase our
44 * speed, current speed refers to the new speed.) If we're already
45 * stopping then we advance by the stopping distance only (since we
46 * don't need to worry about not being told to stop until the next
47 * detection); also we stop advancing when we run out of segments we
48 * own (this deals with segment-sized rounding errors which might
49 * otherwise make us think we will overrun).
51 * At each new track location for our front end we hope to find a
52 * segment which no-one currently owns and which is currently set for
53 * us. (We check this before updating our tail trackloc as below.)
54 * In that case we update the segment's until.) (We use the value
55 * `elapsed' which is the minimum time until the current nose
58 * However it may be a segment which we currently own, or perhaps it
59 * may be set against us. (If it's owned by anyone else or is not
60 * owned but moving then we are definitely doomed and must stop.)
62 * If it's owned by us but we haven't claimed it yet in this
63 * prediction then that's fine we can just claim it.
65 * If we _have_ claimed it then it had better be predicted to be
66 * vacant by the time we get there (pred_present clear, see below).
68 * Regarding moveable features, we continuously develop our plan,
69 * as represented in route_plan and route_reservation.
71 * If the feature is currently moving and owned by us then that must
72 * be OK because we checked it before (except that if we're trying to
73 * increase our speed that may not be OK and we must replan the speed
74 * increase to check whether the movement will happen in time).
76 * If it is not currently moving and is set against us, then we plan
77 * to set it. If there is already a plan (ie, a reservation) and
78 * we're not accelerating then that's fine. Otherwise we make one: we
79 * determine the desired movposcomb make a reservation to move it in
80 * time. (If the segment is already owned by us that means a
81 * reservation to move it between when the tail leaves and the nose
84 * When predicted foredetect enters a new segment we add the new
85 * segment to the tally of segments of the particular kind and
86 * train-relative orientation (for polarity purposes), and check that
87 * a solution is possible. For the first such solution (ie, the one
88 * for the immediately-forthcoming detection) we actually implement
91 * And, when predicted foredetect enters a new segment we advance the
92 * predicted tail (as described above). We set pred_vacated and
93 * until, and remove the departing segments from tallies.
95 * If all of this works then pred_present and pred_vacated constitute
96 * the new ownership and motion_newplan constitutes the new plan. We
97 * commit to it by rewriting owner and det_* and actually requesting
98 * any movfeat changes which we can do now, and replacing motion with
101 * If not then we issue an signal stop command (we'd better not be
102 * doing that already!) or countermand the speed increase or whatever. */
104 * Here is how we use the stuff in the Segment:
106 * owner train which owns this for signalling purposes
107 * no other train may interfere
109 * physical in an unowned segment, this has no particular
110 * feature relationship to anything
112 * movposcomb in an owned segment, if moving, means that
113 * & we have a promise that the feature
114 * moving will be OK by the time we get there
115 * & at our current speed
116 * motion if not moving then motion is our reservation which
117 * allows us to know that we are going to
118 * be able to change this segment between
119 * the tail leaving it and the head arriving
120 * in each case movpos_change_intent tells us the plan
122 * motion_newplan while we are making a plan this is the new value
123 * for motion. If identical to motion it
124 * means we should keep it; if 0 we cancel
125 * or unreserve; if a different value we
126 * commit to the new plan instead
128 * during prediction, motion_newplan can be non-0 for
129 * unowned segments as we make our plan.
130 * If we succeed we will end up
131 * owning the segment, and if we fail
132 * we garbage collect it
134 * det_ignore this is part of the body of a train which we've
135 * already detected here
137 * det_expected this is the next place we expect owner train
139 * (it is not valid to have both det_ignore
142 * VALID DURING PREDICTION ETC. ONLY
144 * pred_present in our prediction, the train physically occupies this
145 * until_here tells us when will be
147 * pred_vacated in our prediction, the train does not occupy this
148 * although it did in previous predictions
149 * until_here tells us when it will have been vacated
150 * if set together with pred_present, means was vacated
151 * and then reentered; do not record vacation
154 * iff either of these is set, this train is going
155 * to own the segment when we're finished
157 * now_present train is in this segment now (in which we include
158 * the segment just before foredetect)
160 * until time until the we will arrive (pred_present), or
161 * depart (pred_vacated), valid only if
162 * one of those is set
163 * value is as for u->elapsed
165 * Valid combinations during prediction:
174 * has never been here as far as we know --- undef
175 * here now, prediction has it still here NP- arrival = 0
176 * here now, but predicted to have departed N-V departure
177 * pred. to have arrived here -P- arrival
178 * pred. to have arrived and departed --V departure
179 * here now, pred. to have dep'd and ret'd NPV re-arrival
180 * now absent, pred. to have arr, dep & ret'd -PV re-arrival
182 * seg->tr_backwards records the sense of the _first_ passage of the train.
184 * invalid/impossible combination:
185 * train here now but never in prediction N--
187 * Meanings of moving, motion and motion_newplan:
188 * For segments NPV==000, no plan made yet:
190 * moving motion _newplan
191 * 0 0 0 segment is entirely stable
192 * 0 non-0 0 existing plan has deferred motion
193 * 1 non-0 0 currently moving - as planned if owned
195 * 1 0 ? always forbidden
196 * ? ? non-0 forbidden for not-yet-planned segments
198 * For segments with NPV!=0, some kind of planning done:
200 * moving motion _newplan new plan involves
201 * 0 0 0 no motion, segment is entirely stable
202 * 0 non-0 0 abandoning planned deferred motion
203 * 0 0 non-0 new motion, perhaps deferred
204 * any non-0 =motion same motion as before
205 * any non-0 !=motion different motion
207 * 1 0 ? always forbidden
208 * 1 non-0 0 forbidden: need explicit motion to cancel
210 * (These are the values when not in the middle of nose_nextseg.)
216 * real stop dist >>>>>
217 * simul. stop dist >>>>>
218 * +++++++++++|+++++++++++|++++++++++|XXXXXX
220 * adv. tail back T FN
221 * adv. nose find T F N
222 * +++++++++++|+++++++++++|++++++++++|XXXXXX
223 * adv. fdet 1 seg T N F
226 * adv. fdet stopdist T N F
229 * +++++++++++|+++++++++++|++++++++++|XXXXXX
233 * real stop dist >>>>>
234 * +++++++++++|+++++++++++|++++++++++|XXXXXX
236 * adv. tail back T FN
237 * adv. nose find T F N
238 * +++++++++++|+++++++++++|++++++++++|XXXXXX
239 * adv. fdet 1 seg T N F
240 * adv. nose T N ProblemPredicted
244 * real stop dist >>>>>
245 * +++++++++++|+++++++++++|++++++++++|XXXXXX
247 * adv. tail back T FN
248 * adv. nose find T F N
249 * +++++++++++|+++++++++++|++++++++++|XXXXXX
250 * adv. fdet stopdist T N F } may give
251 * adv. nose T F N } HorizonReached
252 * adv. tail T F N instead
256 /*========== prediction machinery ==========*/
258 #define now_present mark0
259 #define pred_present mark1
260 #define pred_vacated mark2
261 #define will_polarise mark3
266 speed_info_specified:1,
269 done_first_new_fdet:1,
270 usecurrentposn:1, /* for {pred,report}_getmovpos */
271 optimistic:1, /* for autopoint */
272 walk_compute_polarise:1, /* nose_nextseg still needs to worry */
273 need_polarise:1, /* when we commit */
274 train_polarity_inverted:1, /* right now, or if know_best, the best */
275 know_best_polarity:1, /* longest-lasting into the future */
276 done_fdet:1; /* for report_nextseg */
277 TrackLocation nose, fdet, tail;
278 TrackAdvanceContext nosec, tailc, fdetc;
279 TimeInterval elapsed; /* from now, minimum */
280 Distance was_distance, autopoint_distance, stopping_distance;
282 Segment *hindmost, *furthest, *desire_move;
283 int desire_movposcomb;
284 int lookahead, reportcount;
286 PredictionProblemCallback *problem_callback;
287 void *problem_callback_u;
289 /* tally counts segments which have been entered in fdet_nextseg but
290 * not left in tail_nextseg (ie not the same ones as pred_present),
291 * in each direction (according to tr_backwards) */
293 } PredictUserContext;
295 static int tail_length(Train *tra) {
296 return tra->backwards ? tra->head : tra->tail;
298 static int nose_length(Train *tra) {
299 return tra->backwards ? tra->tail : tra->head;
301 static void advance_elapsed(PredictUserContext *u, int advance_distance) {
302 u->elapsed += advance_distance / u->maxspeed;
304 static int calc_advanced(TrackAdvanceContext *c) {
305 PredictUserContext *u= c->u;
306 return u->was_distance - c->distance;
308 static void qprintf_position(Train *tra,
309 void (*qp)(const char *fmt,...) __attribute__((format(printf,1,2)))) {
311 tra->foredetect->tr_backwards?"-":"",
312 tra->foredetect->i->pname, tra->maxinto, tra->uncertainty);
315 #define SKELETON_PREDICT_USER_CONTEXT(u,tra) \
316 (memset(&(u),0,sizeof((u))), \
317 (u).problem_callback= ppc, \
318 (u).problem_callback_u= ppcu, \
320 /* for callers of predict_problem rather than predict */
322 /*---------- prediction problem reporting ----------*/
324 static ErrorCode predict_vproblem(PredictUserContext *u, Segment *seg,
325 const char *fmt, va_list al) {
329 l= vasprintf(&message, fmt, al); if (l <= 0) diem();
332 DPRINTF(safety,predict," optimistic problem %s\n", message);
333 else if (!u->problem_callback)
334 safety_panic(u->train, seg, "unexpected problem predicted"
335 " (context: %s): %s", (char*)u->problem_callback_u, message);
337 u->problem_callback(u->train, seg, u->problem_callback_u, message);
340 return EC_SignallingPredictedProblem;
343 static ErrorCode predict_problem(PredictUserContext *u, Segment *seg,
344 const char *fmt, ...) {
348 ec= predict_vproblem(u,seg,fmt,al);
353 static const char *motion_pname(Segment *seg, MovPosChange *motion) {
354 if (!motion) return "";
355 return movpos_pname(seg, movpos_change_intent(motion));
358 static void pred_callback_debug(const char *what, TrackLocation *t,
359 struct TrackAdvanceContext *c,
360 const TrackLocation *before) {
361 PredictUserContext *u= c->u;
363 DPRINTF(safety,predictseg," %s"
364 " %c%s dist=%-4d until=%-4ld %c%c%c.%c%c (was %s%s..%d dist=%-4d)"
365 " %c%c%c.%c%c%c%c elapsed=%ld la#%d"
375 "-N"[ t->seg->now_present ],
376 "-P"[ t->seg->pred_present ],
377 "-V"[ t->seg->pred_vacated ],
379 "-p"[ t->seg->will_polarise ],
380 "-i"[ t->seg->seg_inverted ],
382 before && before->backwards?"-":"",
383 before ? before->seg->i->pname : "-",
384 before ? before->remain : -1,
388 "-2"[ u->done_first_new_fdet ],
389 "-c"[ u->usecurrentposn ],
390 "-o"[ u->optimistic ],
392 "-w"[ u->walk_compute_polarise ],
393 "-n"[ u->need_polarise ],
394 "-i"[ u->train_polarity_inverted ],
395 "-b"[ u->know_best_polarity ],
400 movpos_pname(t->seg, t->seg->movposcomb),
401 "~!"[ t->seg->moving ],
402 motion_pname(t->seg, t->seg->motion),
403 motion_pname(t->seg, t->seg->motion_newplan),
405 u->noninv_tally[0], u->noninv_tally[1]);
408 /*---------- prediction trivial callbacks ----------*/
410 static int pred_getmovpos(TrackLocation *t, TrackAdvanceContext *c,
411 MovPosComb *use_io) {
412 PredictUserContext *u= c->u;
413 if (!u->usecurrentposn &&
414 !t->seg->now_present &&
415 t->seg->motion_newplan)
416 *use_io= movpos_change_intent(t->seg->motion_newplan);
418 return predict_problem(u, t->seg, "track route not known");
422 static int pred_trackend(TrackLocation *t, TrackAdvanceContext *c) {
423 PredictUserContext *u= c->u;
424 return predict_problem(u, t->seg, "end of track");
427 static int pred_trackend_panic(TrackLocation *t, TrackAdvanceContext *c) {
428 PredictUserContext *u= c->u;
429 safety_panic(u->train,t->seg,"unexpected end of track");
432 static int initpresent_nextseg(TrackLocation *t, TrackAdvanceContext *c,
434 const TrackLocation *before) {
435 PredictUserContext *u= c->u;
436 pred_callback_debug(" initpresent_nextseg",t,c,before);
437 t->seg->now_present= t->seg->pred_present= t->seg->will_polarise= 1;
438 t->seg->tr_backwards= !t->backwards;
440 if (!t->seg->i->invertible)
441 u->noninv_tally[!t->backwards]++; /* ! since going backwards along train */
445 /*---------- prediction nose advancement ----------*/
447 static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
448 MovPosComb *mpc_io, const TrackLocation *before) {
449 PredictUserContext *u= c->u;
450 MovPosComb route_plan;
454 pred_callback_debug(" nose_nextseg",t,c,before);
455 if (!before) return 0;
458 advance_elapsed(u,calc_advanced(c));
462 if (u->stopping && u->lookahead == u->train->plan_lookahead_nsegs)
463 return EC_SignallingHorizonReached;
466 if (t->seg->owner != u->train)
467 return predict_problem(u, t->seg, "impeded by %s",
468 t->seg->owner->pname);
470 if (t->seg->pred_present)
471 return predict_problem(u, t->seg, "will collide with itself!");
473 if (!u->optimistic) {
474 Segment *interferer= segment_interferer(t->seg);
475 /* We want to avoid calling segment_interferer_does unless we have
476 * to, as it uses getmovpos which, if the movposcomb is unknown,
477 * will predict_problem even if the interferer is empty. */
479 (interferer->owner || interferer->pred_present)) {
481 ec= segment_interferer_does(c,t->seg,interferer, &does);
485 if (interferer->owner && interferer->owner != u->train)
486 return predict_problem(u, t->seg, "impeded by %s at %s",
487 interferer->owner->pname, interferer->i->pname);
488 if (interferer->pred_present)
489 return predict_problem(u, t->seg, "will collide with itself at %s!",
490 interferer->i->pname);
495 /* What about the route ? */
497 if (t->seg->i->n_poscombs==1)
500 if (t->seg->moving) {
501 assert(!t->seg->now_present);
503 return predict_problem(u, t->seg, "route not yet set");
504 if (!u->neednewplan) {
505 t->seg->motion_newplan= t->seg->motion;
506 *mpc_io= movpos_change_intent(t->seg->motion);
511 int autopoint= u->train->autopoint && t->seg->autopoint;
513 if (t->seg->motion_newplan) {
514 route_plan= movpos_change_intent(t->seg->motion);
517 } else if (t->seg == u->desire_move && !t->seg->now_present) {
518 ec= movpos_findcomb_bysegs(before->seg,t->seg,0,
519 u->desire_movposcomb, &route_plan);
521 if (route_plan != u->desire_movposcomb)
522 return predict_problem(u, t->seg, " proposed route would be against"
523 " approaching train");
525 } else if (t->seg->motion) {
526 /* We already have a plan. */
527 route_plan= movpos_change_intent(t->seg->motion);
528 if (!u->neednewplan) {
529 t->seg->motion_newplan= t->seg->motion;
534 /* Extend the plan. */
535 ec= movpos_findcomb_bysegs(before->seg,t->seg,0,*mpc_io, &route_plan);
536 assert(!ec); /* there must be _some_ route since
537 we're moving into t->seg */
539 if (route_plan == *mpc_io && !t->seg->moving) {
540 /* Well, that's all fine then. */
543 /* We can make it work but have to plan some motion: */
545 return predict_problem(u, t->seg, "train arriving but route not yet set");
546 if (t->seg->pred_vacated) {
547 if (!t->seg->now_present)
548 /* And we're not even there yet! This is too hard because we
549 * are too stupid not to change it straight away. */
550 return predict_problem(u, t->seg, "dynamic route too complicated"
551 " - would need multiple queued changes");
552 max_ms= u->elapsed - t->seg->until;
558 return predict_problem(u,t->seg,"will not automatically set route");
560 ec= movpos_reserve(t->seg, max_ms, &t->seg->motion_newplan,
561 route_plan, *mpc_io);
562 if (ec) return predict_problem(u, t->seg, "insufficient time to"
563 " set route: %s", ec2str(ec));
567 /* Now we definitely have a plan which sets a good route at t->seg. */
568 if (!t->seg->pred_vacated)
569 t->seg->tr_backwards= t->backwards;
570 t->seg->pred_present= 1;
571 t->seg->until= u->elapsed < 0 ? 0 : u->elapsed;
576 /*---------- prediction tail advancement ----------*/
578 static int tail_nextseg(TrackLocation *t, TrackAdvanceContext *c,
579 MovPosComb *mpc_io, const TrackLocation *before) {
580 PredictUserContext *u= c->u;
582 pred_callback_debug(" tail_nextseg",t,c,before);
583 if (!before) return 0;
585 if (!before->seg->i->invertible)
586 u->noninv_tally[before->backwards]--;
588 if (before->seg->pred_vacated) return 0; /* only vacate once */
589 before->seg->pred_present= 0;
590 before->seg->pred_vacated= 1;
591 before->seg->until= u->elapsed < 0 ? 0 : u->elapsed;
596 /*---------- prediction foredetect advancement ----------*/
598 static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
599 MovPosComb *mpc_io, const TrackLocation *before) {
600 PredictUserContext *u= c->u;
603 /* NB, on entry the movposcomb of the segment we're entering (and
604 * other similar details) may not be known or sufficient because
605 * when fdet has only just entered a new segment, nose is still in
606 * the previous segment.
609 pred_callback_debug("fdet_nextseg",t,c,before);
610 if (!before) return 0;
612 advanced= calc_advanced(c);
613 advance_elapsed(u,advanced);
615 u->nosec.distance= advanced;
616 r= trackloc_advance(&u->nose,&u->nosec);
618 DPRINTF(safety,predictseg," fdet_nextseg r=%s\n",ec2str(r));
622 /* Now we have advanced the nose and have recorded any appropriate
623 * motion(s). Advancing the nose has updated the segment's
624 * notion of the motion but not trackloc_advance's cached version of
625 * the movposcomb in *mpc_io - but trackloc_advance will call
626 * pred_getmovpos right after we return so we do not need to worry.
631 if (!t->seg->i->invertible) {
632 u->noninv_tally[t->backwards]++;
633 if (u->noninv_tally[0] && u->noninv_tally[1])
634 return predict_problem(u, t->seg, "cannot set track polarity");
637 if (!u->done_first_new_fdet) {
638 t->seg->det_expected= 1;
639 u->done_first_new_fdet= 1;
642 if (u->elapsed < MARGIN_POLARISETIME) {
643 u->need_polarise |= (t->seg->seg_inverted ^ t->backwards ^
644 u->train_polarity_inverted);
646 u->walk_compute_polarise &= u->need_polarise;
649 if (u->train->autopoint && !u->autopoint_distance) {
650 const SegPosCombInfo *pci;
651 r= trackloc_getlink(t,c,&pci,0,-1); assert(!r);
652 u->autopoint_distance= pci->dist;
655 if (u->walk_compute_polarise) {
656 if (t->seg->will_polarise)
657 /* this is our 2nd visit, stop now */
658 u->walk_compute_polarise= 0;
660 if (u->walk_compute_polarise) {
661 if (!u->know_best_polarity) {
662 if (u->noninv_tally[0]) {
663 u->train_polarity_inverted= 0;
664 u->know_best_polarity= 1;
665 } else if (u->noninv_tally[1]) {
666 u->train_polarity_inverted= 1;
667 u->know_best_polarity= 1;
670 if (u->know_best_polarity &&
671 !t->seg->i->invertible &&
672 u->train_polarity_inverted != t->backwards) {
673 /* incompatible, stop now */
674 u->walk_compute_polarise= 0;
677 if (u->walk_compute_polarise) {
678 t->seg->will_polarise= 1;
681 /* Advance the tail */
683 u->tailc.distance += advanced;
684 if (u->tailc.distance > 0) {
685 r= trackloc_advance(&u->tail,&u->tailc);
687 assert(!u->tailc.distance);
690 /* Final adjustments, prepare for next iteration */
692 u->was_distance= c->distance;
696 /*---------- backtracking the optimistic nose ----------*/
698 static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c,
699 MovPosComb *mpc_io, const TrackLocation *before) {
700 PredictUserContext *u= c->u;
702 pred_callback_debug("optunwind_nextseg",t,c,before);
704 if (t->seg == u->furthest ||
705 t->seg->motion_newplan)
706 return EC_SignallingHorizonReached;
708 t->seg->pred_present= 0;
714 /*========== prediction entrypoint ==========*/
716 ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
717 Segment *desire_move, int desire_movposcomb,
718 const SpeedInfo *speed_info,
719 PredictionProblemCallback *ppc, void *ppcu) {
720 PredictUserContext u;
721 SpeedInfo speed_info_buf;
727 memset(&u,0,sizeof(u));
729 u.problem_callback= ppc;
730 u.problem_callback_u= ppcu;
731 u.neednewplan= !(flags & PREDF_OLDPLAN);
732 u.speed_info_specified= !!speed_info;
735 speedmanager_getinfo(tra,tnow,&speed_info_buf);
736 speed_info= &speed_info_buf;
738 u.stopping= (flags & PREDF_OLDPLAN) && speed_info->stopping;
739 u.maxspeed= speed_info->max_speed_estimate;
740 u.stopping_distance= speed_info->stopping_distance;
742 u.desire_move= desire_move;
743 u.desire_movposcomb= desire_movposcomb;
745 Distance detectable= ceil(tra->detectable * MARGIN_TRAINLENGTH);
747 if (DEBUGP(safety,core)) {
748 DPRINTF(safety,core," ***predicting*** %s%s ",
749 tra->backwards?"-":"",tra->pname);
750 qprintf_position(tra,DPRINTFA);
751 DPRINTFA(" det.able=%d %smaxspeed=%f%s stopdist=%d (speed %f, step %d%s)"
752 " flags=%c%c%s desire=%s/%s\n",
754 u.speed_info_specified ? "specified " : "",
755 u.maxspeed, u.stopping ? "(stopping)" : "",
757 tra->speed.speed, tra->speed.step,
758 tra->speed.decel.running ? " decel" : "",
759 "-j"[ !!(flags & PREDF_JUSTDET) ],
760 "no"[ !!(flags & PREDF_OLDPLAN) ],
761 u.stopping ? " stopping" : "",
762 u.desire_move ? u.desire_move->i->pname : "",
763 u.desire_move ? u.desire_move->i->poscombs[u.desire_movposcomb]
768 if (!seg->owner || seg->owner == u.train)
769 seg->det_expected= 0;
770 seg->now_present= seg->pred_present=
771 seg->pred_vacated= seg->will_polarise= 0;
774 foredet= tra->foredetect;
777 u.walk_compute_polarise= 1;
779 u.train_polarity_inverted= foredet->seg_inverted ^ foredet->tr_backwards;
781 /*----- find the train's foredetect (rearmost possibility) -----*/
783 * If tra->uncertainty > tra->maxinto then in theory the foredetect
784 * might be in the previous segment. We don't take that into account,
785 * so we must instead extend our idea of the length of the train.
788 int effective_uncertainty= tra->uncertainty;
789 int effective_length= MARGIN_NOSE + tail_length(tra) + detectable;
791 if (effective_uncertainty > tra->maxinto) {
792 effective_length += tra->uncertainty - tra->maxinto;
793 effective_uncertainty= tra->maxinto;
796 int effective_into_fdet= tra->maxinto - effective_uncertainty;
797 int tail_advance_delaydist= MARGIN_TAIL;
799 DPRINTF(safety,core," predict eff.uncert=%d eff.len=%d"
800 " eff.into_fdet=%d tail.adv.delay=%d\n",
801 effective_uncertainty, effective_length,
802 effective_into_fdet, tail_advance_delaydist);
805 u.fdetc.getmovpos= pred_getmovpos;
807 trackloc_set_exactinto(&u.fdet, &u.fdetc, foredet, foredet->tr_backwards,
808 effective_into_fdet);
810 /*----- find the train's tail (rearmost possibility) -----*/
812 * We walk backwards, also marking the train present.
816 u.tailc.getmovpos= pred_getmovpos;
817 u.tailc.nextseg= initpresent_nextseg;
818 u.tailc.trackend= pred_trackend_panic;
819 u.tailc.distance= effective_length;
823 ec= trackloc_reverse_exact(&u.tail,&u.tailc); assert(!ec);
824 ec= trackloc_advance(&u.tail,&u.tailc); assert(!ec);
825 ec= trackloc_reverse_exact(&u.tail,&u.tailc); assert(!ec);
827 u.hindmost= u.tail.seg;
829 /*----- find the train's nose (rearmost possibility) -----*/
833 u.nosec.getmovpos= pred_getmovpos;
834 u.nosec.nextseg= nose_nextseg;
835 u.nosec.trackend= pred_trackend;
836 u.nosec.distance= nose_length(tra);
840 ec= trackloc_advance(&u.nose,&u.nosec);
841 if (ec && ec != EC_SignallingHorizonReached) goto xproblem;
843 /*----- predict the future - (including the present uncertainty) -----*/
845 u.fdetc.nextseg= fdet_nextseg;
846 u.fdetc.getmovpos= pred_getmovpos;
847 u.fdetc.trackend= pred_trackend;
848 u.tailc.nextseg= tail_nextseg;
849 u.tailc.distance= -tail_advance_delaydist;
852 /* we're carrying on until the next segment */
853 u.fdetc.distance= u.fdet.remain;
854 } else if (flags & PREDF_JUSTDET) {
855 /* we actually know exactly where we are */
857 } else { /* stopping, but we're mid-segment */
858 u.fdetc.distance= effective_uncertainty;
860 u.elapsed= -u.fdetc.distance / u.maxspeed; /* imagine we're already there */
861 u.fdetc.distance += u.stopping_distance + MARGIN_NOSE;
862 u.was_distance= u.fdetc.distance;
864 ec= trackloc_advance(&u.fdet,&u.fdetc);
865 if (ec && ec != EC_SignallingHorizonReached) goto xproblem;
867 /*----- look ahead for autopoint -----*/
869 if (tra->autopoint) {
870 Distance min_autopoint_dist= MARGIN_AUTOPOINTTIME * u.maxspeed;
872 DPRINTF(safety,predict," optimism %d (min %d) nose %s\n",
873 u.autopoint_distance, min_autopoint_dist, u.nose.seg->i->pname);
875 if (u.autopoint_distance < min_autopoint_dist)
876 u.autopoint_distance= min_autopoint_dist;
878 u.furthest= u.nose.seg;
880 u.was_distance= u.nosec.distance= u.autopoint_distance;
881 trackloc_advance(&u.nose,&u.nosec);
883 /* But actually we don't want to end up owning
884 * segments unless we have to for motions. */
886 u.nose.backwards ^= 1;
888 u.nosec.nextseg= optunwind_nextseg;
889 u.nosec.distance= TL_DIST_INF;
891 ec= trackloc_advance(&u.nose,&u.nosec);
892 assert(ec == EC_SignallingHorizonReached);
895 /*----- commit to the plan -----*/
897 DPRINTF(safety,predict," committing la#%d %c%c%c\n",
899 "-n"[ u.need_polarise ],
900 "-i"[ u.train_polarity_inverted ],
901 "-b"[ u.know_best_polarity ]);
903 tra->plan_lookahead_nsegs= u.lookahead;
905 if (u.need_polarise) {
906 DPRINTF(safety,predict," polarising train_polarity=%d\n",
907 u.train_polarity_inverted),
908 actual_inversions_start();
911 DPRINTF1(safety,predictplan," ");
913 DPRINTF2(" %s%s%s%c%c%c%c/%s%s%s%s%s",
914 seg->tr_backwards?"-":"", seg->i->pname,
915 seg->owner == tra ? "=" : seg->owner ? "#" : "-",
916 "-N"[ seg->now_present ],
917 "-P"[ seg->pred_present ],
918 "-V"[ seg->pred_vacated ],
919 "-p"[ seg->will_polarise ],
920 movpos_pname(seg,seg->movposcomb),
921 seg->motion ? (seg->moving ? "!" : "~") : "",
922 seg->motion ? motion_pname(seg,seg->motion) : "",
923 seg->motion_newplan ? ">" : "",
924 seg->motion && seg->motion_newplan==seg->motion ? "=" :
925 motion_pname(seg,seg->motion_newplan));
927 /* set ownership and det_ignore */
928 if (seg->pred_present || seg->pred_vacated) {
930 seg->det_ignore= seg->now_present;
931 } else if (seg->owner == tra) {
934 /* garbage collect our old plan for now-irrelevant segments */
935 assert(!seg->motion_newplan);
937 movpos_unreserve(seg->motion);
942 /* segment not relevant to us at all */
946 /* now it's our segment: */
948 /* clear any irrelevant old plan */
949 if (seg->motion && !seg->moving) {
950 if (seg->motion != seg->motion_newplan)
951 movpos_unreserve(seg->motion);
954 /* now motion!=0 only if seg->moving */
956 /* install the new plan, if any */
957 if (seg->motion_newplan) {
958 if (seg->motion_newplan == seg->motion) {
959 /* excellent, already doing it */
960 } else if (!seg->now_present) {
961 MovPosComb target= movpos_change_intent(seg->motion_newplan);
964 ec= movpos_change(seg, target, -1, seg->motion_newplan);
966 DPRINTF1(safety,predictplan, " ... ");
967 /* motion is updated by movpos_change */
969 /* we'll do it later, then */
970 seg->motion= seg->motion_newplan;
972 seg->motion_newplan= 0;
974 /* now the new plan is in motion and moving */
976 if (u.need_polarise && seg->will_polarise) {
977 seg->seg_inverted= seg->tr_backwards ^ u.train_polarity_inverted;
978 if (seg->i->invertible) {
979 actual_inversions_segment(seg);
980 Segment *interferer= segment_interferes_simple(0,seg);
981 if (interferer && !interferer->will_polarise &&
982 interferer->i->invertible) {
983 interferer->seg_inverted= seg->seg_inverted
984 ^ seg->i->interferes_polarity_opposed;
985 actual_inversions_segment(interferer);
988 assert(!seg->seg_inverted);
991 seg->now_present= seg->pred_present=
992 seg->pred_vacated= seg->will_polarise= 0;
997 actual_inversions_done();
999 report_train_ownerships(tra,u.hindmost,0);
1001 if (u.desire_move && !u.desire_move->owner)
1002 /* Oh! Train must be slower and less far advanced than expected.
1003 * Well, then we can move it right away. */
1004 return movpos_change(u.desire_move,u.desire_movposcomb,-1,0);
1009 DPRINTF(safety,predict," returning %s\n", ec2str(ec));
1012 seg->now_present= seg->pred_present=
1013 seg->pred_vacated= seg->will_polarise= 0;
1015 if (seg->motion_newplan==seg->motion) {
1016 seg->motion_newplan= 0;
1018 if (seg->motion_newplan) {
1019 movpos_unreserve(seg->motion_newplan);
1020 seg->motion_newplan= 0;
1022 if (!seg->owner && !seg->moving && seg->motion) {
1023 movpos_unreserve(seg->motion);
1030 /*========== reporting position and ownership ==========*/
1032 void report_train_position(Train *tra) {
1033 ouprintf("train %s at ",tra->pname);
1034 qprintf_position(tra,ouprintf);
1035 ouprintf(" %s\n", tra->backwards ? "backwards" : "forwards");
1038 static int report_getmovpos(TrackLocation *t, TrackAdvanceContext *c,
1039 MovPosComb *use_io) {
1040 PredictUserContext *u= c->u;
1041 if (u->done_fdet && t->seg != u->train->foredetect)
1042 /* we must use current posn for foredetect itself */
1043 u->usecurrentposn= 0;
1044 if (!u->usecurrentposn && t->seg->motion)
1045 *use_io= movpos_change_intent(t->seg->motion);
1050 static int report_nextseg(TrackLocation *t, struct TrackAdvanceContext *c,
1051 MovPosComb *mpc, const TrackLocation *before) {
1052 PredictUserContext *u= c->u;
1057 if (u->reportcount > NUM_SEGMENTS * 2) {
1058 ouprintf(" [... infinite loop!]\n");
1062 if (u->done_fdet) /* we've had foredetect */
1063 if (++u->lookahead > u->train->plan_lookahead_nsegs)
1068 if (t->seg->det_expected)
1070 if (t->seg->det_ignore)
1072 if (t->seg == u->train->foredetect && !u->done_fdet) {
1077 ouprintf(" %s%s", t->backwards?"-":"", t->seg->i->pname);
1079 if (t->seg->i->n_poscombs > 1) {
1080 r= report_getmovpos(t,c,mpc); assert(!r); assert(*mpc>=0);
1081 ouprintf("/%s", t->seg->i->poscombs[*mpc].pname);
1083 ouprintf("%s",flags);
1088 void report_train_ownerships(Train *tra, Segment *hindmost,
1089 int always_use_motions) {
1090 PredictUserContext u;
1092 memset(&u,0,sizeof(u));
1095 u.usecurrentposn= !always_use_motions;
1098 /* Walk along the train printing its segments: */
1099 ouprintf("train %s has", tra->pname);
1101 u.nose.seg= hindmost;
1103 u.nose.backwards= hindmost->tr_backwards;
1105 u.nosec.distance= TL_DIST_INF;;
1106 u.nosec.nextseg= report_nextseg;
1107 u.nosec.getmovpos= report_getmovpos;
1109 u.hindmost= hindmost;
1111 trackloc_advance(&u.nose,&u.nosec);
1116 /*========== reversing a train ==========*/
1118 static int reverse_nextseg(TrackLocation *t, struct TrackAdvanceContext *c,
1119 MovPosComb *mpc_io, const TrackLocation *before) {
1120 PredictUserContext *u= c->u;
1122 DPRINTF(safety,predictseg," reverse_nextseg "
1123 " %c%s dist=%-4d\n",
1129 if (*mpc_io==-1 || t->seg->motion)
1130 return predict_problem(u,t->seg, "segment under train is not stable");
1134 static int reverse_trackend(TrackLocation *t, struct TrackAdvanceContext *c) {
1138 ErrorCode safety_setdirection(Train *tra, int backwards,
1139 PredictionProblemCallback *ppc, void *ppcu) {
1141 * tra->foredetect, maxinto, uncertainty
1143 * tra->plan_lookahead_nsegs
1145 * The following remain unchanged as all their users have
1146 * regard to the train's direction:
1149 TrackLocation newfdet;
1150 TrackAdvanceContext c;
1151 PredictUserContext u;
1152 const SegPosCombInfo *pci;
1154 int oldmaxinto, oldfdet_trbackwards, old_planlookaheadnsegs;
1157 struct timeval tnow;
1159 SKELETON_PREDICT_USER_CONTEXT(u,tra);
1161 MUSTECR( safety_checktrain(tra,ppc,ppcu) );
1163 if (tra->backwards == backwards)
1166 if (!speedmanager_stopped(tra))
1167 return predict_problem(&u,0, "train is not stopped");
1169 newfdet.seg= tra->foredetect;
1170 newfdet.remain= tra->maxinto;
1171 newfdet.backwards= !tra->foredetect->tr_backwards;
1173 c.distance= ceil(tra->detectable * MARGIN_TRAINLENGTH) + tra->uncertainty;
1174 c.nextseg= reverse_nextseg;
1176 c.trackend= reverse_trackend;
1179 MUSTECR( trackloc_advance(&newfdet,&c) );
1181 mgettimeofday(&tnow);
1183 /* right, now let's start fiddling */
1184 oldfdet= tra->foredetect;
1185 oldmaxinto= tra->maxinto;
1186 oldfdet_trbackwards= tra->foredetect->tr_backwards;
1187 old_planlookaheadnsegs= tra->plan_lookahead_nsegs;
1189 r= trackloc_getlink(&newfdet,&c,&pci,0,-1); assert(!r);
1191 tra->foredetect= newfdet.seg;
1192 tra->maxinto= pci->dist - newfdet.remain;
1193 tra->backwards ^= 1;
1194 newfdet.seg->tr_backwards ^= 1;
1195 tra->plan_lookahead_nsegs= INT_MAX;
1197 ec= predict(tra,tnow, PREDF_OLDPLAN, 0,0, 0,ppc,ppcu);
1200 report_train_position(tra);
1204 /* It can happen eg that the uncertainty is more troublesome when
1205 * considered one way than the other. In which case just put it back now. */
1206 assert(ec == EC_SignallingPredictedProblem);
1208 tra->foredetect= oldfdet;
1209 tra->maxinto= oldmaxinto;
1210 tra->backwards ^= 1;
1211 tra->foredetect->tr_backwards= oldfdet_trbackwards;
1212 tra->plan_lookahead_nsegs= old_planlookaheadnsegs;
1214 ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,0,(char*)"abandon reverse");
1220 /*========== entrypoints from rest of the program ==========*/
1222 static void detection_report_problem(Train *tra, Segment *seg,
1223 void *pu, const char *message) {
1224 ouprintf("train %s signalling-problem %s : %s\n",
1225 tra->pname, seg ? seg->i->pname : "-", message);
1228 void safety_notify_detection(Segment *seg) {
1232 struct timeval tnow;
1233 SpeedInfo speed_info;
1235 if (seg->det_ignore) return;
1237 debug_count_event("detection");
1239 if (!seg->det_expected) {
1240 Segment *interferer= segment_interferes_simple(0,seg);
1241 if (!interferer) safety_panic(0,seg, "unexpected detection");
1242 if (interferer->det_ignore) return;
1243 if (!interferer->det_expected)
1244 safety_panic(0,seg, "unexpected detection, perhaps really at %s",
1245 interferer->i->pname);
1246 DPRINTF(safety,core, " detection %s using interferer %s",
1247 seg->i->pname, interferer->i->pname);
1252 if (seg->movposcomb < 0)
1253 safety_panic(tra,seg, "track route not set and train has arrived");
1255 mgettimeofday(&tnow);
1257 speedmanager_getinfo(tra,tnow,&speed_info);
1258 maxinto= seg->i->poscombs[seg->movposcomb].dist;
1259 if (speed_info.stopping && maxinto > speed_info.stopping_distance)
1260 maxinto= speed_info.stopping_distance;
1262 tra->foredetect= seg;
1263 tra->uncertainty= tra->maxinto= maxinto;
1264 tra->plan_lookahead_nsegs--;
1265 report_train_position(tra);
1267 ec= predict(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN,0,0,
1268 &speed_info, detection_report_problem,0);
1271 assert(ec == EC_SignallingPredictedProblem);
1273 if (maxinto > speed_info.stopping_distance)
1274 maxinto= speed_info.stopping_distance;
1275 tra->maxinto= tra->uncertainty= maxinto;
1277 report_train_position(tra);
1278 speedmanager_safety_stop(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN);
1281 ErrorCode safety_movposchange(Segment *seg, MovPosComb comb,
1282 int allow_queueing, PredictionProblemCallback *ppc, void *ppcu) {
1283 PredictUserContext u;
1284 struct timeval tnow;
1287 SKELETON_PREDICT_USER_CONTEXT(u,seg->owner);
1290 return movpos_change(seg,comb,-1,0);
1292 if (allow_queueing<2) {
1293 if (seg->det_ignore)
1294 return predict_problem(&u,seg, "route set for train");
1295 if (seg->det_expected)
1296 return predict_problem(&u,seg, "route set for immiment train");
1298 if (allow_queueing<1)
1299 return predict_problem(&u,seg, "route set for approaching train");
1301 mgettimeofday(&tnow);
1302 ec= predict(seg->owner,tnow, 0, seg,comb, 0, ppc,ppcu);
1305 ec2= predict(seg->owner,tnow, PREDF_OLDPLAN, 0,0, 0,
1306 0,(char*)"abandon movposchange");
1312 ErrorCode safety_checktrain(Train *tra,
1313 PredictionProblemCallback *ppc, void *ppcu) {
1314 if (!tra->foredetect) {
1315 ppc(tra,0,ppcu,"train is not on layout");
1316 return EC_SignallingPredictedProblem;
1321 void safety_abandon_run(void) {
1325 if (seg->moving) continue;
1326 movpos_unreserve(seg->motion);