9 void actual_speed(Train *tra, int step) {
11 enco_nmra_speed126(&n, tra->addr, step, tra->backwards);
12 ouprintf("train %s speed commanding %d\n", tra->pname, step);
13 retransmit_urgent_requeue(&tra->speed.rn, &n);
16 static void xmit(Train *tra) {
17 actual_speed(tra,tra->speed.step);
20 static void decel_done(TimeoutEvent *toev) {
21 Train *tra= (void*)((char*)toev - offsetof(Train, speed.decel));
25 tra->speed.speed= tra->speedcurve[tra->speed.step];
27 DPRINTF(speed,core, "decel_done %s step %d speed %f\n",
28 tra->pname, tra->speed.step, tra->speed.speed);
31 ec= predict(tra,tnow, PREDF_OLDPLAN, 0,0, 0, 0,(char*)"deceleration done");
35 static const SpeedRange *stop_info(Train *tra, double speed) {
37 for (i=0; i<tra->n_speedregimes; i++)
38 if (speed <= tra->speedregimes[i].speed + UNMARGIN_ROUNDING)
39 return &tra->speedregimes[i];
43 static double current_speed(Train *tra, const struct timeval tnow) {
45 double v1sq, v2sq, vtsq;
46 double left_to_go, ts_v2, cs;
48 DPRINTF(speed,query, "current? %s decel.running=%d speed=%f"
49 " step=%d\n", tra->pname,
50 tra->speed.decel.running, tra->speed.speed, tra->speed.step);
52 if (!tra->speed.decel.running) return tra->speed.speed;
54 left_to_go= (tra->speed.decel.abs.tv_sec - tnow.tv_sec) * 1000.0 +
55 (tra->speed.decel.abs.tv_usec - tnow.tv_usec) * 0.001;
57 if (left_to_go <= 0) {
58 DPRINTF(speed,query, "current? decel-done\n");
59 /* we don't cancel the timeout because we need the callback to lay out
60 * the train again at the lower speed */
61 return tra->speed.speed;
65 v1= tra->speedcurve[tra->speed.step];
68 ts_v2= stop_info(tra,v2)->ts;
71 vtsq= v1sq + (v2sq-v1sq) * left_to_go / ts_v2;
73 DPRINTF(speed,query, "current? decel-inprogress v2=%f v1=%f"
74 " ts_v2=%f ltg=%f returning %f\n",
75 v2,v1, ts_v2, left_to_go, cs);
79 static double stoppingdistance(Train *tra, struct timeval tnow,
80 const SpeedRange *regime, double v) {
85 xs= tra->speedregimes[tra->n_speedregimes-1].xs;
87 double v_ts_vcrit= v * regime->ts; if (xs > v_ts_vcrit) xs= v_ts_vcrit;
88 double xs_vcrit= regime->xs; if (xs > xs_vcrit) xs= xs_vcrit;
90 return xs + v * MARGIN_STOPTIME;
93 void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo *ir) {
94 double vnow= current_speed(tra,tnow);
96 ir->max_speed_estimate= vnow * MARGIN_SPEED;
97 ir->stopping_distance= stoppingdistance(tra,tnow, stop_info(tra,vnow), vnow);
98 ir->stopping= !tra->speed.step;
100 DPRINTF(speed,query, "getinfo? %s max=%f stopdist=%d %c\n", tra->pname,
101 ir->max_speed_estimate, ir->stopping_distance,
105 int speedmanager_stopped(Train *tra) {
106 return tra->speed.speed == 0;
109 static ErrorCode request_core(Train *tra, int step, struct timeval tnow,
110 unsigned predictflags,
111 PredictionProblemCallback *ppc, void *ppcu) {
112 /* We ignore the value of PREDF_OLDPLAN in predictflags, as we can tell
113 * whether the old plan is good by the change in speed */
115 double vnow, vtarg, vmax;
117 const SpeedRange *regime;
120 DPRINTF(speed,core, "request %s%s step %d\n",
121 tra->backwards?"-":"",tra->pname,
124 if (step == tra->speed.step)
127 vnow= current_speed(tra,tnow);
128 vtarg= tra->speedcurve[step];
129 oldmaxinto= tra->maxinto;
131 DPRINTF(speed,core, " request vnow=%f vtarg=%f\n", vnow,vtarg);
133 vmax= MAX(vtarg,vnow);
134 try.max_speed_estimate= vmax * MARGIN_SPEED;
135 regime= stop_info(tra,vmax);
136 try.stopping_distance= stoppingdistance(tra,tnow, regime, vmax);
140 Segment *fdet= tra->foredetect;
141 tra->maxinto= fdet->i->poscombs[fdet->movposcomb].dist;
142 assert(tra->maxinto >= oldmaxinto);
143 tra->uncertainty += tra->maxinto - oldmaxinto;
146 predictflags &= ~PREDF_OLDPLAN;
148 ec= predict(tra,tnow,
149 predictflags | (step <= tra->speed.step ? PREDF_OLDPLAN : 0),
150 0,0, &try, ppc,ppcu);
152 tra->uncertainty -= tra->maxinto - oldmaxinto;
153 tra->maxinto= oldmaxinto;
154 ec2= predict(tra,tnow, predictflags | PREDF_OLDPLAN, 0,0, 0,
155 0,(char*)"abandoned acceleration");
160 toev_stop(&tra->speed.decel);
161 tra->speed.step= step;
162 tra->speed.speed= vmax;
165 tra->speed.decel.duration= regime->ts;
166 toev_start(&tra->speed.decel);
168 report_train_position(tra);
173 ErrorCode speedmanager_speedchange_request(Train *tra, int step,
174 PredictionProblemCallback *ppc, void *ppcu) {
178 MUSTECR( safety_checktrain(tra,ppc,ppcu) );
179 mgettimeofday(&tnow);
180 return request_core(tra,step, tnow,0, ppc,ppcu);
183 void speedmanager_safety_stop(Train *tra, struct timeval tnow,
184 unsigned predictflags) {
186 ec= request_core(tra,0,tnow,predictflags, 0,(char*)"emergency stop");
190 void speedmanager_reset_train(Train *tra) {
192 toev_init(&tra->speed.decel);
193 tra->speed.decel.callback= decel_done;
194 tra->speed.decel.pclass= "decel";
195 tra->speed.decel.pinst= tra->pname;
200 actual_speed(tra, 0);
203 void speedmanager_init(void) {
208 retransmit_urgent_queue_relaxed(&tra->speed.rn, &n);