+convert .stopping to appropriate .speeds.record
+
either only POLARITY when we've had POLARISED or delete
that bit from the docs
-
-
-with santafe at X8, we get this:
-
- picio out polarity <>
- lay_train X8 @santafe added_slop=0 maxinto=509 tra->backwards=0 tloc.backwards=1
- lay_train_pass santafe @X8+509 check_clash=1 advance=20 speed=0 remain=20 backwards=0
- lay_train_pass loop @X8 remain=20
- lay_train_pass further=1 tloc=@X6+0 remain=20
- lay_train_pass loop @X6 remain=20
- lay_train_pass further=0 tloc=@X6+20 remain=0
- lay_train_pass santafe @X8+509 check_clash=0 advance=0 speed=0 remain=0 backwards=0
- lay_train_pass loop @X8 remain=0
- lay_train_pass santafe @X8+0 check_clash=1 advance=605 speed=0 remain=605 backwards=1
- lay_train_pass loop @X8 remain=605
- lay_train_pass further=1 tloc=@X10+0 remain=96
- lay_train_pass loop @X10 remain=96
- message safety @X10 santafe: collision with shinkansen
- lay_train_pass further=0 tloc=@X10+96 remain=0
- message safety @X8 santafe: emergency stop on reaching here
- lay_train X8 @santafe added_slop=300 maxinto=509 tra->backwards=0 tloc.backwards=1
- lay_train_pass santafe @X8+509 check_clash=1 advance=320 speed=0 remain=320 backwards=0
- lay_train_pass loop @X8 remain=320
- lay_train_pass further=1 tloc=@X6+0 remain=320
- lay_train_pass loop @X6 remain=320
- lay_train_pass further=1 tloc=@X5+0 remain=68
- lay_train_pass loop @X5 remain=68
- lay_train_pass further=0 tloc=@X5+68 remain=0
- lay_train_pass santafe @X8+509 check_clash=0 advance=0 speed=0 remain=0 backwards=0
- lay_train_pass loop @X8 remain=0
- lay_train_pass santafe @X8+0 check_clash=1 advance=605 speed=0 remain=605 backwards=1
- lay_train_pass loop @X8 remain=605
- lay_train_pass further=1 tloc=@X10+0 remain=96
- lay_train_pass loop @X10 remain=96
- lay_train_pass further=0 tloc=@X10+96 remain=0
- picio out polarity <>
-
-but why does emergency stop help ?
-
-
-
-
-
-
-
-dunno but maybe before can test
+optional but maybe before can test:
wiring to gui display
-things not yet considered at all in safety code
+things not yet considered at all in safety code:
min. curve specifications
+make it possible to reverse a train ?
+
would be nice to do something with POINTED
+ does it even exist
at { STR AT; }
has { STR HAS; }
step { STR STEP; }
+stops { STR STOPS; }
home { STR HOME; }
end { STR END; }
/* new keywords must be added to %token<name> and ident: in record-y.y */
double dbl;
}
-%token <name> TRAIN FEATURE SEG IS AT HAS STEP HOME END IDENT FEATLETTER
+%token <name> TRAIN FEATURE SEG IS AT HAS STEP STOPS HOME END
+%token <name> IDENT FEATLETTER
%token <name> NL
%token <num> NUM
%token <dbl> DBL
| TRAIN train HOME { cur_train=$2; } segments
{
}
- | TRAIN train STEP NUM '=' dbl NUM '/' NUM
- { if (!trains) record_train_step_count();
- else if ($2) record_train_step($2,$4,$6,$7,$9);
+ | TRAIN train STEP NUM '=' dbl
+ { if ($2) record_train_step_speed($2,$4,$6);
+ }
+ | TRAIN train STOPS dbl NUM NUM
+ { if (!trains) record_train_stopregime_count();
+ else if ($2) record_train_stopregime($2,$4,$5,$6);
}
| SEG seg HAS backwards train
{ if ($2 && $5) record_seg_has($2,$4,$5);
segments: { cur_train= (void*)-1; }
| backwards seg { record_train_home(cur_train,$1,$2); } segments
-ident: TRAIN | SEG | IS | AT | HAS | STEP | HOME | END
+ident: TRAIN | SEG | IS | AT | HAS | STEP | STOPS | HOME | END
| IDENT | FEATLETTER
dbl: DBL
| NUM { $$= $1 }
* max-trains num
* train <trainpn> at [-]<foredetectpn>:<maxinto>+-<uncertainty>
* train <trainpn> is <addr> <head>+<detectable>+<tail>
- * train <trainpn> step <step>=<speed> <upwait>/<downwait>
+ * train <trainpn> step <step>=<speed>
+ * train <trainpn> step <step> at <distance> after <milliseconds>
* train <trainpn> home [-]<segpn> [-]<segpn> [-]<segpn>
* seg <segpn> has [-]<ownerpn>
* seg <segpn> at <movposcomb>
seg->ho_backwards= backw;
}
+void record_train_step_speed(Train *tra, int step, double speed) {
+ if (step<1 || step>SPEEDSTEPS)
+ record_yyerror("speed step out of range 0..127");
+ tra->speedcurve[step]= speed;
+}
+
void record_seg_has(Segment *seg, int backw, Train *tra) {
seg->owner= tra;
seg->tr_backwards= backw;
/*---------- speed curves ----------*/
-static SpeedCurveEntry *curvebuf;
-static int curvebufsz, curvebufused;
+static SpeedRange *rangebuf;
+static int rangebufsz, rangebufused;
-void record_train_step_count(void) {
- curvebufsz++;
+void record_train_stopregime_count(void) {
+ rangebufsz++;
}
-void record_train_step(Train *tra, int step, double speed,
- int upw, int downw) {
+void record_train_stopregime(Train *tra, double from, int xs, int ts) {
Train *other;
- SpeedCurveEntry *new;
+ SpeedRangeEntry *new;
int i;
-
- if (curvebufused >= curvebufsz)
+
+ if (rangebufused >= rangebufsz)
record_yyerror("more speed points on 2nd pass!");
- if (!tra->accel.curve) {
- tra->accel.curve= curvebuf + curvebufused;
+ if (!tra->speedregimes) {
+ tra->speedregimes= rangebuf + rangebufused;
}
/* Make room for an extra speed step point for this train,
* by moving all of the other trains up. First move the data: */
- memmove(tra->accel.curve + tra->accel.curvesz + 1,
- tra->accel.curve + tra->accel.curvesz,
- (char*)(curvebuf + curvebufused)
- - (char*)(tra->accel.curve + tra->accel.curvesz));
+ memmove(tra->speedregimes + tra->n_speedregimes + 1,
+ tra->speedregimes + tra->n_speedregimes,
+ (char*)(rangebuf + rangebufused)
+ - (char*)(tra->speedregimes + tra->n_speedregimes));
/* Then adjust everyone's pointers: */
for (i=0, other=trains;
i<n_trains;
i++, other++)
- if (other != tra && other->accel.curve &&
- other->accel.curve >= tra->accel.curve)
- other->accel.curve++;
- /* ... that whole thing is O(n^2) if the speed points aren't sorted
- * by train; we hope they are, in which case we are always adding
- * to the last train in the list and there is then no data to copy,
- * which makes it O(n*t) where n is the number of speed points and
- * t is the number of trains. If we cared we could optimise away
- * the loop in the common case where this train is right at the end
- * but a special case seems like asking for a bug when we do have
- * out of order speed points. */
-
- new= &tra->accel.curve[tra->accel.curvesz++];
- curvebufused++;
-
- new->step= step;
- new->speed= speed;
- new->upwait= upw;
- new->downwait= downw;
+ if (other != tra && other->speedregimes &&
+ other->speedregimes >= tra->speedregimes)
+ other->speedregimes++;
+ /* ... that whole thing is O(n^2) if the regimes aren't sorted by
+ * train; we hope they are, in which case we are always adding to
+ * the last train in the list and there is then no data to copy,
+ * which makes it O(n*t) where n is the number of regimes and t is
+ * the number of trains. If we cared we could optimise away the
+ * loop in the common case where this train is right at the end but
+ * a special case seems like asking for a bug when we do have out of
+ * order speed points. */
+
+ new= &tra->speedregimes[tra->n_speedregimes++];
+ rangebufused++;
+
+ new->speed= from;
+ new->xs= xs;
+ new->ts= ts;
}
-static int speedcurveentry_compare(const void *av, const void *bv) {
- const SpeedCurveEntry *a= av, *b= bv;
+static int speedregime_compare(const void *av, const void *bv) {
+ const SpeedRange *a= av, *b= bv;
if (a->step == b->step)
record_yyerror("multiple speed curve points at same step");
return a->step - b->step;
}
-static void sort_curves(void) {
+static void speeds_postprocess(void) {
TRA_IV;
FOR_TRA {
- if (tra->accel.curve) {
- if (tra->accel.curvesz < 2)
- die("config: speed curve too short for %s", tra->pname);
- qsort(tra->accel.curve, tra->accel.curvesz,
- sizeof(*tra->accel.curve), speedcurveentry_compare);
- if (tra->accel.curve[0].step || tra->accel.curve[0].speed)
- die("config: speed curve missing zero point for %s", tra->pname);
+ if (!tra->speedregimes || tra->speedcurve[1]<0) {
+ tra->addr= -1;
+ continue;
}
+ if (tra->n_speedregimes < 1)
+ die("config: speed curve too short for %s", tra->pname);
+
+ qsort(tra->speedregimes, tra->n_speedregimes,
+ sizeof(*tra->speedregimes), speedregime_compare);
+ if (tra->speedregimes[tra->n_speedregimes-1].speed
+ <= tra->speedcurve[SPEEDSTEPS-1])
+ die("config: speed curve missing top point for %s", tra->pname);
+
+ for (step=1; step<=SPEEDSTEPS; step++)
+ if (tra->speedcurve[step] < 0)
+ die("config: speed curve for %s missing step %d", tra->pname, step);
+ tra->speedcurve[0]= 0;
}
}
tra->addr= -1;
tra->foredetect= 0;
tra->backwards= 0;
+ for (step=0; step<=SPEEDSTEPS; step++)
+ tra->speedcurve[step]= -1;
}
}
alloc();
parse_pass(argv); /* trains!=0: populates data area */
record_tempzone_clear();
- sort_curves();
+ speeds_postprocess();
}
/*---------- prediction entrypoint ----------*/
-static ErrorCode predict_confirm(Train *tra, int accelerate,
- PredictionProblemCallback *ppc, void *ppcu) {
+ErrorCode predict_confirm(Train *tra, int accelerate,
+ PredictionProblemCallback *ppc, void *ppcu) {
/* Caller must call this with different situations until it succeeds! */
PredictUserContext u;
Segment *foredet;
/* find the train's nose */
u.nose= fdet;
- u.nosec.distance= speedmanager_nosesafetmargin(u->train)
- + (tra->backwards ? tra->tail : tra->head);
+ u.nosec.distance= MARGIN_NOSE + (tra->backwards ? tra->tail : tra->head);
u.nosec.nextseg= nose_nextseg;
u.nosec.getmovpos= pred_getmovpos;
u.nosec.trackend= pred_trackend;
assert(ec == EC_SignallingProblemPredicted);
tra->sigstopping= 1;
- predict_confirm(tra, 0, 0,"detection");
+ ec= speedmanager_speedchange_request(tra,0, 0,"detection sigstop");
+ /* that calls predict_confirm with our supplied arguments */
assert(!ec);
}
}
return 0;
}
-
- /* ============ OLD CODE ========== */
-
-ErrorCode safety_requestspeed(Train *tra, long newspeed) {
- long oldspeed;
- ErrorCode ec;
-
- oldspeed= tra->speed;
- tra->speed= newspeed;
-
- ec= lay_train(tra, 0);
-
- if (ec) {
- ErrorCode revert_ec;
-
- if (oldspeed && oldspeed < newspeed) {
- logmsg(ec,tra,0, "countermanded acceleration"
- " from %ld to %ld", oldspeed, newspeed);
- } else if (oldspeed) {
- safety_panic(tra,0, "deceleration forbidden!"
- " (from %ld to %ld", oldspeed, newspeed);
- } else {
- logmsg(ec,tra,0, "countermanded motion start");
- }
- oprintf(UPO, "countermanded %s %ld %ld\n",
- tra->pname, oldspeed, newspeed);
- revert_ec= lay_train(tra, 0);
- if (revert_ec)
- safety_panic(tra,0, "countermanding"
- " speed change insufficient!");
- }
- return ec;
-}
typedef struct MovPosChange MovPosChange;
+#define SPEEDSTEPS 126
+
typedef struct {
- int step;
- Speed speed;
- TimeInterval upwait, downwait; /* between this insn and next one */
-} SpeedCurveEntry;
+ float speed;
+ Distance xs;
+ TimeInterval ts;
+} SpeedRange;
struct Train {
/* Configuration (excluding speed curve): */
Distance head, detectable, tail;
/* Location: */
- struct Segment *foredetect; /* train's detectable part is at most maxinto */
- // Distance maxinto, uncertainty; /* into foredetect but train may be */
- unsigned /* uncertainty less far advanced */
+ struct Segment *foredetect;
+ unsigned
backwards:1, /* train is moving backwards wrt its own front and back */
/* Speed: */
sigstopping:1, /* set and cleared by speed.c */
/* Startup resolution (resolve.c): */
- resolution:2;
- Speed speed; /* when accelerating/decelerating, is maximum at this moment */
+ resolution:2; /* for use by speedmanager */
struct {
- int target; /* index into curve */
- int commanded; /* when ac-/decel, eq last value xmitted */
- TimeoutEvent more;
+ int step; /* commanded */
+ TimeoutEvent decel; /* running iff decelerating */
+ double speed; /* iff decerating, at last change; otherwise accurate */
+ double try_speed; /* during speedchange_request only, otherwise < 0 */
RetransmitUrgentNode rn;
+ } speed;
- /* Configuration for acceleration: */
- SpeedCurveEntry *curve;
- int curvesz;
- } accel;
+ int n_speedregimes;
+ SpeedRange *speedregimes;
+ float speedcurve[SPEEDSTEPS];
};
struct Segment {
/*---------- safety margin parameters ----------*/
+#define MARGIN_NOSE 6
+#define MARGIN_SPEED 1.2
+
+#if 0
#define CLEAR_FORESIGHT_TIME 500 /*ms*/
#define AUTOSTOP_MAXSPEED 0.050 /*m/s*/
#define AUTOSTOP_UNCERTAINTY 20 /*mm*/
#define TIMEINTERVAL2MS(ti) (ti)
#define SPEED_CLEAR_MULT SPEED_CALC_DIST(1,CLEAR_FORESIGHT_TIME,UP)
+#endif
#endif /*SAFETY_H*/
#include "realtime.h"
#include "nmra.h"
-static void changereq_internal(Train *tra, int newcommanded, int inautostop);
-static void accel_more(TimeoutEvent *toev);
-
-void speedmanager_reset_train(Train *tra) {
+static void xmit(Train *tra) {
Nmra n;
-
- tra->estopping= 0;
- tra->speed= 0;
- tra->accel.target= tra->accel.commanded= 0;
- if (tra->addr < 0)
- return;
-
- toev_init(&tra->accel.more);
- enco_nmra_speed126(&n, tra->addr, 0, tra->backwards);
- retransmit_urgent_queue_relaxed(&tra->accel.rn, &n);
+ enco_nmra_speed126(&n, tra->addr, tra->step, tra->backwards);
+ retransmit_urgent_requeue(&tra->speed.rn, &n);
}
-static void xmit_speed(Train *tra) {
- Nmra n;
- enco_nmra_speed126(&n, tra->addr,
- tra->accel.curve[tra->accel.commanded].step,
- tra->backwards);
- retransmit_urgent_requeue(&tra->accel.rn, &n);
+static void decel_done(TimeoutEvent *toev) {
+ Train *tra= (void*)((char*)toev - offsetof(Train, speed.decel));
+ tra->speed.speed= tra->speedcurve[tra->speed.step];
}
-void speedmanager_autostop(Train *tra) {
- if (tra->speed < AUTOSTOP_MAXSPEED) {
- toev_stop(&tra->accel.more);
- tra->accel.commanded= tra->accel.target= tra->speed= 0;
- xmit_speed(tra);
- } else {
- changereq_internal(tra, 0, 1);
- }
+static const SpeedRange *stop_info(Train *tra, double speed) {
+ for (i=0; i<tra->n_speedregimes; i++)
+ if (speed <= speedregimes[i].speed)
+ return &speedregimes[i];
+ abort();
}
-static void estop_done(TimeoutEvent *toev) {
- Train *tra= (void*)((char*)toev - offsetof(Train, accel.more));
- ErrorCode ec;
- retransmit_urgent_cancel(&tra->accel.rn);
- speedmanager_reset_train(tra);
- ec= safety_requestspeed(tra, 0);
- if (ec)
- safety_panic(tra, 0, "position reset impossible!");
-}
-
-void speedmanager_emergencystop(Train *tra) {
- Nmra n;
- tra->estopping= 1;
- toev_stop(&tra->accel.more);
- enco_nmra_speed126(&n, tra->addr, 0, tra->backwards);
- retransmit_urgent_requeue(&tra->accel.rn, &n);
+static double current_speed(Train *tra, const struct timeval tnow) {
+ double v1, v2, elapsed;
+ double v1sq, v2sq, vtsq;
+
+ if (tra->speed.try_speed >= 0) return tra->speed.try_speed;
+ if (!tra->speed.decel.running) return tra->speed.speed;
- toev_stop(&tra->accel.more);
- tra->accel.more.callback= estop_done;
- tra->accel.more.duration= TIMEINTERVAL2MS(ESTOP_DEADTIME);
- toev_start(&tra->accel.more);
-}
+ left_to_go= (tra->speed.decel.abs.tv_sec - tnow.tv_sec) * 1000.0 +
+ (tra->speed.decel.abs.tv_usec - tnow.tv_usec) * 0.001;
-static void adjust_next(Train *tra, int inautostop) {
- long newspeed;
-
- if (tra->accel.target > tra->accel.commanded) {
- tra->accel.commanded++;
- tra->accel.more.duration=
- TIMEINTERVAL2MS(tra->accel.curve[tra->accel.commanded].upwait);
- newspeed= tra->accel.curve[tra->accel.commanded].speed;
- } else if (tra->accel.target < tra->accel.commanded) {
- newspeed= tra->accel.curve[tra->accel.commanded].speed;
- tra->accel.commanded--;
- tra->accel.more.duration=
- TIMEINTERVAL2MS(tra->accel.curve[tra->accel.commanded].downwait);
- } else {
- return;
+ if (left_to_go <= 0) {
+ toev_stop(&tra->speed.decel);
+ return tra->speed.speed= tra->speedcurve[tra->speed.step];
}
- if (!inautostop) {
- ErrorCode ec= safety_requestspeed(tra, newspeed);
- if (ec) {
- assert(newspeed > tra->speed);
- assert(tra->accel.target >= tra->accel.commanded);
- tra->accel.target= --tra->accel.commanded;
- toev_stop(&tra->accel.more);
- return;
- }
- } else {
- tra->speed= newspeed;
- }
+ v2= tra->speed;
+ v1= tra->speedcurve[tra->speed.step];
+ assert(v2 >= v1);
- toev_stop(&tra->accel.more);
- tra->accel.more.callback= accel_more;
- toev_start(&tra->accel.more);
- xmit_speed(tra);
+ ts_v2= stop_info(v2)->ts;
+ v1sq= v1*v1;
+ v2sq= v2*v2;
+ vtsq= v1sq + (v2sq-v1sq) * left_to_go / ts_v2;
+ return sqrt(vtsq);
}
-static void accel_more(TimeoutEvent *toev) {
- Train *tra= (void*)((char*)toev - offsetof(Train, accel.more));
- adjust_next(tra, 0);
+static double speedmanager_speed_maxestimate(Train *tra) {
+ struct timeval tnow;
+ mgettimeofday(&tnow);
+ return current_speed(tra,tnow) * MARGIN_SPEED;
}
-static void changereq_internal(Train *tra, int newcommanded, int inautostop) {
- int reverse, newold;
-
- if (!tra->accel.more.running) {
- assert(tra->accel.commanded == tra->accel.target);
- adjust_next(tra, 0);
- } else {
- if (tra->accel.target > tra->accel.commanded) {
- newold= tra->accel.commanded+1;
- reverse= newcommanded < newold;
- } else if (tra->accel.target < tra->accel.commanded) {
- newold= tra->accel.commanded-1;
- reverse= newcommanded > newold;
- } else {
- abort();
- }
- tra->accel.target= newcommanded;
-
- if (reverse) {
- /* switch from accel to decel or vice versa */
- toev_stop(&tra->accel.more);
- tra->accel.commanded= newold;
- adjust_next(tra, 0);
- }
- }
+static double speedmanager_stoppingdistance(Train *tra) {
+ struct timeval tnow;
+ double vt;
+ const SpeedRange *regime;
+
+ mgettimeofday(&tnow);
+ v= current_speed(tra,tnow);
+
+ if (v==0) return 0;
+
+ regime= stop_info(tra,vt);
+
+ xs= tra->speedregimes[tra->n_speedregimes-1].xs;
+
+ v_ts_vcrit= v * regime->ts; if (xs > v_ts_vcrit) xs= v_ts_vcrit;
+ xs_vcrit= regime->xs; if (xs > xs_vcrit) xs= xs_vcrit;
+
+ return xs;
}
-void speedmanager_speedchange_request(Train *tra, long speed) {
- changereq_internal(tra, speed, 0);
- int a, b, try, found;
- const SpeedCurveEntry *curve= tra->accel.curve;
+ErrorCode speedmanager_speedchange_request(Train *tra, int step,
+ PredictionProblemCallback *ppc, void *ppcu) {
+ ErrorCode ec, ec2;
+ struct timeval tnow;
+ double vnow, vtarg;
- if (tra->addr < 0) {
- logmsg(EC_Invalid, tra, 0, "speed request ignored, train address unknown");
- return;
+ if (step == tra->speed.step)
+ return 0;
+
+ mgettimeofday(&tnow);
+ vnow= current_speed(tra,tnow);
+ vtarg= tra->speedcurve[step];
+
+ if (vtarg <= vnow) {
+ toev_stop(&tra->speed.decel);
+ tra->speed.step= step;
+ tra->speed.decel.duration= stop_info(tra,vnow)->ts;
+ toev_start(&tra->speed.decel);
+ tra->speed.speed= vnow;
+ xmit(tra);
+ return 0;
}
- if (tra->estopping) {
- logmsg(EC_Invalid, tra, 0, "speed request ignored during emergency stop");
- return;
+ tra->speed.try_speed= vtarg;
+ ec= predict_confirm(tra,1, ppc,ppcu);
+
+ if (ec) {
+ tra->speed.try_speed= -1;
+ ec2= predict_confirm(tra,1, 0,"abandoned acceleration");
+ assert(!ec2);
+ return ec;
}
+
+ tra->step= step;
+ toev_stop(&tra->speed.decel);
+ tra->speed= vtarg;
+ tra->try_speed= -1;
+ xmit(tra);
+ return 0;
+}
+
+void speedmanager_reset_train(Train *tra) {
+ Nmra n;
- for (a=0, b=tra->accel.curvesz;
- a < b;
- ) {
- try= (a + b) >> 2;
- if (curve[try].speed > speed) b= try; else a= try+1;
- }
- /* Loop postconditions:
- * a==b
- * either a=try+1 never executed ie all curve[].speed > speed
- * hence a=0
- * or curve[a-1].speed <= speed
- * either b=try never executed ie all curve[].speed <= speed
- * hence b=trai->nsteps
- * or curve[b].speed > speed
- */
- found= a>1 ? a-1 : speed ? 1 : 0;
-
- if (curve[found].speed < speed && speed != INT_MAX)
- logmsg(EC_Invalid, tra,0,
- "requested speed %ld excessive; capping at %f",
- speed, curve[found].speed);
-
- changereq_internal(tra, found, 0);
+ tra->sigstopping= 0;
+ tra->speed.step= 0;
+ toev_init(&tra->speed.decel);
+ tra->speed.decel.callback= decel_done;
+ tra->speed.speed= 0;
+ tra->speed.try_speed= -1;
+ tra->decelerating= 0;
+ if (tra->addr < 0)
+ return;
+
+ enco_nmra_speed126(&n, tra->addr, 0, tra->backwards);
+ retransmit_urgent_queue_relaxed(&tra->speed.rn, &n);
}
~ijackson / trains.git / commitdiff