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. */
143 double max_speed_estimate;
144 Distance stopping_distance;
147 ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
148 Segment *desire_move, int desire_movposcomb,
149 const SpeedInfo *speed_info /* 0: use speedmgr */,
150 PredictionProblemCallback *ppc, void *ppcu);
151 /* Lower-level interface for speedmanager etc.
152 * Caller must call this with different situations until it succeeds!
153 * Caller may pass ppc=0 and ppcu=(char*)"some context" to
154 * cause safety_panic if it fails.
157 void report_train_position(Train *tra);
158 void report_train_ownerships(Train *tra, Segment *furthest,
159 int always_use_motions);
160 /* ... for use by things which update these only, which
161 * is basically safety.c and resolve.c */
163 void safety_abandon_run(void);
165 /*========== movpos.c ==========*/
167 * movpos.c manages the CDU and points and other moveable features.
168 * Only safety.c should request changes.
172 movpos_change(Segment *tomove, MovPosComb target,
173 int maxdelay_ms, MovPosChange *reservation);
174 /* If segment has already been requested to change, an attempt is
175 * made to replace that change with the new request; if this is not
176 * successful then the existing change will still happen.
178 * reservation should be 0, or the results of movpos_reserve
179 * on the same move. It is always consumed, even on error.
181 * One of the following must be true of tomove on entry:
182 * moving==1 motion already owned by movpos, see above
183 * motion==0 motion empty, will become owned by movpos
185 * On successful exit, tomove->moving=1 and tomove->motion!=0; from
186 * then on until ->moving becomes 0, movposcomb may not reflect the
187 * real physical state of the layout; instead it gives only
188 * information about the target configuration. (Choreographers are
189 * allowed to know implementation details and may know what
190 * movposcomb means for certain segments depending on the movement
193 * If a reservation is supplied, maxdelay_ms may be -1 to use the
194 * same value as was given to movpos_reserve, or if no reservation
195 * given, to say we don't care.
198 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
199 MovPosComb startpoint, MovPosComb *chosen_r);
200 /* Looks for a movposcomb of move where the adjacent segment in one
201 * direction is back and the other fwd. back and fwd may be 0 if we
202 * don't care (and must be if there is no track in that direction.
203 * It is immaterial which is back and which fwd.
205 * The returned movposcomb is the `best' one which is the one
206 * closest to the starting point (and if that is not unique, the
209 * startpoint=-1 means starting with current state. chosen_r may be 0.
213 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
214 MovPosComb target, MovPosComb startpoint /*as for findcomb*/);
215 /* The resulting MovPosChange is a reservation which is guaranteed
216 * to be useable successfully later for any movpos_change for the
217 * same move, target and startpoint and a greater or equal
218 * maxdelay_ms. On successful exit *res_r is set to non-0.
220 * On transition out of Sta_Run, all unconfirmed reservations must
221 * be unreserved before points_all_abandon is called (see
225 void movpos_unreserve(MovPosChange *reservation /* 0 ok */);
227 MovPosComb movpos_poscomb_actual(Segment *seg);
228 /* gives actual current position as published by movfeatkind
229 * -1 means not known or cannot be represented as a MovPosComb
232 MovPosComb movpos_change_intent(MovPosChange *chg);
233 /* Returns the target value supplied to _reserve or _change */
235 /* Error returns from movpos calls
237 * EC_Invalid there is no movposcomb of tomove matching back/fwd
238 * EC_MovFeatTooLate maxdelay_ms could not be met
239 * EC_MovFeatReservationInapplicable
240 * reservation not compatible with change request
241 * and/or current situation
244 const char *movpos_pname(Segment *move, MovPosComb poscomb);
246 MovPosComb movposcomb_update_feature(MovPosComb startpoint,
247 const MovFeatInfo *mfi,
249 /* Returns movposcomb representing a changed combination of positions;
252 /*========== speed.c ==========*/
254 ErrorCode speedmanager_speedchange_request(Train *tra, int step,
255 PredictionProblemCallback *ppc, void *ppcu);
256 /* ppc and ppcu as for predict_confirm */
258 void speedmanager_safety_stop(Train *tra, struct timeval tnow,
259 unsigned predictflags);
260 /* Ignores value of PREDF_OLDPLAN */
261 void speedmanager_reset_train(Train *tra);
263 void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo*);
264 int speedmanager_stopped(Train *tra);
266 void actual_speed(Train *tra, int step);
268 void speedmanager_init(void);
270 /*========== actual.c ==========*/
271 /* actual.c should only be called from safety.c.
272 * It is responsible for communicating with the PICs, including
273 * repeating the NMRA commands and redacting detection information.
275 * In general, when State information is shared between actual.c
276 * and safety.c, safety.c is responsible for modifying State, and
277 * will then call an actual_... function to notify the change.
280 void actual_setspeed(Train *tra);
281 void actual_emergencystop(Train *tra);
283 void actual_inversions_start(void);
284 void actual_inversions_segment(Segment *seg);
285 void actual_inversions_done(void);
286 /* safety.c will call these in this order: first start, then segment
287 * for 0 or more segments (whose s.segments[segn].seg_inverted may
288 * or may not have changed), and finally done. At done, the PICs
289 * should be told to (de)invert the segments as specified. */
292 * Entrypoints are: Called from, and as a result of:
293 * actual_setspeed safety.c
294 * actual_emergencystop safety.c
297 /*========== utils.c ==========*/
299 typedef struct TrackLocation TrackLocation;
300 struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
301 Segment *seg; /* current segment */
302 Distance remain; /* distance from end of segment as we look at it */
303 unsigned backwards:1; /* if 1, into is positive and measured from end */
306 void trackloc_set_maxinto(TrackLocation *tloc, Segment *seg, int backwards);
308 typedef struct TrackAdvanceContext {
309 /* all set by caller, only distance modified by trackloc: */
311 /* All event callbacks are optional; if not supplied we pretend
312 * that they were a no-op which returned 0. */
313 int (*nextseg)(TrackLocation *t, struct TrackAdvanceContext *c,
314 MovPosComb *mpc_io, const TrackLocation *before);
315 /* On entry *mpc_io is from next->movposcomb. nextseg may modify
316 * it if it feels like it in which case the modified value will
317 * be used instead. If on 0-return it is still -1, getmovpos is used.
318 * t->remain is not valid on entry and should not be touched.
319 * Called once right at the start with before==0. */
320 int (*getmovpos)(TrackLocation *t, struct TrackAdvanceContext *c,
322 /* Will be called *use_io on entry is
323 * - at the start of trackloc_advance value from segment
324 * - after nextseg value left by nextseg
325 * - by trackloc_getlink getlink's mpc argument
326 * - by interfering_segment base->movposcomb
327 * May modify *use_io if it wishes. It is OK to leave it as
328 * -1 in which case advance will return whatever getmovpos did
329 * (even if 0). When called by interfering_segment, non-0 returns
330 * will be treated as an indication that the movposcomb is unknown.
331 * On entry t->remain is invalid; it should not be modified. */
332 int (*trackend)(TrackLocation *t, struct TrackAdvanceContext *c);
333 /* trackloc_advance stops even if this returns 0. */
335 } TrackAdvanceContext;
337 int trackloc_getlink(TrackLocation *t, TrackAdvanceContext *c,
338 const SegPosCombInfo **pci_r /* 0 ok */,
339 const SegmentLinkInfo **link_r /* 0 ok */,
340 MovPosComb mpc /* -1: get from segment */);
341 /* Of c, only c->getmovpos is used. t->remain is not used.
342 * c may be 0 which works just like c->getmovpos==0. */
344 Segment *segment_interferer(Segment *base);
345 /* Returns the segment which may interfere with base, if any. */
347 ErrorCode segment_interferer_does(TrackAdvanceContext *c, Segment *base,
348 Segment *interferer, int *does_r);
349 /* Checks whether it interferes with base.
350 * Return is 0 or the error value from c->getmovpos.
351 * *does_r is set (only) on successful return,
352 * c is used as for trackloc_getlink. If c->getmovpos is 0
353 * the segments are taken to interfere if they would in any movposcomb.
354 * interferer may be 0 in which case succeeds, setting *does_r=0.
357 Segment *segment_interferes_simple(TrackAdvanceContext *c, Segment *base);
358 /* Combines segment_interferer and segment_interferer_does.
359 * c->getmovpos MUST NOT return errors; we abort if it does. */
361 int trackloc_advance(TrackLocation *t, TrackAdvanceContext *c);
362 /* Advances t by dist until distance becomes 0, some callback
363 * returns non-0, or sometimes 0 if we cannot continue and the
364 * relevant callback is missing or returned 0. If we exhausted
365 * c->distance then *t is the end of the counted distance, on the
366 * earlier of the two segments if there is a choice. Otherwise *t
367 * is the last location on the last sensible segment. (Ie, nextseg
368 * and getmovpos get a putative new segment and if they don't work
369 * trackloc_advance rewinds.) In any case c->distance is updated by
370 * the distance successfully traversed. */
372 int trackloc_reverse_exact(TrackLocation *t, TrackAdvanceContext *c);
373 int trackloc_set_exactinto(TrackLocation *t, TrackAdvanceContext *c,
374 Segment *seg, int backwards, int into);
375 /* c is used just as for trackloc_getlink.
376 * If we can't determine the movposcombs we call abort. */
378 #define TL_DIST_INF (INT_MAX/16)
380 /*========== useful macros and declarations ==========*/
382 /*---------- looping over trains and segments ----------*/
384 #define SEGMENT_ITERVARS(seg) \
387 const SegmentInfo *seg##i
389 #define TRAIN_ITERVARS(tra) \
393 #define FOR_SEGMENT(seg, initx, stepx) \
394 for (seg##n=0, seg=segments, seg##i=info_segments; \
395 seg##n < NUM_SEGMENTS; \
396 seg##n++, seg++, seg##i++, stepx)
398 #define FOR_TRAIN(tra, initx, stepx) \
399 for (tra##n=0, tra=trains, initx; \
401 tra##n++, tra++, stepx)
403 #define SEG_IV SEGMENT_ITERVARS(seg)
404 #define FOR_SEG FOR_SEGMENT(seg,(void)0,(void)0)
406 #define TRA_IV TRAIN_ITERVARS(tra)
407 #define FOR_TRA FOR_TRAIN(tra,(void)0,(void)0)
409 /*---------- calculations with fixed point speeds ----------*/
411 #define SPEED_CALC_TIME(speed,dist,round) ((dist) / ((speed)+1e-6))
412 #define SPEED_CALC_DIST(speed,time,round) ((time) * (speed))
414 /*---------- safety margin parameters ----------*/
416 #define MARGIN_NOSE 6 /*mm*/ /* physical spare at each end of train */
417 #define MARGIN_TAIL 6 /*mm*/ /* spare when in motion only */
418 #define MARGIN_SPEED 1.2 /*ratio*/
419 #define MARGIN_AUTOPOINTTIME 500 /*ms*/
420 #define UNMARGIN_ROUNDING 1e-4 /* mm/ms; for 1s, leads to max err of 100um */
421 #define MARGIN_TRAINLENGTH 1.05
423 #define MARGIN_STOPTIME 240 /*ms*/
424 #define MARGIN_POLARISETIME 120 /*ms*/
426 #define UNMARGIN_WATCHDOG_16 7 /* 16ms units */
427 #define UNMARGIN_WATCHDOG (UNMARGIN_WATCHDOG_16*16) /*ms*/
428 /* should be < MARGIN_POLARISETIME */
429 #define MARGIN_WATCHDOG 40