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);
438 /* it had better be set for where we came from */
440 assert(SOMEP(*mpc_io));
441 const SegPosCombInfo *pci= &t->seg->i->poscombs[*mpc_io];
442 const SegmentLinkInfo *link= &pci->link[!t->backwards];
443 if (!SOMEP(link->next) || &segments[link->next] != before->seg)
444 return predict_problem(u, t->seg, "route set against proposed location");
447 t->seg->now_present= t->seg->pred_present= t->seg->will_polarise= 1;
448 t->seg->tr_backwards= !t->backwards;
450 if (!t->seg->i->invertible)
451 u->noninv_tally[!t->backwards]++; /* ! since going backwards along train */
455 /*---------- prediction nose advancement ----------*/
457 static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
458 MovPosComb *mpc_io, const TrackLocation *before) {
459 PredictUserContext *u= c->u;
460 MovPosComb route_plan;
464 pred_callback_debug(" nose_nextseg",t,c,before);
465 if (!before) return 0;
468 advance_elapsed(u,calc_advanced(c));
472 if (u->stopping && u->lookahead == u->train->plan_lookahead_nsegs)
473 return EC_SignallingHorizonReached;
476 if (t->seg->owner != u->train)
477 return predict_problem(u, t->seg, "impeded by %s",
478 t->seg->owner->pname);
480 if (t->seg->pred_present)
481 return predict_problem(u, t->seg, "will collide with itself!");
483 if (!u->optimistic) {
484 Segment *interferer= segment_interferer(t->seg);
485 /* We want to avoid calling segment_interferer_does unless we have
486 * to, as it uses getmovpos which, if the movposcomb is unknown,
487 * will predict_problem even if the interferer is empty. */
489 (interferer->owner || interferer->pred_present)) {
491 ec= segment_interferer_does(c,t->seg,interferer, &does);
495 if (interferer->owner && interferer->owner != u->train)
496 return predict_problem(u, t->seg, "impeded by %s at %s",
497 interferer->owner->pname, interferer->i->pname);
498 if (interferer->pred_present)
499 return predict_problem(u, t->seg, "will collide with itself at %s!",
500 interferer->i->pname);
505 /* What about the route ? */
507 if (t->seg->i->n_poscombs==1)
510 if (t->seg->moving) {
511 assert(!t->seg->now_present);
513 return predict_problem(u, t->seg, "route not yet set");
514 if (!u->neednewplan) {
515 t->seg->motion_newplan= t->seg->motion;
516 *mpc_io= movpos_change_intent(t->seg->motion);
521 int autopoint= u->train->autopoint && t->seg->autopoint;
523 if (t->seg->motion_newplan) {
524 route_plan= movpos_change_intent(t->seg->motion);
527 } else if (t->seg == u->desire_move && !t->seg->now_present) {
528 ec= movpos_findcomb_bysegs(before->seg,t->seg,0,
529 u->desire_movposcomb, &route_plan);
531 if (route_plan != u->desire_movposcomb)
532 return predict_problem(u, t->seg, " proposed route would be against"
533 " approaching train");
535 } else if (t->seg->motion) {
536 /* We already have a plan. */
537 route_plan= movpos_change_intent(t->seg->motion);
538 if (!u->neednewplan) {
539 t->seg->motion_newplan= t->seg->motion;
544 /* Extend the plan. */
545 ec= movpos_findcomb_bysegs(before->seg,t->seg,0,*mpc_io, &route_plan);
546 assert(!ec); /* there must be _some_ route since
547 we're moving into t->seg */
549 if (route_plan == *mpc_io && !t->seg->moving) {
550 /* Well, that's all fine then. */
553 /* We can make it work but have to plan some motion: */
555 return predict_problem(u, t->seg, "train arriving but route not yet set");
556 if (t->seg->pred_vacated) {
557 if (!t->seg->now_present)
558 /* And we're not even there yet! This is too hard because we
559 * are too stupid not to change it straight away. */
560 return predict_problem(u, t->seg, "dynamic route too complicated"
561 " - would need multiple queued changes");
562 max_ms= u->elapsed - t->seg->until;
568 return predict_problem(u,t->seg,"will not automatically set route");
570 ec= movpos_reserve(t->seg, max_ms, &t->seg->motion_newplan,
571 route_plan, *mpc_io);
572 if (ec) return predict_problem(u, t->seg, "insufficient time to"
573 " set route: %s", ec2str(ec));
577 /* Now we definitely have a plan which sets a good route at t->seg. */
578 if (!t->seg->pred_vacated)
579 t->seg->tr_backwards= t->backwards;
580 t->seg->pred_present= 1;
581 t->seg->until= u->elapsed < 0 ? 0 : u->elapsed;
586 /*---------- prediction tail advancement ----------*/
588 static int tail_nextseg(TrackLocation *t, TrackAdvanceContext *c,
589 MovPosComb *mpc_io, const TrackLocation *before) {
590 PredictUserContext *u= c->u;
592 pred_callback_debug(" tail_nextseg",t,c,before);
593 if (!before) return 0;
595 if (!before->seg->i->invertible)
596 u->noninv_tally[before->backwards]--;
598 if (before->seg->pred_vacated) return 0; /* only vacate once */
599 before->seg->pred_present= 0;
600 before->seg->pred_vacated= 1;
601 before->seg->until= u->elapsed < 0 ? 0 : u->elapsed;
606 /*---------- prediction foredetect advancement ----------*/
608 static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
609 MovPosComb *mpc_io, const TrackLocation *before) {
610 PredictUserContext *u= c->u;
613 /* NB, on entry the movposcomb of the segment we're entering (and
614 * other similar details) may not be known or sufficient because
615 * when fdet has only just entered a new segment, nose is still in
616 * the previous segment.
619 pred_callback_debug("fdet_nextseg",t,c,before);
620 if (!before) return 0;
622 advanced= calc_advanced(c);
623 advance_elapsed(u,advanced);
625 u->nosec.distance= advanced;
626 r= trackloc_advance(&u->nose,&u->nosec);
628 DPRINTF(safety,predictseg," fdet_nextseg r=%s\n",ec2str(r));
632 /* Now we have advanced the nose and have recorded any appropriate
633 * motion(s). Advancing the nose has updated the segment's
634 * notion of the motion but not trackloc_advance's cached version of
635 * the movposcomb in *mpc_io - but trackloc_advance will call
636 * pred_getmovpos right after we return so we do not need to worry.
641 if (!t->seg->i->invertible) {
642 u->noninv_tally[t->backwards]++;
643 if (u->noninv_tally[0] && u->noninv_tally[1])
644 return predict_problem(u, t->seg, "cannot set track polarity");
647 if (!u->done_first_new_fdet) {
648 t->seg->det_expected= 1;
649 u->done_first_new_fdet= 1;
652 if (u->elapsed < MARGIN_POLARISETIME) {
653 u->need_polarise |= (t->seg->seg_inverted ^ t->backwards ^
654 u->train_polarity_inverted);
656 u->walk_compute_polarise &= u->need_polarise;
659 if (u->train->autopoint && !u->autopoint_distance) {
660 const SegPosCombInfo *pci;
661 r= trackloc_getlink(t,c,&pci,0,-1); assert(!r);
662 u->autopoint_distance= pci->dist;
665 if (u->walk_compute_polarise) {
666 if (t->seg->will_polarise)
667 /* this is our 2nd visit, stop now */
668 u->walk_compute_polarise= 0;
670 if (u->walk_compute_polarise) {
671 if (!u->know_best_polarity) {
672 if (u->noninv_tally[0]) {
673 u->train_polarity_inverted= 0;
674 u->know_best_polarity= 1;
675 } else if (u->noninv_tally[1]) {
676 u->train_polarity_inverted= 1;
677 u->know_best_polarity= 1;
680 if (u->know_best_polarity &&
681 !t->seg->i->invertible &&
682 u->train_polarity_inverted != t->backwards) {
683 /* incompatible, stop now */
684 u->walk_compute_polarise= 0;
687 if (u->walk_compute_polarise) {
688 t->seg->will_polarise= 1;
691 /* Advance the tail */
693 u->tailc.distance += advanced;
694 if (u->tailc.distance > 0) {
695 r= trackloc_advance(&u->tail,&u->tailc);
697 assert(!u->tailc.distance);
700 /* Final adjustments, prepare for next iteration */
702 u->was_distance= c->distance;
706 /*---------- backtracking the optimistic nose ----------*/
708 static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c,
709 MovPosComb *mpc_io, const TrackLocation *before) {
710 PredictUserContext *u= c->u;
712 pred_callback_debug("optunwind_nextseg",t,c,before);
714 if (t->seg == u->furthest ||
715 t->seg->motion_newplan)
716 return EC_SignallingHorizonReached;
718 t->seg->pred_present= 0;
724 /*========== prediction entrypoint ==========*/
726 ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
727 Segment *desire_move, int desire_movposcomb,
728 const SpeedInfo *speed_info,
729 PredictionProblemCallback *ppc, void *ppcu) {
730 PredictUserContext u;
731 SpeedInfo speed_info_buf;
737 memset(&u,0,sizeof(u));
739 u.problem_callback= ppc;
740 u.problem_callback_u= ppcu;
741 u.neednewplan= !(flags & PREDF_OLDPLAN);
742 u.speed_info_specified= !!speed_info;
745 speedmanager_getinfo(tra,tnow,&speed_info_buf);
746 speed_info= &speed_info_buf;
748 u.stopping= (flags & PREDF_OLDPLAN) && speed_info->stopping;
749 u.maxspeed= speed_info->max_speed_estimate;
750 u.stopping_distance= speed_info->stopping_distance;
752 u.desire_move= desire_move;
753 u.desire_movposcomb= desire_movposcomb;
755 Distance detectable= ceil(tra->detectable * MARGIN_TRAINLENGTH);
757 if (DEBUGP(safety,core)) {
758 DPRINTF(safety,core," ***predicting*** %s%s ",
759 tra->backwards?"-":"",tra->pname);
760 qprintf_position(tra,DPRINTFA);
761 DPRINTFA(" det.able=%d %smaxspeed=%f%s stopdist=%d (speed %f, step %d%s)"
762 " flags=%c%c%s desire=%s/%s\n",
764 u.speed_info_specified ? "specified " : "",
765 u.maxspeed, u.stopping ? "(stopping)" : "",
767 tra->speed.speed, tra->speed.step,
768 tra->speed.decel.running ? " decel" : "",
769 "-j"[ !!(flags & PREDF_JUSTDET) ],
770 "no"[ !!(flags & PREDF_OLDPLAN) ],
771 u.stopping ? " stopping" : "",
772 u.desire_move ? u.desire_move->i->pname : "",
773 u.desire_move ? u.desire_move->i->poscombs[u.desire_movposcomb]
778 if (!seg->owner || seg->owner == u.train)
779 seg->det_expected= 0;
780 seg->now_present= seg->pred_present=
781 seg->pred_vacated= seg->will_polarise= 0;
784 foredet= tra->foredetect;
787 u.walk_compute_polarise= 1;
789 u.train_polarity_inverted= foredet->seg_inverted ^ foredet->tr_backwards;
791 /*----- find the train's foredetect (rearmost possibility) -----*/
793 * If tra->uncertainty > tra->maxinto then in theory the foredetect
794 * might be in the previous segment. We don't take that into account,
795 * so we must instead extend our idea of the length of the train.
798 int effective_uncertainty= tra->uncertainty;
799 int effective_length= MARGIN_NOSE + tail_length(tra) + detectable;
801 if (effective_uncertainty > tra->maxinto) {
802 effective_length += tra->uncertainty - tra->maxinto;
803 effective_uncertainty= tra->maxinto;
806 int effective_into_fdet= tra->maxinto - effective_uncertainty;
807 int tail_advance_delaydist= MARGIN_TAIL;
809 DPRINTF(safety,core," predict eff.uncert=%d eff.len=%d"
810 " eff.into_fdet=%d tail.adv.delay=%d\n",
811 effective_uncertainty, effective_length,
812 effective_into_fdet, tail_advance_delaydist);
815 u.fdetc.getmovpos= pred_getmovpos;
817 trackloc_set_exactinto(&u.fdet, &u.fdetc, foredet, foredet->tr_backwards,
818 effective_into_fdet);
820 /*----- find the train's tail (rearmost possibility) -----*/
822 * We walk backwards, also marking the train present.
826 u.tailc.getmovpos= pred_getmovpos;
827 u.tailc.nextseg= initpresent_nextseg;
828 u.tailc.trackend= pred_trackend_panic;
829 u.tailc.distance= effective_length;
833 ec= trackloc_reverse_exact(&u.tail,&u.tailc); assert(!ec);
835 ec= trackloc_advance(&u.tail,&u.tailc);
836 if (ec) { assert(flags & PREDF_INITQUEST); return ec; }
838 ec= trackloc_reverse_exact(&u.tail,&u.tailc); assert(!ec);
840 u.hindmost= u.tail.seg;
842 /*----- find the train's nose (rearmost possibility) -----*/
846 u.nosec.getmovpos= pred_getmovpos;
847 u.nosec.nextseg= nose_nextseg;
848 u.nosec.trackend= pred_trackend;
849 u.nosec.distance= nose_length(tra);
853 ec= trackloc_advance(&u.nose,&u.nosec);
854 if (ec && ec != EC_SignallingHorizonReached) goto xproblem;
856 /*----- predict the future - (including the present uncertainty) -----*/
858 u.fdetc.nextseg= fdet_nextseg;
859 u.fdetc.getmovpos= pred_getmovpos;
860 u.fdetc.trackend= pred_trackend;
861 u.tailc.nextseg= tail_nextseg;
862 u.tailc.distance= -tail_advance_delaydist;
865 /* we're carrying on until the next segment */
866 u.fdetc.distance= u.fdet.remain;
867 } else if (flags & PREDF_JUSTDET) {
868 /* we actually know exactly where we are */
870 } else { /* stopping, but we're mid-segment */
871 u.fdetc.distance= effective_uncertainty;
873 u.elapsed= -u.fdetc.distance / u.maxspeed; /* imagine we're already there */
874 u.fdetc.distance += u.stopping_distance + MARGIN_NOSE;
875 u.was_distance= u.fdetc.distance;
877 ec= trackloc_advance(&u.fdet,&u.fdetc);
878 if (ec && ec != EC_SignallingHorizonReached) goto xproblem;
880 /*----- look ahead for autopoint -----*/
882 if (tra->autopoint) {
883 Distance min_autopoint_dist= MARGIN_AUTOPOINTTIME * u.maxspeed;
885 DPRINTF(safety,predict," optimism %d (min %d) nose %s\n",
886 u.autopoint_distance, min_autopoint_dist, u.nose.seg->i->pname);
888 if (u.autopoint_distance < min_autopoint_dist)
889 u.autopoint_distance= min_autopoint_dist;
891 u.furthest= u.nose.seg;
893 u.was_distance= u.nosec.distance= u.autopoint_distance;
894 trackloc_advance(&u.nose,&u.nosec);
896 /* But actually we don't want to end up owning
897 * segments unless we have to for motions. */
899 u.nose.backwards ^= 1;
901 u.nosec.nextseg= optunwind_nextseg;
902 u.nosec.distance= TL_DIST_INF;
904 ec= trackloc_advance(&u.nose,&u.nosec);
905 assert(ec == EC_SignallingHorizonReached);
908 /*----- commit to the plan -----*/
910 DPRINTF(safety,predict," committing la#%d %c%c%c\n",
912 "-n"[ u.need_polarise ],
913 "-i"[ u.train_polarity_inverted ],
914 "-b"[ u.know_best_polarity ]);
916 tra->plan_lookahead_nsegs= u.lookahead;
918 if (u.need_polarise) {
919 DPRINTF(safety,predict," polarising train_polarity=%d\n",
920 u.train_polarity_inverted),
921 actual_inversions_start();
924 DPRINTF1(safety,predictplan," ");
926 DPRINTF2(" %s%s%s%c%c%c%c/%s%s%s%s%s",
927 seg->tr_backwards?"-":"", seg->i->pname,
928 seg->owner == tra ? "=" : seg->owner ? "#" : "-",
929 "-N"[ seg->now_present ],
930 "-P"[ seg->pred_present ],
931 "-V"[ seg->pred_vacated ],
932 "-p"[ seg->will_polarise ],
933 movpos_pname(seg,seg->movposcomb),
934 seg->motion ? (seg->moving ? "!" : "~") : "",
935 seg->motion ? motion_pname(seg,seg->motion) : "",
936 seg->motion_newplan ? ">" : "",
937 seg->motion && seg->motion_newplan==seg->motion ? "=" :
938 motion_pname(seg,seg->motion_newplan));
940 /* set ownership and det_ignore */
941 if (seg->pred_present || seg->pred_vacated) {
943 seg->det_ignore= seg->now_present;
944 } else if (seg->owner == tra) {
947 /* garbage collect our old plan for now-irrelevant segments */
948 assert(!seg->motion_newplan);
950 movpos_unreserve(seg->motion);
955 /* segment not relevant to us at all */
959 /* now it's our segment: */
961 /* clear any irrelevant old plan */
962 if (seg->motion && !seg->moving) {
963 if (seg->motion != seg->motion_newplan)
964 movpos_unreserve(seg->motion);
967 /* now motion!=0 only if seg->moving */
969 /* install the new plan, if any */
970 if (seg->motion_newplan) {
971 if (seg->motion_newplan == seg->motion) {
972 /* excellent, already doing it */
973 } else if (!seg->now_present) {
974 MovPosComb target= movpos_change_intent(seg->motion_newplan);
977 ec= movpos_change(seg, target, -1, seg->motion_newplan);
979 DPRINTF1(safety,predictplan, " ... ");
980 /* motion is updated by movpos_change */
982 /* we'll do it later, then */
983 seg->motion= seg->motion_newplan;
985 seg->motion_newplan= 0;
987 /* now the new plan is in motion and moving */
989 if (u.need_polarise && seg->will_polarise) {
990 seg->seg_inverted= seg->tr_backwards ^ u.train_polarity_inverted;
991 if (seg->i->invertible) {
992 actual_inversions_segment(seg);
993 Segment *interferer= segment_interferes_simple(0,seg);
994 if (interferer && !interferer->will_polarise &&
995 interferer->i->invertible) {
996 interferer->seg_inverted= seg->seg_inverted
997 ^ seg->i->interferes_polarity_opposed;
998 actual_inversions_segment(interferer);
1001 assert(!seg->seg_inverted);
1004 seg->now_present= seg->pred_present=
1005 seg->pred_vacated= seg->will_polarise= 0;
1009 if (u.need_polarise)
1010 actual_inversions_done();
1012 report_train_ownerships(tra,u.hindmost,0);
1014 if (u.desire_move && !u.desire_move->owner)
1015 /* Oh! Train must be slower and less far advanced than expected.
1016 * Well, then we can move it right away. */
1017 return movpos_change(u.desire_move,u.desire_movposcomb,-1,0);
1022 DPRINTF(safety,predict," returning %s\n", ec2str(ec));
1025 seg->now_present= seg->pred_present=
1026 seg->pred_vacated= seg->will_polarise= 0;
1028 if (seg->motion_newplan==seg->motion) {
1029 seg->motion_newplan= 0;
1031 if (seg->motion_newplan) {
1032 movpos_unreserve(seg->motion_newplan);
1033 seg->motion_newplan= 0;
1035 if (!seg->owner && !seg->moving && seg->motion) {
1036 movpos_unreserve(seg->motion);
1043 /*========== reporting position and ownership ==========*/
1045 void report_train_position(Train *tra) {
1046 ouprintf("train %s at ",tra->pname);
1047 qprintf_position(tra,ouprintf);
1048 ouprintf(" %s\n", tra->backwards ? "backwards" : "forwards");
1051 static int report_getmovpos(TrackLocation *t, TrackAdvanceContext *c,
1052 MovPosComb *use_io) {
1053 PredictUserContext *u= c->u;
1054 if (u->done_fdet && t->seg != u->train->foredetect)
1055 /* we must use current posn for foredetect itself */
1056 u->usecurrentposn= 0;
1057 if (!u->usecurrentposn && t->seg->motion)
1058 *use_io= movpos_change_intent(t->seg->motion);
1063 static int report_nextseg(TrackLocation *t, struct TrackAdvanceContext *c,
1064 MovPosComb *mpc, const TrackLocation *before) {
1065 PredictUserContext *u= c->u;
1070 if (u->reportcount > NUM_SEGMENTS * 2) {
1071 ouprintf(" [... infinite loop!]\n");
1075 if (u->done_fdet) /* we've had foredetect */
1076 if (++u->lookahead > u->train->plan_lookahead_nsegs)
1081 if (t->seg->det_expected)
1083 if (t->seg->det_ignore)
1085 if (t->seg == u->train->foredetect && !u->done_fdet) {
1090 ouprintf(" %s%s", t->backwards?"-":"", t->seg->i->pname);
1092 if (t->seg->i->n_poscombs > 1) {
1093 r= report_getmovpos(t,c,mpc); assert(!r); assert(*mpc>=0);
1094 ouprintf("/%s", t->seg->i->poscombs[*mpc].pname);
1096 ouprintf("%s",flags);
1101 void report_train_ownerships(Train *tra, Segment *hindmost,
1102 int always_use_motions) {
1103 PredictUserContext u;
1105 memset(&u,0,sizeof(u));
1108 u.usecurrentposn= !always_use_motions;
1111 /* Walk along the train printing its segments: */
1112 ouprintf("train %s has", tra->pname);
1114 u.nose.seg= hindmost;
1116 u.nose.backwards= hindmost->tr_backwards;
1118 u.nosec.distance= TL_DIST_INF;;
1119 u.nosec.nextseg= report_nextseg;
1120 u.nosec.getmovpos= report_getmovpos;
1122 u.hindmost= hindmost;
1124 trackloc_advance(&u.nose,&u.nosec);
1129 /*========== reversing a train ==========*/
1131 static int reverse_nextseg(TrackLocation *t, struct TrackAdvanceContext *c,
1132 MovPosComb *mpc_io, const TrackLocation *before) {
1133 PredictUserContext *u= c->u;
1135 DPRINTF(safety,predictseg," reverse_nextseg "
1136 " %c%s dist=%-4d\n",
1142 if (*mpc_io==-1 || t->seg->motion)
1143 return predict_problem(u,t->seg, "segment under train is not stable");
1147 static int reverse_trackend(TrackLocation *t, struct TrackAdvanceContext *c) {
1151 ErrorCode safety_setdirection(Train *tra, int backwards,
1152 PredictionProblemCallback *ppc, void *ppcu) {
1154 * tra->foredetect, maxinto, uncertainty
1156 * tra->plan_lookahead_nsegs
1158 * The following remain unchanged as all their users have
1159 * regard to the train's direction:
1162 TrackLocation newfdet;
1163 TrackAdvanceContext c;
1164 PredictUserContext u;
1165 const SegPosCombInfo *pci;
1167 int oldmaxinto, oldfdet_trbackwards, old_planlookaheadnsegs;
1170 struct timeval tnow;
1172 SKELETON_PREDICT_USER_CONTEXT(u,tra);
1174 MUSTECR( safety_checktrain(tra,ppc,ppcu) );
1176 if (tra->backwards == backwards)
1179 if (!speedmanager_stopped(tra))
1180 return predict_problem(&u,0, "train is not stopped");
1182 newfdet.seg= tra->foredetect;
1183 newfdet.remain= tra->maxinto;
1184 newfdet.backwards= !tra->foredetect->tr_backwards;
1186 c.distance= ceil(tra->detectable * MARGIN_TRAINLENGTH) + tra->uncertainty;
1187 c.nextseg= reverse_nextseg;
1189 c.trackend= reverse_trackend;
1192 MUSTECR( trackloc_advance(&newfdet,&c) );
1194 mgettimeofday(&tnow);
1196 /* right, now let's start fiddling */
1197 oldfdet= tra->foredetect;
1198 oldmaxinto= tra->maxinto;
1199 oldfdet_trbackwards= tra->foredetect->tr_backwards;
1200 old_planlookaheadnsegs= tra->plan_lookahead_nsegs;
1202 r= trackloc_getlink(&newfdet,&c,&pci,0,-1); assert(!r);
1204 tra->foredetect= newfdet.seg;
1205 tra->maxinto= pci->dist - newfdet.remain;
1206 tra->backwards ^= 1;
1207 newfdet.seg->tr_backwards ^= 1;
1208 tra->plan_lookahead_nsegs= INT_MAX;
1210 ec= predict(tra,tnow, PREDF_OLDPLAN|PREDF_INITQUEST, 0,0, 0,ppc,ppcu);
1213 report_train_position(tra);
1217 /* It can happen eg that the uncertainty is more troublesome when
1218 * considered one way than the other. In which case just put it back now. */
1219 assert(ec == EC_SignallingPredictedProblem);
1221 tra->foredetect= oldfdet;
1222 tra->maxinto= oldmaxinto;
1223 tra->backwards ^= 1;
1224 tra->foredetect->tr_backwards= oldfdet_trbackwards;
1225 tra->plan_lookahead_nsegs= old_planlookaheadnsegs;
1227 ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,0,(char*)"abandon reverse");
1233 /*========== entrypoints from rest of the program ==========*/
1235 static void detection_report_problem(Train *tra, Segment *seg,
1236 void *pu, const char *message) {
1237 ouprintf("train %s signalling-problem %s : %s\n",
1238 tra->pname, seg ? seg->i->pname : "-", message);
1241 void safety_notify_detection(Segment *seg) {
1245 struct timeval tnow;
1246 SpeedInfo speed_info;
1248 if (seg->det_ignore) return;
1250 debug_count_event("detection");
1252 if (!seg->det_expected) {
1253 Segment *interferer= segment_interferes_simple(0,seg);
1254 if (!interferer) safety_panic(0,seg, "unexpected detection");
1255 if (interferer->det_ignore) return;
1256 if (!interferer->det_expected)
1257 safety_panic(0,seg, "unexpected detection, perhaps really at %s",
1258 interferer->i->pname);
1259 DPRINTF(safety,core, " detection %s using interferer %s",
1260 seg->i->pname, interferer->i->pname);
1265 if (seg->movposcomb < 0)
1266 safety_panic(tra,seg, "track route not set and train has arrived");
1268 mgettimeofday(&tnow);
1270 speedmanager_getinfo(tra,tnow,&speed_info);
1271 maxinto= seg->i->poscombs[seg->movposcomb].dist;
1272 if (speed_info.stopping && maxinto > speed_info.stopping_distance)
1273 maxinto= speed_info.stopping_distance;
1275 tra->foredetect= seg;
1276 tra->uncertainty= tra->maxinto= maxinto;
1277 tra->plan_lookahead_nsegs--;
1278 report_train_position(tra);
1280 ec= predict(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN,0,0,
1281 &speed_info, detection_report_problem,0);
1284 assert(ec == EC_SignallingPredictedProblem);
1286 if (maxinto > speed_info.stopping_distance)
1287 maxinto= speed_info.stopping_distance;
1288 tra->maxinto= tra->uncertainty= maxinto;
1290 report_train_position(tra);
1291 speedmanager_safety_stop(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN);
1294 ErrorCode safety_movposchange(Segment *seg, MovPosComb comb,
1295 int allow_queueing, PredictionProblemCallback *ppc, void *ppcu) {
1296 PredictUserContext u;
1297 struct timeval tnow;
1300 SKELETON_PREDICT_USER_CONTEXT(u,seg->owner);
1303 return movpos_change(seg,comb,-1,0);
1305 if (allow_queueing<2) {
1306 if (seg->det_ignore)
1307 return predict_problem(&u,seg, "route set for train");
1308 if (seg->det_expected)
1309 return predict_problem(&u,seg, "route set for immiment train");
1311 if (allow_queueing<1)
1312 return predict_problem(&u,seg, "route set for approaching train");
1314 mgettimeofday(&tnow);
1315 ec= predict(seg->owner,tnow, 0, seg,comb, 0, ppc,ppcu);
1318 ec2= predict(seg->owner,tnow, PREDF_OLDPLAN, 0,0, 0,
1319 0,(char*)"abandon movposchange");
1325 ErrorCode safety_checktrain(Train *tra,
1326 PredictionProblemCallback *ppc, void *ppcu) {
1327 if (!tra->foredetect) {
1328 ppc(tra,0,ppcu,"train is not on layout");
1329 return EC_SignallingPredictedProblem;
1334 void safety_abandon_run(void) {
1338 if (seg->moving) continue;
1339 movpos_unreserve(seg->motion);