From ba1c8f19b605c173e8aa68af8ac370410027b663 Mon Sep 17 00:00:00 2001 From: ian Date: Sat, 26 Jul 2008 16:36:28 +0000 Subject: [PATCH] Fix pretty much wholly buggy speed change logic --- hostside/safety.c | 15 +++++++------ hostside/safety.h | 1 - hostside/speed.c | 57 ++++++++++++++++++++--------------------------- 3 files changed, 32 insertions(+), 41 deletions(-) diff --git a/hostside/safety.c b/hostside/safety.c index 9838a22..bdcdbfe 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -263,6 +263,7 @@ Segment *segments; typedef struct { Train *train; unsigned + speed_info_specified:1, neednewplan:1, stopping:1, done_first_new_fdet:1, @@ -708,6 +709,7 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags, u.problem_callback= ppc; u.problem_callback_u= ppcu; u.neednewplan= !(flags & PREDF_OLDPLAN); + u.speed_info_specified= !!speed_info; if (!speed_info) { speedmanager_getinfo(tra,tnow,&speed_info_buf); @@ -720,13 +722,15 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags, u.desire_move= desire_move; u.desire_movposcomb= desire_movposcomb; - DPRINTF(safety,core," ***predicting*** %s%s maxspeed=%f" - " (speed %f try %f, step %d%s) stopdist=%d flags=%c%c%s" + DPRINTF(safety,core," ***predicting*** %s%s %smaxspeed=%f%s stopdist=%d" + " (speed %f, step %d%s) flags=%c%c%s" " desire=%s/%s\n", tra->backwards?"-":"",tra->pname, - u.maxspeed, tra->speed.speed, tra->speed.try_speed, tra->speed.step, - tra->speed.decel.running ? " decel" : "", + u.speed_info_specified ? "specified " : "", + u.maxspeed, u.stopping ? "(stopping)" : "", u.stopping_distance, + tra->speed.speed, tra->speed.step, + tra->speed.decel.running ? " decel" : "", "-j"[ !!(flags & PREDF_JUSTDET) ], "no"[ !!(flags & PREDF_OLDPLAN) ], u.stopping ? " stopping" : "", @@ -1224,9 +1228,6 @@ void safety_notify_detection(Segment *seg) { report_train_position(tra); speedmanager_safety_stop(tra,tnow); - ec= predict(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN,0,0, - &speed_info, 0,(char*)"safety commanding stop"); - assert(!ec); } ErrorCode safety_movposchange(Segment *seg, MovPosComb comb, diff --git a/hostside/safety.h b/hostside/safety.h index fe48cd9..abb3bac 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -54,7 +54,6 @@ struct Train { 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; diff --git a/hostside/speed.c b/hostside/speed.c index ae42bd7..e365f6f 100644 --- a/hostside/speed.c +++ b/hostside/speed.c @@ -37,12 +37,10 @@ static double current_speed(Train *tra, const struct timeval tnow) { double v1sq, v2sq, vtsq; double left_to_go, ts_v2, cs; - DPRINTF(speed,query, "current? %s try=%f decel.running=%d speed=%f" - " step=%d\n", - tra->pname, tra->speed.try_speed, + DPRINTF(speed,query, "current? %s decel.running=%d speed=%f" + " step=%d\n", tra->pname, tra->speed.decel.running, tra->speed.speed, tra->speed.step); - if (tra->speed.try_speed >= 0) return tra->speed.try_speed; if (!tra->speed.decel.running) return tra->speed.speed; left_to_go= (tra->speed.decel.abs.tv_sec - tnow.tv_sec) * 1000.0 + @@ -70,14 +68,12 @@ static double current_speed(Train *tra, const struct timeval tnow) { return cs; } -static double stoppingdistance(Train *tra, struct timeval tnow, double v) { +static double stoppingdistance(Train *tra, struct timeval tnow, + const SpeedRange *regime, double v) { double xs; - const SpeedRange *regime; if (v==0) return 0; - regime= stop_info(tra,v); - xs= tra->speedregimes[tra->n_speedregimes-1].xs; double v_ts_vcrit= v * regime->ts; if (xs > v_ts_vcrit) xs= v_ts_vcrit; @@ -90,8 +86,8 @@ void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo *ir) { double vnow= current_speed(tra,tnow); ir->max_speed_estimate= vnow * MARGIN_SPEED; - ir->stopping_distance= stoppingdistance(tra,tnow,vnow); - ir->stopping= tra->speed.try_speed < 0 && !tra->speed.step; + ir->stopping_distance= stoppingdistance(tra,tnow, stop_info(tra,vnow), vnow); + ir->stopping= !tra->speed.step; DPRINTF(speed,query, "getinfo? %s max=%f stopdist=%d %c\n", tra->pname, ir->max_speed_estimate, ir->stopping_distance, @@ -106,6 +102,8 @@ static ErrorCode request_core(Train *tra, int step, struct timeval tnow, PredictionProblemCallback *ppc, void *ppcu) { ErrorCode ec, ec2; double vnow, vtarg; + SpeedInfo try; + const SpeedRange *regime; DPRINTF(speed,core, "request %s%s step %d\n", tra->backwards?"-":"",tra->pname, @@ -119,36 +117,30 @@ static ErrorCode request_core(Train *tra, int step, struct timeval tnow, DPRINTF(speed,core, " request vnow=%f vtarg=%f\n", vnow,vtarg); - 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; - if (ppc || ppcu) { - ec= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0, - 0,(char*)"deceleration forbidden"); - assert(!ec); - } - xmit(tra); - return 0; - } - - tra->speed.try_speed= vtarg; - ec= predict(tra,tnow, 0, 0,0, 0, ppc,ppcu); + try.max_speed_estimate= MAX(vtarg,vnow); + regime= stop_info(tra,try.max_speed_estimate); + try.stopping_distance= stoppingdistance(tra,tnow, regime, + try.max_speed_estimate); + try.stopping= !step; + ec= predict(tra,tnow, + step <= tra->speed.step ? PREDF_OLDPLAN : 0, + 0,0, &try, ppc,ppcu); if (ec) { - tra->speed.try_speed= -1; ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0, 0,(char*)"abandoned acceleration"); assert(!ec2); return ec; } - tra->speed.step= step; toev_stop(&tra->speed.decel); - tra->speed.speed= vtarg; - tra->speed.try_speed= -1; + tra->speed.step= step; + tra->speed.speed= try.max_speed_estimate; + + if (vtarg <= vnow) { + tra->speed.decel.duration= regime->ts; + toev_start(&tra->speed.decel); + } xmit(tra); return 0; } @@ -165,7 +157,7 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step, void speedmanager_safety_stop(Train *tra, struct timeval tnow) { ErrorCode ec; - ec= request_core(tra,0,tnow,0,0); + ec= request_core(tra,0,tnow,0,(char*)"emergency stop"); assert(!ec); } @@ -176,7 +168,6 @@ void speedmanager_reset_train(Train *tra) { tra->speed.decel.pclass= "decel"; tra->speed.decel.pinst= tra->pname; tra->speed.speed= 0; - tra->speed.try_speed= -1; if (tra->addr < 0) return; -- 2.30.2