typedef union RetransmitUrgentNode RetransmitUrgentNode;
typedef unsigned Retransmit__Time;
-struct RetransmitRelaxedNode {
- PicInsn pi; /* callers must touch only this */
+struct RetransmitRelaxedNode { /* all for use by retransmit.c only */
+ PicInsn pi;
DLIST_NODE(RetransmitRelaxedNode) rr;
};
-union RetransmitUrgentNode {
- PicInsn pi; /* callers must touch only this */
+union RetransmitUrgentNode { /* all for use by retransmit.c only */
+ PicInsn pi;
struct {
RetransmitRelaxedNode relaxed;
int ix;
} u;
};
-void retransmit_relaxed_queue(RetransmitRelaxedNode *rn);
+void retransmit_relaxed_queue(RetransmitRelaxedNode *rn, const Nmra *n);
+void retransmit_relaxed_requeue(RetransmitRelaxedNode *rn, const Nmra *n);
void retransmit_relaxed_cancel(RetransmitRelaxedNode *rn);
-void retransmit_urgent_queue(RetransmitUrgentNode *rn);
+
+void retransmit_urgent_queue(RetransmitUrgentNode *rn, const Nmra *n);
+void retransmit_urgent_queue_relaxed(RetransmitUrgentNode *urg, const Nmra *n);
+void retransmit_urgent_requeue(RetransmitUrgentNode *rn, const Nmra *n);
void retransmit_urgent_cancel(RetransmitUrgentNode *rn);
+ /* ... NB: these are NOT idempotent. Use _requeue it's queued;
+ * _requeue is just _cancel followed by queue. */
+
/*---------- global variables, in realtime.c ----------*/
extern CommandInput cmdi;
elapsed++;
}
-static void retransmit_something() {
+static void retransmit_something(void) {
PerSpeedyTrans *spd;
RetransmitUrgentNode *urg;
RetransmitRelaxedNode *rlx;
spd++;
DLIST2_APPEND(spd->queue,urg,u.queue);
} else {
- urg->d.urgent_ix= -1;
+ urg->u.ix= -1;
}
retransmit_this(&urg->pi);
return;
serial_transmit(&linefill);
}
-void retransmit_relaxed_queue(RetransmitRelaxedNode *rlx) {
+void retransmit_relaxed_queue(RetransmitRelaxedNode *rlx, const Nmra *n) {
+ nmra_encodeforpic(n, &rlx->pi);
DLIST2_PREPEND(relaxed,rn,rr);
}
+void retransmit_relaxed_requeue(RetransmitRelaxedNode *rn, const Nmra *n) {
+ retransmit_relaxed_cancel(rlx);
+ retransmit_relaxed_queue(rlx, n);
+}
void retransmit_relaxed_cancel(RetransmitRelaxedNode *rlx) {
DLIST2_REMOVE(relaxed,rlx,rr);
}
-void retransmit_urgent_queue(RetransmitUrgentNode *urg) {
+void retransmit_urgent_queue(RetransmitUrgentNode *urg, const Nmra *n) {
+ nmra_encodeforpic(n, &urg->pi);
urg->u.ix= 0;
urg->u.when= elapsed;
DLIST2_APPEND(speedies[0].queue,urg,u.queue);
retransmit_relaxed_queue(&urg->u.relaxed);
}
+void retransmit_urgent_queue_relaxed(RetransmitUrgentNode *urg, const Nmra *n){
+ nmra_encodeforpic(n, &urg->pi);
+ urg->u.ix= -1;
+ urg->u.when= elapsed;
+ retransmit_relaxed_queue(&urg->u.relaxed);
+}
+void retransmit_urgent_requeue(RetransmitUrgentNode *rn, const Nmra *n) {
+ retransmit_urgent_cancel(urg);
+ retransmit_urgent_queue(urg, n);
+}
void retransmit_urgent_cancel(RetransmitUrgentNode *urg) {
if (urg->u.ix >= 0)
DLIST2_REMOVE(speedies[urg->u.ix].queue,urg,u.queue);
l.ec= 0;
l.invert_count[0]= l.invert_count[1]= 0;
- head= tra->backwards ? trai->tail : trai->head;
+ head= tra->backwards ? tra->tail : tra->head;
headslop= head + added_slop;
/* 1st pass:
trackloc_reverse(&tloc);
seg->tr_updated= 0; /* we're about to do this one again */
- tail= tra->backwards ? trai->head : trai->tail;
- taildet= trai->detectable + tail;
+ tail= tra->backwards ? tra->head : tra->tail;
+ taildet= tra->detectable + tail;
lay_train_pass(&l, tloc, taildet, 0, 1, 1);
return l.ec;
}
-static void setspeed(TrainNum tran, Speed newspeed) {
- /* does not lay the train, caller must do that (or have done it,
- * in which case they should already have set tra->speed). */
- State *s= &safety_state;
- TrainState *tra= &s->trains[tran];
-
- tra->speed= newspeed;
- actual_setspeed(tran);
- speedmanager_speedchange_notify(tran);
-}
-
void safety_notify_detection(SegmentNum segn) {
S; SEG;
TrainNum tran= seg->owner;
tra->foredetect= segn;
tra->maxinto= trackloc_remaininseg(&tloc);
-
- if (seg->cm_autostop) {
- if (tra->speed < AUTOSTOP_MAXSPEED &&
- tra->maxinto > AUTOSTOP_UNCERTAINTY)
- /* At some point we may need to allow more slop when the
- * next segment is points than when this is the last segment
- * (ie, just buffers next). */
- tra->maxinto= AUTOSTOP_UNCERTAINTY;
+ if (seg->cm_autostop) {
seg->cm_autostop= 0;
- setspeed(tran, 0);
+ if (!tra->estopping) {
+ speedmanager_autostop(tran);
+ if (!tra->speed && tra->maxinto > AUTOSTOP_UNCERTAINTY)
+ /* At some point we may need to allow more slop when the
+ * next segment is points than when this is the last segment
+ * (ie, just buffers next). */
+ tra->maxinto= AUTOSTOP_UNCERTAINTY;
+ }
}
tra->uncertainty= tra->maxinto;
-
- ec= lay_train(tran, 0);
- if (ec) {
- logmsg(ec, tran, segn, "emergency stop");
- safety_emergencystop(tran);
+
+ if (!tra->estopping) {
+ ec= lay_train(tran, 0);
+ if (ec) {
+ logmsg(ec, tran, segn, "emergency stop");
+ safety_emergencystop(tran);
+ }
}
}
S; TRA;
ErrorCode ec;
- tra->speed= 0;
- fixme /* this is wrong
- we need to take into account the finite stopping time or distance
- of the train
- and predict and track its location while it stops
- */;
+ if (tra->estopping) return;
- actual_emergencystop(tran);
ec= lay_train(tran, ESTOP_UNCERTAINTY);
if (ec) safety_panic(tran, NOTA(Segment), "emergency stop forbidden!");
- speedmanager_emergencystop_notify(tran);
+ speedmanager_emergencystop(tran);
}
-void safety_requestspeed(TrainNum tran, long newspeed) {
+ErrorCode safety_requestspeed(TrainNum tran, long newspeed) {
S; TRA;
long oldspeed;
ErrorCode ec;
} else {
logmsg(ec, tran, NOTA(Segment), "countermanded motion start");
}
- setspeed(tran, oldspeed);
oprintf(UPO, "countermanded %s %ld %ld\n",
info_trains[tran].pname, oldspeed, newspeed);
ec= lay_train(tran, 0);
" speed change insufficient!");
return;
}
-
- setspeed(tran, newspeed);
}
/*========== state of the layout ==========*/
typedef struct {
+ fixme init location;
+ fixme call init_fixed_train;
+ int addr;
+ Distance head, detectable, tail;
SegmentNum foredetect; /* train's detectable part is at most maxinto */
Distance maxinto, uncertainty; /* into foredetect but train may be */
unsigned /* uncertainty less far advanced */
- backwards:1; /* train is moving backwards wrt its own front and back */
- Speed speed;
- Speed speedtarget; fixme put this in some different array ?
- TimeoutEvent speedadjust; fixme initialise
+ backwards:1, /* train is moving backwards wrt its own front and back */
+ estopping:1; /* set and cleared by speed.c */
+ Speed speed; /* when accelerating/decelerating, is maximum at this moment */
+
+ struct {
+ const SpeedCurveEntry *curve;
+ int curvesz;
+ int target; /* index into curve */
+ int commanded; /* when ac-/decel, eq last value xmitted */
+ TimeoutEvent more;
+ RetransmitUrgentNode rn;
+ } accel;
} TrainState;
typedef struct {
void safety_emergencystop(TrainNum);
/* Callable directly in response to application command. */
-void safety_setdirection(TrainNum tran, int sense_fixme_define_this_properly);
-void safety_requestspeed(TrainNum tran, long newspeed);
+ErrorCode safety_requestspeed(TrainNum tran, long newspeed);
/* To be called only by the speed manager, thus indirectly from
- * user request.
- * Will result in a call to speedmanager_speedchange_notify (and of
- * course to actual_setspeed). Speed manager must apply accel/decel
- * curve so that if safety.c agrees the command, the actual speed of
- * the train changes straight away (at least for decel).
+ * user request. Any error will have been logged. On success,
+ * ->speed has been updated. Speed manager is responsible for
+ * calling actual_setspeed.
*/
+void safety_setdirection(TrainNum tran, int sense_fixme_define_this_properly);
+
void safety_notify_detection(SegmentNum segn);
/* Called by startup.c when new train detection occurs in state Run. */
/*========== speedmgr.c ==========*/
void speedmanager_speedchange_request(TrainNum tran, Speed speed);
+ /* Callable directly in response to application command. */
-void speedmanager_emergencystop_notify(TrainNum tran);
-void speedmanager_speedchange_notify(TrainNum tran);
- /* To be called only by safety.c, whenever speed is actually set.
- * New speed has already been recorded in State. */
+void speedmanager_emergencystop(TrainNum tran);
+void speedmanager_autostop(TrainNum tran);
+ /* These are responsible for calling actual_setspeed.
+ *
+ * After speedmanager_autostop, ->speed will have been updated to a
+ * new desired speed. If it is 0 the train was going slowly and has
+ * been instructed to stop right now.
+ */
/*========== actual.c ==========*/
/* actual.c should only be called from safety.c.
#include "realtime.h"
-static void adjust_next(TrainNum tran) {
- S; TRA;
- calculate next speed step in the appropriate direction
- call safety_setspeed
+typedef struct {
+ SpeedStep step;
+ Speed speed;
+ TimeInterval upwait, downwait; /* between this insn and next one */
+} SpeedCurveEntry;
+void reset_train(Train *tra) {
+ Nmra n;
+
+ tra->estopping= 0;
+ tra->speed= 0;
+ tra->accel.target= tra->accel.commanded= 0;
+ toev_init(&tra->accel.more);
+ enco_nmra_speed126(&n, tra->addr, 0, tra->backwards);
+ retransmit_urgent_queue_relaxed(&tra->accel.rn, &n);
+ ec= safety_requestspeed(tran, 0);
+ if (ec)
+ safety_panic(tra, 0, "position reset impossible!");
+}
+
+void init_fixed_train(Train *tra, const TrainInfo *trai) {
+ /* does not init location (ie foredetect, maxinto, uncertainty, backwards);
+ * at least backwards must already be valid */
+ tra->addr= trai->addr;
+ tra->head= trai->head;
+ tra->detectable= trai->detectable;
+ tra->tail= trai->tail;
+ tra->accel.curve= trai->curve;
+ tra->accel.curvesz= trai->curvesz;
+ reset_train(tra);
+}
+
+void xmit_speed(Train *tra) {
+ Nmra n;
+ enco_nmra_speed126(&n, tra->addr,
+ tra->curve[tra->accel.commanded].step,
+ tra->backwards);
+ retransmit_urgent_requeue(&tra->accel.rn, &n);
+}
+
+void speedmanager_autostop(TrainNum tran) {
+ if (tra->speed < AUTOSTOP_MAXSPEED) {
+ toev_stop(&tra->speedadjust);
+ tra->accel.commanded= tra->accel.target= tra->speed= 0;
+ xmit_speed(tra);
+ } else {
+ changereq_internal(tra, 0, 1);
+ }
+}
+
+static void estop_done(TimeoutEvent *toev) {
+ Train *tra= (void*)((char*)toev - offsetof(Train, accel.more));
+ retransmit_urgent_cancel(&tra->accel.rn);
+ reset_train(tra);
+}
+
+void speedmanager_emergencystop(Train *tra) {
+ Nmra n;
+ tra->estopping= 1;
+ toev_stop(&tra->speedadjust);
+ enco_nmra_estop(&n, tra->addr);
+ retransmit_urgent_requeue(&tra->accel.rn, &n);
+
+ toev_stop(&tra->accel.more);
+ tra->accel.more.callback= estop_done;
+ tra->accel.more.duration= ESTOP_DEADTIME;
+ toev_start(&tra->accel.more);
+}
+
+static void accel_more(TimeoutEvent *toev) {
+ Train *tra= (void*)((char*)toev - offsetof(Train, accel.more));
+ adjust_next(tra, 0);
+}
-speedmanager_speedchange_notify {
- fixme need to know whether this is a countermand or not
+static void adjust_next(Train *tra, int inautostop) {
+ int newold, xmit;
+ long newspeed;
- actually send speed to train
- (fixme: combine speedmanager and actual?)
- queue an adjust_next
+ if (tra->accel.target > tra->accel.commanded) {
+ tra->accel.commanded++;
+ tra->accel.more.duration= tra->curve[tra->accel.commanded].upwait;
+ newspeed= tra->curve[tra->accel.commanded].speed;
+ } else if (tra->accel.target < tra->accel.commanded) {
+ newspeed= tra->curve[tra->accel.commanded].speed;
+ tra->accel.commanded--;
+ tra->accel.more.duration= tra->curve[tra->accel.commanded].downwait;
+ } else {
+ return;
+ }
+
+ if (!inautostop) {
+ 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;
+ }
+
+ toev_stop(&tra->accel.more);
+ tra->accel.more.callback= accel_more;
+ toev_start(&tra->accel.more);
+ xmit_speed(tra);
+}
+
+int changereq_internal(Train *tra, int newcommanded, int inautostop) {
+ int reverse, newold;
+
+ if (!speedadjust->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;
- make speedmanager also responsible for doing actuals ?
- pass speed step number through ?
+ if (reverse) {
+ /* switch from accel to decel or vice versa */
+ toev_stop(&tra->speedadjust);
+ tra->accel.commanded= newold;
+ adjust_next(tra, 0);
+ }
+ }
+}
void speedmanager_speedchange_request(TrainNum tran, Speed speed) {
- S; TRA; TRAI;
+ changereq_internal(tra, speed, 0);
+ int a, b, found;
+ const SpeedCurveEntry *curve;
- if (speed > trai->maxspeed) {
+ if (tra->estopping) {
+ logmsg(EC_Invalid, tra, 0, "speed request ignored during emergency stop");
+ return;
+ }
+
+ for (a=0, b=trai->nsteps;
+ 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 && !NOTA(speed))
logmsg(EC_Invalid, tran, NOTA(SegmentNum),
"requested speed %l excessive; capping at %l",
- (long)speed, (long)trai->maxspeed);
- speed= trai->maxspeed;
- }
- tra->targetspeed= speed;
- if (!speedadjust->running)
- adjust_next(tran);
+ (long)speed, (long)curve[found].speed);
+
+ changereq_internal(tra, found, 0);
}
~ijackson / trains.git / commitdiff