From: ian Date: Sun, 3 Sep 2006 16:56:13 +0000 (+0000) Subject: speed manager and interaction with safety improved; new data structure arrangements... X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?a=commitdiff_plain;h=7c3571474daa88ef3bc757cfec3d5786f96f41db;p=trains.git speed manager and interaction with safety improved; new data structure arrangements (mmap thing) not yet started --- diff --git a/hostside/realtime.h b/hostside/realtime.h index 8738277..c18118d 100644 --- a/hostside/realtime.h +++ b/hostside/realtime.h @@ -25,12 +25,12 @@ typedef struct RetransmitRelaxedNode RetransmitRelaxedNode; 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; @@ -39,11 +39,18 @@ union RetransmitUrgentNode { } 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; diff --git a/hostside/retransmit.c b/hostside/retransmit.c index 7cb3836..ce6a0da 100644 --- a/hostside/retransmit.c +++ b/hostside/retransmit.c @@ -54,7 +54,7 @@ static void retransmit_this(const PicInsn *pi) { elapsed++; } -static void retransmit_something() { +static void retransmit_something(void) { PerSpeedyTrans *spd; RetransmitUrgentNode *urg; RetransmitRelaxedNode *rlx; @@ -74,7 +74,7 @@ static void retransmit_something() { spd++; DLIST2_APPEND(spd->queue,urg,u.queue); } else { - urg->d.urgent_ix= -1; + urg->u.ix= -1; } retransmit_this(&urg->pi); return; @@ -91,19 +91,35 @@ static void retransmit_something() { 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); diff --git a/hostside/safety.c b/hostside/safety.c index d8c5802..3e069ac 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -166,7 +166,7 @@ static ErrorCode lay_train(TrainNum tran, long added_slop) { 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: @@ -187,8 +187,8 @@ static ErrorCode lay_train(TrainNum tran, long added_slop) { 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); @@ -198,17 +198,6 @@ static ErrorCode lay_train(TrainNum tran, long added_slop) { 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; @@ -228,24 +217,26 @@ void safety_notify_detection(SegmentNum segn) { 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); + } } } @@ -253,20 +244,14 @@ void safety_emergencystop(TrainNum 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; @@ -286,7 +271,6 @@ void safety_requestspeed(TrainNum tran, long newspeed) { } 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); @@ -295,6 +279,4 @@ void safety_requestspeed(TrainNum tran, long newspeed) { " speed change insufficient!"); return; } - - setspeed(tran, newspeed); } diff --git a/hostside/safety.h b/hostside/safety.h index 770b614..b1c88fa 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -27,13 +27,25 @@ typedef short TimeInterval; /*ms*/ /*========== 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 { @@ -87,27 +99,31 @@ ErrorCode safety_problem(TrainNum tran, SegmentNum segn, const char *fmt, ...) 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. diff --git a/hostside/speed.c b/hostside/speed.c index ba4dced..dea39f0 100644 --- a/hostside/speed.c +++ b/hostside/speed.c @@ -5,32 +5,173 @@ #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); }