From 65bc3f00d4e20dee3a74689f854e1264637d5d07 Mon Sep 17 00:00:00 2001 From: ian Date: Sat, 26 Jul 2008 15:41:00 +0000 Subject: [PATCH] Nicer interface between safety and speedmanager --- hostside/safety.c | 36 +++++++++++++++++++++++------------- hostside/safety.h | 15 +++++++++++---- hostside/speed.c | 33 ++++++++++++++++----------------- hostside/xs.gdb | 2 +- 4 files changed, 51 insertions(+), 35 deletions(-) diff --git a/hostside/safety.c b/hostside/safety.c index bfa6c23..9838a22 100644 --- a/hostside/safety.c +++ b/hostside/safety.c @@ -694,8 +694,10 @@ static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c, ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags, Segment *desire_move, int desire_movposcomb, + const SpeedInfo *speed_info, PredictionProblemCallback *ppc, void *ppcu) { PredictUserContext u; + SpeedInfo speed_info_buf; Segment *foredet; SEG_IV; ErrorCode ec; @@ -706,9 +708,14 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags, u.problem_callback= ppc; u.problem_callback_u= ppcu; u.neednewplan= !(flags & PREDF_OLDPLAN); - u.maxspeed= speedmanager_speed_maxestimate(u.train, tnow); - u.stopping= (flags & PREDF_OLDPLAN) && speedmanager_stopping(u.train); - u.stopping_distance= speedmanager_stoppingdistance(u.train, tnow); + + if (!speed_info) { + speedmanager_getinfo(tra,tnow,&speed_info_buf); + speed_info= &speed_info_buf; + } + u.stopping= (flags & PREDF_OLDPLAN) && speed_info->stopping; + u.maxspeed= speed_info->max_speed_estimate; + u.stopping_distance= speed_info->stopping_distance; u.desire_move= desire_move; u.desire_movposcomb= desire_movposcomb; @@ -1135,7 +1142,7 @@ ErrorCode safety_setdirection(Train *tra, int backwards, tra->backwards ^= 1; newfdet.seg->tr_backwards ^= 1; - ec= predict(tra,tnow, 0, 0,0, ppc,ppcu); + ec= predict(tra,tnow, 0, 0,0, 0,ppc,ppcu); if (!ec) { /* yay! */ report_train_position(tra); @@ -1151,7 +1158,7 @@ ErrorCode safety_setdirection(Train *tra, int backwards, tra->backwards ^= 1; newfdet.seg->tr_backwards ^= 1; /* in case it's the same as oldfdet */ - ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,(char*)"abandon reverse"); + ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0,0,(char*)"abandon reverse"); assert(!ec2); return ec; @@ -1169,8 +1176,9 @@ void safety_notify_detection(Segment *seg) { Train *tra; ErrorCode ec; Segment *interferer; - int stopdist, maxinto; + int maxinto; struct timeval tnow; + SpeedInfo speed_info; if (seg->det_ignore) return; @@ -1194,9 +1202,10 @@ void safety_notify_detection(Segment *seg) { mgettimeofday(&tnow); - stopdist= speedmanager_stoppingdistance(tra,tnow); + speedmanager_getinfo(tra,tnow,&speed_info); maxinto= seg->i->poscombs[seg->movposcomb].dist; - if (speedmanager_stopping(tra) && maxinto > stopdist) maxinto= stopdist; + if (speed_info.stopping && maxinto > speed_info.stopping_distance) + maxinto= speed_info.stopping_distance; tra->foredetect= seg; tra->uncertainty= tra->maxinto= maxinto; @@ -1204,18 +1213,19 @@ void safety_notify_detection(Segment *seg) { report_train_position(tra); ec= predict(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN,0,0, - detection_report_problem,0); + &speed_info, detection_report_problem,0); if (!ec) return; assert(ec == EC_SignallingPredictedProblem); - if (maxinto > stopdist) maxinto= stopdist; + if (maxinto > speed_info.stopping_distance) + maxinto= speed_info.stopping_distance; tra->maxinto= tra->uncertainty= maxinto; report_train_position(tra); speedmanager_safety_stop(tra,tnow); ec= predict(tra,tnow, PREDF_JUSTDET|PREDF_OLDPLAN,0,0, - 0,(char*)"safety commanding stop"); + &speed_info, 0,(char*)"safety commanding stop"); assert(!ec); } @@ -1240,10 +1250,10 @@ ErrorCode safety_movposchange(Segment *seg, MovPosComb comb, return predict_problem(&u,seg, "route set for approaching train"); mgettimeofday(&tnow); - ec= predict(seg->owner,tnow, 0, seg,comb, ppc,ppcu); + ec= predict(seg->owner,tnow, 0, seg,comb, 0, ppc,ppcu); if (!ec) return 0; - ec2= predict(seg->owner,tnow, PREDF_OLDPLAN, 0,0, + ec2= predict(seg->owner,tnow, PREDF_OLDPLAN, 0,0, 0, 0,(char*)"abandon movposchange"); assert(!ec2); diff --git a/hostside/safety.h b/hostside/safety.h index ebde0ae..fe48cd9 100644 --- a/hostside/safety.h +++ b/hostside/safety.h @@ -130,13 +130,21 @@ ErrorCode safety_setdirection(Train *tra, int backwards, /* the old plan is necessarily sufficient as we are not intending * to accelerate, change a movpos, etc. */ +typedef struct { + unsigned stopping:1; + double max_speed_estimate; + Distance stopping_distance; +} SpeedInfo; + ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags, Segment *desire_move, int desire_movposcomb, + const SpeedInfo *speed_info /* 0: use speedmgr */, PredictionProblemCallback *ppc, void *ppcu); /* Lower-level interface for speedmanager etc. * Caller must call this with different situations until it succeeds! * Caller may pass ppc=0 and ppcu=(char*)"some context" to - * cause safety_panic if it fails. */ + * cause safety_panic if it fails. + */ void report_train_position(Train *tra); void report_train_ownerships(Train *tra, Segment *furthest, @@ -241,9 +249,8 @@ ErrorCode speedmanager_speedchange_request(Train *tra, int step, void speedmanager_safety_stop(Train *tra, struct timeval tnow); void speedmanager_reset_train(Train *tra); -double speedmanager_speed_maxestimate(Train *tra, struct timeval tnow); -double speedmanager_stoppingdistance(Train *tra, struct timeval tnow); -int speedmanager_stopping(Train *tra); + +void speedmanager_getinfo(Train *tra, struct timeval tnow, SpeedInfo*); int speedmanager_stopped(Train *tra); void actual_speed(Train *tra, int step); diff --git a/hostside/speed.c b/hostside/speed.c index e8ad203..ae42bd7 100644 --- a/hostside/speed.c +++ b/hostside/speed.c @@ -70,15 +70,9 @@ static double current_speed(Train *tra, const struct timeval tnow) { return cs; } -double speedmanager_speed_maxestimate(Train *tra, struct timeval tnow) { - return current_speed(tra,tnow) * MARGIN_SPEED; -} - -double speedmanager_stoppingdistance(Train *tra, struct timeval tnow) { - double v, xs; +static double stoppingdistance(Train *tra, struct timeval tnow, double v) { + double xs; const SpeedRange *regime; - - v= current_speed(tra,tnow); if (v==0) return 0; @@ -92,12 +86,17 @@ double speedmanager_stoppingdistance(Train *tra, struct timeval tnow) { return xs + v * MARGIN_STOPTIME; } -int speedmanager_stopping(Train *tra) { - int r; - r= tra->speed.try_speed < 0 && !tra->speed.step; - DPRINTF(speed,query, "stopping? %s returning %d\n", tra->pname, r); - return r; -} +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; + + DPRINTF(speed,query, "getinfo? %s max=%f stopdist=%d %c\n", tra->pname, + ir->max_speed_estimate, ir->stopping_distance, + "-s"[ir->stopping]); +} int speedmanager_stopped(Train *tra) { return tra->speed.speed == 0; @@ -127,7 +126,7 @@ static ErrorCode request_core(Train *tra, int step, struct timeval tnow, toev_start(&tra->speed.decel); tra->speed.speed= vnow; if (ppc || ppcu) { - ec= predict(tra,tnow, PREDF_OLDPLAN,0,0, + ec= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0, 0,(char*)"deceleration forbidden"); assert(!ec); } @@ -136,11 +135,11 @@ static ErrorCode request_core(Train *tra, int step, struct timeval tnow, } tra->speed.try_speed= vtarg; - ec= predict(tra,tnow, 0, 0,0, ppc,ppcu); + ec= predict(tra,tnow, 0, 0,0, 0, ppc,ppcu); if (ec) { tra->speed.try_speed= -1; - ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, + ec2= predict(tra,tnow, PREDF_OLDPLAN,0,0, 0, 0,(char*)"abandoned acceleration"); assert(!ec2); return ec; diff --git a/hostside/xs.gdb b/hostside/xs.gdb index 4630998..b5ad1e4 100644 --- a/hostside/xs.gdb +++ b/hostside/xs.gdb @@ -1,7 +1,7 @@ file ./realtime break vdie break nmra_errchk_fail -break predict_problem +#break predict_problem break safety_panic break obc_error #break findhead_nextseg -- 2.30.2