8 #include "../layout/layout-data.h"
9 #include "layoutinfo.h"
10 #include "errorcodes.h"
12 /*========== more basic types etc. ==========*/
14 typedef long TimeInterval; /*ms*/
16 /*---------- units and scales ----------*/
19 * Distances are in mm.
20 * Times are generally in s.
23 /*========== state of the layout ==========*/
25 typedef struct MovPosChange MovPosChange;
27 #define SPEEDSTEPS 126
30 float speed; /* applies to all speeds up to and including this one */
31 Distance xs; /* max stopping distance */
32 TimeInterval ts; /* max stopping time */
36 /* Configuration (excluding speed curve): */
39 Distance head, detectable, tail;
42 struct Segment *foredetect;
43 Distance maxinto, uncertainty;
44 /* Front of train's detectable portion (the `reference') is at
45 * most maxinto into foredetect, but may be up to uncertainty
46 * further back (which may put it in a previous segment). If the
47 * train is stationary it owns the track forwards from the
48 * foremost reference possibility, plus ->head, plus MARGIN_NOSE
49 * (and if it is not stationary it may own more). It owns the
50 * track backwards from the rearmost reference possibility for
51 * ceil(->detectable * MARGIN_TRAINLENGTH) + ->tail + MARGIN_NOSE.
54 backwards:1, /* train is moving backwards wrt its own front and back */
55 autopoint:1, /* set points automatically if point has autopoint too */
57 /* Startup resolution (resolve.c): */
58 resolution:2; /* for use by speedmanager */
60 int plan_lookahead_nsegs; /* counts from foredetect exclusive */
63 int step; /* commanded */
64 TimeoutEvent decel; /* running iff decelerating */
65 double speed; /* iff decerating, at last change; otherwise accurate */
66 RetransmitUrgentNode rn;
70 SpeedRange *speedregimes;
71 float speedcurve[SPEEDSTEPS+1];
75 Train *owner; /* or 0 */
76 Train *home; /* or 0 */
78 tr_backwards:1, /* train's motion is (would be) backwards wrt track */
79 ho_backwards:1, /* home train has its front and rear backwards wrt track */
80 autopoint:1, /* set points automatically if train has autopoint too */
81 seg_inverted:1, /* polarity is inverted */
82 det_ignore:1, det_expected:1, /* safety.c */
83 moving:1, /* feature(s) have been told to change */
84 mark0:1,mark1:1,mark2:1,mark3:1, /* for temporary private uses */
85 res_movposset:1, /* we have set the position so it is known good */
86 res_detect:1, /* detection noticed here during resolution */
87 detect_actual:1, /* last detection state received */
88 detect_reported:1; /* detection state reported to parent */
90 MovPosComb movposcomb; /* -1 means not known or moving */
91 MovPosChange *motion; /* if ->moving, owned by movpos, otherwise by safety */
92 TimeInterval until; /* } for use by safety.c */
93 MovPosChange *motion_newplan; /* } during predict */
97 /*========== embed.c ==========*/
98 /* surrounds the algorithms and machinery in a program: logging, error
99 * handling, arg parsing, etc.
102 void safety_vpanic(Train *tra, Segment *seg, const char *fmt, va_list al)
103 __attribute__((format(printf,3,0),noreturn));
104 void safety_panic(Train *tra, Segment *seg, const char *fmt,...)
105 __attribute__((format(printf,3,4),noreturn));
107 /*========== safety.c ==========*/
109 * safety.c is responsible for ensuring that things don't go
110 * physically wrong (eg, collisions, derailments, short circuits,
114 typedef void PredictionProblemCallback(Train *tra, Segment *seg /* may be 0 */,
115 void *pu, const char *message);
117 ErrorCode safety_checktrain(Train *tra,
118 PredictionProblemCallback *ppc, void *ppcu);
120 void safety_notify_detection(Segment *seg);
121 /* Called by startup.c when new train detection occurs in state Run. */
123 ErrorCode safety_movposchange(Segment *seg, MovPosComb comb,
124 int allow_queueing, PredictionProblemCallback *ppc, void *ppcu);
125 /* If this succeeds, caller need not call movpos_change.
126 * allow_queueing==0 means we only allow unowned segments to move
127 * ==1 allows changing a segment the train is approaching
128 * ==2 allows queueing a change for when the train has left
131 ErrorCode safety_setdirection(Train *tra, int backwards,
132 PredictionProblemCallback *ppc, void *ppcu);
134 #define PREDF_JUSTDET 001u
135 /* we have just detected the train entering its foredetect so we
136 * can subtract the uncertainty from the stopping distance */
137 #define PREDF_OLDPLAN 002u
138 /* the old plan is necessarily sufficient as we are not intending
139 * to accelerate, change a movpos, etc. */
140 #define PREDF_INITQUEST 004u
141 /* the initial location is questionable (ie, the initial present
142 * marking is allowed to fail) */
146 double max_speed_estimate;
147 Distance stopping_distance;
150 ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
151 Segment *desire_move, int desire_movposcomb,
152 const SpeedInfo *speed_info /* 0: use speedmgr */,
153 PredictionProblemCallback *ppc, void *ppcu);
154 /* Lower-level interface for speedmanager etc.
155 * Caller must call this with different situations until it succeeds!
156 * Caller may pass ppc=0 and ppcu=(char*)"some context" to
157 * cause safety_panic if it fails.
160 void report_train_position(Train *tra);
161 void report_train_ownerships(Train *tra, Segment *furthest,
162 int always_use_motions);
163 /* ... for use by things which update these only, which
164 * is basically safety.c and resolve.c */
166 void safety_abandon_run(void);
168 /*========== movpos.c ==========*/
170 * movpos.c manages the CDU and points and other moveable features.
171 * Only safety.c should request changes.
175 movpos_change(Segment *tomove, MovPosComb target,
176 int maxdelay_ms, MovPosChange *reservation);
177 /* If segment has already been requested to change, an attempt is
178 * made to replace that change with the new request; if this is not
179 * successful then the existing change will still happen.
181 * reservation should be 0, or the results of movpos_reserve
182 * on the same move. It is always consumed, even on error.
184 * One of the following must be true of tomove on entry:
185 * moving==1 motion already owned by movpos, see above
186 * motion==0 motion empty, will become owned by movpos
188 * On successful exit, tomove->moving=1 and tomove->motion!=0; from
189 * then on until ->moving becomes 0, movposcomb may not reflect the
190 * real physical state of the layout; instead it gives only
191 * information about the target configuration. (Choreographers are
192 * allowed to know implementation details and may know what
193 * movposcomb means for certain segments depending on the movement
196 * If a reservation is supplied, maxdelay_ms may be -1 to use the
197 * same value as was given to movpos_reserve, or if no reservation
198 * given, to say we don't care.
201 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
202 MovPosComb startpoint, MovPosComb *chosen_r);
203 /* Looks for a movposcomb of move where the adjacent segment in one
204 * direction is back and the other fwd. back and fwd may be 0 if we
205 * don't care (and must be if there is no track in that direction.
206 * It is immaterial which is back and which fwd.
208 * The returned movposcomb is the `best' one which is the one
209 * closest to the starting point (and if that is not unique, the
212 * startpoint=-1 means starting with current state. chosen_r may be 0.
216 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
217 MovPosComb target, MovPosComb startpoint /*as for findcomb*/);
218 /* The resulting MovPosChange is a reservation which is guaranteed
219 * to be useable successfully later for any movpos_change for the
220 * same move, target and startpoint and a greater or equal
221 * maxdelay_ms. On successful exit *res_r is set to non-0.
223 * On transition out of Sta_Run, all unconfirmed reservations must
224 * be unreserved before points_all_abandon is called (see
228 void movpos_unreserve(MovPosChange *reservation /* 0 ok */);
230 MovPosComb movpos_poscomb_actual(const Segment *seg);
231 /* gives actual current position as published by movfeatkind
232 * -1 means not known or cannot be represented as a MovPosComb
235 MovPosComb movpos_change_intent(MovPosChange *chg);
236 /* Returns the target value supplied to _reserve or _change */
238 /* Error returns from movpos calls
240 * EC_Invalid there is no movposcomb of tomove matching back/fwd
241 * EC_MovFeatTooLate maxdelay_ms could not be met
242 * EC_MovFeatReservationInapplicable
243 * reservation not compatible with change request
244 * and/or current situation
247 const char *movpos_pname(const Segment *move, MovPosComb poscomb);
249 MovPosComb movposcomb_update_feature(MovPosComb startpoint,
250 const MovFeatInfo *mfi,
252 /* Returns movposcomb representing a changed combination of positions;
255 /*========== speed.c ==========*/
257 ErrorCode speedmanager_speedchange_request(Train *tra, int step,
258 PredictionProblemCallback *ppc, void *ppcu);
259 /* ppc and ppcu as for predict_confirm */
261 void speedmanager_safety_stop(Train *tra, struct timeval tnow,
262 unsigned predictflags);
263 /* Ignores value of PREDF_OLDPLAN */
264 void speedmanager_reset_train(Train *tra);
266 void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo*);
267 int speedmanager_stopped(Train *tra);
269 void actual_speed(Train *tra, int step);
271 void speedmanager_init(void);
273 /*========== actual.c ==========*/
274 /* actual.c should only be called from safety.c.
275 * It is responsible for communicating with the PICs, including
276 * repeating the NMRA commands and redacting detection information.
278 * In general, when State information is shared between actual.c
279 * and safety.c, safety.c is responsible for modifying State, and
280 * will then call an actual_... function to notify the change.
283 void actual_setspeed(Train *tra);
284 void actual_emergencystop(Train *tra);
286 void actual_inversions_start(void);
287 void actual_inversions_segment(Segment *seg);
288 void actual_inversions_done(void);
289 /* safety.c will call these in this order: first start, then segment
290 * for 0 or more segments (whose s.segments[segn].seg_inverted may
291 * or may not have changed), and finally done. At done, the PICs
292 * should be told to (de)invert the segments as specified. */
295 * Entrypoints are: Called from, and as a result of:
296 * actual_setspeed safety.c
297 * actual_emergencystop safety.c
300 /*========== utils.c ==========*/
302 typedef struct TrackLocation TrackLocation;
303 struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
304 Segment *seg; /* current segment */
305 Distance remain; /* distance from end of segment as we look at it */
306 unsigned backwards:1; /* if 1, into is positive and measured from end */
309 void trackloc_set_maxinto(TrackLocation *tloc, Segment *seg, int backwards);
311 typedef struct TrackAdvanceContext {
312 /* all set by caller, only distance modified by trackloc: */
314 /* All event callbacks are optional; if not supplied we pretend
315 * that they were a no-op which returned 0. */
316 int (*nextseg)(TrackLocation *t, struct TrackAdvanceContext *c,
317 MovPosComb *mpc_io, const TrackLocation *before);
318 /* On entry *mpc_io is from next->movposcomb. nextseg may modify
319 * it if it feels like it in which case the modified value will
320 * be used instead. If on 0-return it is still -1, getmovpos is used.
321 * t->remain is not valid on entry and should not be touched.
322 * Called once right at the start with before==0. */
323 int (*getmovpos)(TrackLocation *t, struct TrackAdvanceContext *c,
325 /* Will be called *use_io on entry is
326 * - at the start of trackloc_advance value from segment
327 * - after nextseg value left by nextseg
328 * - by trackloc_getlink getlink's mpc argument
329 * - by interfering_segment base->movposcomb
330 * May modify *use_io if it wishes. It is OK to leave it as
331 * -1 in which case advance will return whatever getmovpos did
332 * (even if 0). When called by interfering_segment, non-0 returns
333 * will be treated as an indication that the movposcomb is unknown.
334 * On entry t->remain is invalid; it should not be modified. */
335 int (*trackend)(TrackLocation *t, struct TrackAdvanceContext *c);
336 /* trackloc_advance stops even if this returns 0. */
338 } TrackAdvanceContext;
340 int trackloc_getlink(TrackLocation *t, TrackAdvanceContext *c,
341 const SegPosCombInfo **pci_r /* 0 ok */,
342 const SegmentLinkInfo **link_r /* 0 ok */,
343 MovPosComb mpc /* -1: get from segment */);
344 /* Of c, only c->getmovpos is used. t->remain is not used.
345 * c may be 0 which works just like c->getmovpos==0. */
347 Segment *segment_interferer(Segment *base);
348 /* Returns the segment which may interfere with base, if any. */
350 ErrorCode segment_interferer_does(TrackAdvanceContext *c, Segment *base,
351 Segment *interferer, int *does_r);
352 /* Checks whether it interferes with base.
353 * Return is 0 or the error value from c->getmovpos.
354 * *does_r is set (only) on successful return,
355 * c is used as for trackloc_getlink. If c->getmovpos is 0
356 * the segments are taken to interfere if they would in any movposcomb.
357 * interferer may be 0 in which case succeeds, setting *does_r=0.
360 Segment *segment_interferes_simple(TrackAdvanceContext *c, Segment *base);
361 /* Combines segment_interferer and segment_interferer_does.
362 * c->getmovpos MUST NOT return errors; we abort if it does. */
364 int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c);
365 /* Advances t by dist until distance becomes 0, some callback
366 * returns non-0, or sometimes 0 if we cannot continue and the
367 * relevant callback is missing or returned 0. If we exhausted
368 * c->distance then *t is the end of the counted distance, on the
369 * earlier of the two segments if there is a choice. Otherwise *t
370 * is the last location on the last sensible segment. (Ie, nextseg
371 * and getmovpos get a putative new segment and if they don't work
372 * trackloc_advance rewinds.) In any case c->distance is updated by
373 * the distance successfully traversed. */
375 int trackloc_reverse_exact(TrackLocation *t, TrackAdvanceContext *c);
376 int trackloc_set_exactinto(TrackLocation *t, TrackAdvanceContext *c,
377 Segment *seg, int backwards, int into);
378 /* c is used just as for trackloc_getlink.
379 * If we can't determine the movposcombs we call abort. */
381 #define TL_DIST_INF (INT_MAX/16)
383 /*========== useful macros and declarations ==========*/
385 /*---------- looping over trains and segments ----------*/
387 #define SEGMENT_ITERVARS(seg) \
390 const SegmentInfo *seg##i
392 #define TRAIN_ITERVARS(tra) \
396 #define FOR_SEGMENT(seg, initx, stepx) \
397 for (seg##n=0, seg=segments, seg##i=info_segments; \
398 seg##n < NUM_SEGMENTS; \
399 seg##n++, seg++, seg##i++, stepx)
401 #define FOR_TRAIN(tra, initx, stepx) \
402 for (tra##n=0, tra=trains, initx; \
404 tra##n++, tra++, stepx)
406 #define SEG_IV SEGMENT_ITERVARS(seg)
407 #define FOR_SEG FOR_SEGMENT(seg,(void)0,(void)0)
409 #define TRA_IV TRAIN_ITERVARS(tra)
410 #define FOR_TRA FOR_TRAIN(tra,(void)0,(void)0)
412 /*---------- calculations with fixed point speeds ----------*/
414 #define SPEED_CALC_TIME(speed,dist,round) ((dist) / ((speed)+1e-6))
415 #define SPEED_CALC_DIST(speed,time,round) ((time) * (speed))
417 /*---------- safety margin parameters ----------*/
419 #define MARGIN_NOSE 6 /*mm*/ /* physical spare at each end of train */
420 #define MARGIN_TAIL 6 /*mm*/ /* spare when in motion only */
421 #define MARGIN_SPEED 1.2 /*ratio*/
422 #define MARGIN_AUTOPOINTTIME 500 /*ms*/
423 #define UNMARGIN_ROUNDING 1e-4 /* mm/ms; for 1s, leads to max err of 100um */
424 #define MARGIN_TRAINLENGTH 1.05
426 #define MARGIN_STOPTIME 240 /*ms*/
427 #define MARGIN_POLARISETIME 120 /*ms*/
429 #define UNMARGIN_WATCHDOG_16 7 /* 16ms units */
430 #define UNMARGIN_WATCHDOG (UNMARGIN_WATCHDOG_16*16) /*ms*/
431 /* should be < MARGIN_POLARISETIME */
432 #define MARGIN_WATCHDOG 40