From: ian <ian>
Date: Sun, 27 Feb 2005 22:58:22 +0000 (+0000)
Subject: work at Redemption, checkin on return: reorganise things a bit and work on lay_train... 
X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?a=commitdiff_plain;h=7e8837d9da361daa0cd45088ca337fd793aa9ba0;p=trains.git

work at Redemption, checkin on return: reorganise things a bit and work on lay_train and inversion etc.
---

diff --git a/hostside/TODO b/hostside/TODO
new file mode 100644
index 0000000..c08e6f4
--- /dev/null
+++ b/hostside/TODO
@@ -0,0 +1,3 @@
+things not yet considered at all in safety code
+	polarity
+	min. curve specifications
diff --git a/hostside/safety.c b/hostside/safety.c
index 4900e1d..7a791a9 100644
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -1,127 +1,43 @@
-/**/
+/*
+ */
 
 #include <stdio.h>
 
 #include "safety.h"
 
-State safety_actual;
-
-typedef struct TrackLocation TrackLocation;
-struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
-  SegmentNum segn; /* current segment */
-  long into; /* distance from start of segment */
-  unsigned backwards:1; /* if 1, into is positive and measured from end */
-};
-
-const SegmentLinkInfo *trackloc_segmentlink_near(const TrackLocation *tloc) {
-  SegmentState *seg = s->seg[tloc->segn];
-  const SegmentInfo *segi= &safety_segis[tloc->segn];
-
-  return (tloc->backwards ? &segi->forwards :
-	  seg->pt_sideways ?  &segi->sideways :
-	  &segi->backwards);
-}
-  
-const SegmentLinkInfo *trackloc_segmentlink_far(const TrackLocation *tloc) {
-  SegmentState *seg = s->seg[tloc->segn];
-  const SegmentInfo *segi= &safety_segis[tloc->segn];
-
-  return (tloc->backwards ? &segi->backwards :
-	  seg->pt_sideways ?  &segi->sideways :
-	  &segi->forwards);
-}
-  
-long trackloc_remaininseg(const TrackLocation *tloc) {
-  /* Returns dist that tloc can advance before it goes into next segment. */
-  State *s = &safety_state;
-  SegmentState *seg = s->seg[tloc->segn];
-  const SegmentInfo *segi= &safety_segis[tloc->segn];
-  const SegmentLinkInfo *lnki_near, *lnki_far;
-  long segment_len;
-  
-  lnki_near= trackloc_segmentlink_near(tloc);
-  lnki_far=  trackloc_segmentlink_far(tloc);
-  segment_len= linki_near->dist + link_far->dist;
-  assert(tloc->into <= segment_len);
-  return segment_len - tloc->info;
-}  
-
-void trackloc_further(TrackLocation *tloc, long *remain_io) {
-  /* Advances tloc, decrementing *remain_io, until either
-   * *remain_io becomes zero, or tloc->segn changes. */
-  State *s = &safety_state;
-  SegmentState *seg = s->seg[tloc->segn];
-  const SegmentInfo *segi= &safety_segis[tloc->segn];
-  const SegmentLinkInfo *lnki_far;
-
-  segment_remain= trackloc_remaininseg(tloc);
-
-  if (*remain_io <= segment_remain) {
-    tloc->into += *remain_io;
-    *remain_io= 0;
-  } else {
-    lnki_far= trackloc_segmentlink_far(tloc);
-    *remain_io -= segment_remain;
-    tloc->segn= lnki_far->next;
-    tloc->into= 0;
-    tloc->backwards ^= lnki_far->next_backwards;
-  }
-}
-
-static voi seg_clear_stale(SegmentState *seg) {
+static void seg_clear_stale(SegmentState *seg) {
   if (!seg->tr_updated) {
-    seg->tr_here_now= seg->tr_here_future=
-      seg->tr_detect_now= seg->tr_detect_future= 0;
+    seg->owned= 0;
+    seg->until_here= seg->until_detect= NOTA(TimeInterval);
   }
 }
 
-void lay_train_checkclash(ErrorCode *ec, SegmentLinkInfo *lnki
-			  TrainNum tran,) {
-  SegmentNum clash_segn;
-  SegmentState *clash_seg;
-  TrainNum clash_tran;
-  TrainState *clash_tra;
-
-  clash_segn= lnki->clashing;
-  if (clash_segn == NOTA(Segment)) return;
-  
-  clash_seg= &s->segs[lnki->clashing];
-  if (!clash_seg->owned) return;
-
-  clash_tran= clash->owner;
-  clash_tra= &s->tras[clash_tran];
-
-  if (clash_tra->justarrived) {
-    TrackLocation clash_loc;
-    clash_loc.segn= clash_tra->foredetect;
-    clash_loc.
-      
-
-= &s->segs[lnki->clashing];
-  if (clash->
-  
-  lay_train_checkclash2( 
-
-static void lay_train(ErrorCode *ec, TrainNum tran,
-		      TrackLocation tloc, long into,
-		      unsigned backwards, long speed) {
-  /* pass 0: update actual train location, check for train clashing
-   * with itself (ie, fail if we find segment with same train and
-   * tr_updated set.
-   * pass 1: update detection only.  Clashingness checking is
-   * done where it is convenient not to avoid it, but is not
-   * necessary. */
+typedef struct {
+  /* constant inputs */
+  TrainNum tran;
+  /* modified in place by lay_train_pass: */
+  ErrorCode ec;
+  int invert_count[1]; /* count of (switchable) segments,
+			* invert_count[0]: inverted from train's pov
+			*   iff train is backwards (ie, train not inverted)
+			* invert_count[1]: train is inverted
+			* set to -1 if any unswitchable is the other way */
+  SegmentNum invert_forcer; /* the unswitchable which forces */
+} LayTrainState;
+
+static void lay_train_pass(LayTrainState l,
+			   TrackLocation tloc, long advance,
+			   unsigned backwards, long speed,
+			   unsigned check_clash) {
   SegmentState *seg;
   const SegmentInfo *segi;
   long overall, remain;
+  int *invert_likehere, *invert_unlikehere;
 
-  if (*ec) return;
+  if (l->ec) return;
 
   segn= tra->segn;
   seg= &s->segs[segn];
-  tloc.segn= segn;
-  tloc.into= into;
-  tloc.backwards= seg->tr_backwards ^ backwards;
 
   remain= overall= advance + speed * SPEED_CLEAR_MULT;
 
@@ -132,12 +48,13 @@ static void lay_train(ErrorCode *ec, TrainNum tran,
 
     if (check_clash) {
       if (seg->tr_updated) {
-	*ec= safety_problem(tloc.segn, tran, tran, "collision with itself");
+	*ec= safety_problem(tloc.segn, l->tran, "self-collision");
 	return;
       }
       if (seg->owned) {
 	if (seg->owner != tran) {
-	  *ec= safety_problem(tloc.segn, tran, seg->owner, "collision");
+	  l->ec= safety_problem(tloc.segn, l->tran, seg->owner, "collision"
+				" with %s", info_trans[seg->owner].pname);
 	  return;
 	}
 	seg_clear_stale(seg);
@@ -147,23 +64,66 @@ static void lay_train(ErrorCode *ec, TrainNum tran,
     seg->owned= 1;
     seg->owner_backwards= tloc.backwards ^ backwards;
     seg->tr_updated= 1;
-    seg->tran= tran;
+    seg->tran= l->tran;
+
+    train_inverted_here= seg->seg_inverted ^ seg->tr_backwards;
+    invert_likehere= &l->invert_count[train_inverted_here];
+
+    if (segi->invertible) {
+      if (*invert_likehere >= 0)
+	(*invert_likehere)++;
+    } else {
+      if (*invert_likehere < 0) {
+	l->ec= safety_problem(l->tran, NOTA(SegmentNum), "train requires"
+			      " noninvertible segments with opposite polarity:"
+			      " @%s, @%s", segi->pname,
+			      info_segments[l->invert_forcer].pname);
+	return;
+      }
+      assert(!seg->seg_inverted);
+      (*invert_likehere)++;
+      l->invert_count[!train_inverted_here]= -1;
+      l->invert_forcer= segn;
+    }
 
     dist_until= (overall - remain) - advance;
-    time_until= (SPEED_FACTOR * dist_until) / tra->speed;
-    *(pass==0 ? &seg->until_here : &seg->until_detect)= time_until;
+    time_until= !speed ? 0 :
+      (SPEED_FACTOR * dist_until) / speed;
+    *(check_clash ? &seg->until_here : &seg->until_detect)= time_until;
 
     if (!remain) break;
     trackloc_further(&tloc, &remain);
   }
 }
 
-static void lay_train_done() {
+static void lay_train_inversions(LayTrainState *l) {
+  SegmentNum segn;
+  SegmentState *seg;
+  int train_be_inverted, seg_be_inverted;
+ 
+  if (l->ec) return;
+
+  train_be_inverted= l->invert_count[1] > l->invert_count[0];
+  assert(l->invert_count[train_be_inverted] >= 0);
+
   for (segn=0, seg=s->segs;
        segn <= NUM_SEGMENTS;
        segn++, seg++) {
-    if (seg->tr_here_future && seg->tran == tran) {
-      seg_clear_stale(seg);
+    if (!seg->tr_updated) continue;
+    assert(seg->owner == l->tran);
+    seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
+    
+}
+
+static void lay_train_done(LayTrainState *l) {
+  SegmentNum segn;
+  SegmentState *seg;
+  
+  for (segn=0, seg=s->segs;
+       segn <= NUM_SEGMENTS;
+       segn++, seg++) {
+    if (seg->tran == l->tran) {
+      if (!seg->tr_updated) seg_clear_stale(seg);
       seg->tr_updated= 0;
     }
     assert(!seg->tr_updated);
@@ -174,49 +134,78 @@ static void lay_train_done() {
   }
 }
 
-void safety_train_changed(TrainNum tran) {
+static ErrorCode lay_train(TrainNum tran, long added_slop) {
   State *s= &safety_state;
-  TrainState *tra= &s->tras[tran];
-  TrainInfo *trai= info_train[tran];
+  TrainState *tra= &s->trains[tran];
+  TrainInfo *trai= info_trains[tran];
   SegmentState *seg;
   TrackLocation tloc;
   long head, future;
+  LayTrainState l;
 
   segn= tra->foredetect;
-  seg= &s->seg[segn];
-
-  head= tra->backwards ? trai->tail : trai->head;
-  lay_train(tran, &ec, tloc, head, 0,1);
-
-  ec= 0;
-  lay_train(&ec, tran, tloc, head, 0);
-  lay_train(&ec, tran, tloc, 0,    1);
-  lay_train_done(tran);
-  if (ec) return ec;
-
+  seg= &s->segments[segn];
 
   tloc.segn= segn;
-  tloc.into= 
-    
-    
-
+  tloc.into= tra->maxinto;
+  tloc.backwards= seg->tr_backwards ^ backwards;
 
+  l.tran= tran;
+  l.ec= 0;
+  l.invert_count[0]= l.invert_count[1]= 0;
+  
+  head= tra->backwards ? trai->tail : trai->head;
+  headslop= head + added_slop;
+
+  /* 1st pass:
+   * update actual train location (train's head and future locations).
+   *
+   * 2nd pass:
+   * update detection (future locations) only.
+   *
+   * 3rd pass:
+   * train location (current location and tail)
+   *
+   * Clash checking is done in passes 1 and 3, including checking
+   * whether the train clashes with itself (ie, fail if we find
+   * segment with same train and tr_updated set).
+   */
+  lay_train(&l, tloc, tra->speed,  headslop, 1);
+  lay_train(&l, tloc, tra->speed,  0,        0);
+
+  trackloc_reverse(&tloc);
+  seg->tr_updated= 0; /* we're about to do this one again */
+  tail= tra->backwards ? trai->head : trai->tail;
+  taildet= tra->detectable + tail;
+
+  lay_train(&l, tloc, 0,           taildet,  1); 
+
+  lay_train_inversions(&l);
+  lay_train_done(&l);
+
+  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). */
+  TrainState *tra = s->trains[tran];
 
-  for (segn = 
-s->tras[tran]
-  SegmentState *seg = s->seg[segn];
+  tra->speed= newspeed;
+  actual_setspeed(tran);
+  speedmanager_speedchange_notify(tran);
+}
 
 void safety_notify_detection(SegmentNum segn) {
   State *s = &safety_state;
-  SegmentState *seg = s->seg[segn];
+  SegmentState *seg = s->segments[segn];
   TrainNum tran = segs->owner;
-  TrainState *tra = s->tras[tran];
+  TrainState *tra = s->trains[tran];
   TrackLocation tloc;
 
   if (seg->detectable_now) return;
   if (!seg->detectable_future)
-    safety_panic(segn, NONE, "unexpected detection");
+    safety_panic(NOTA(TrainNum), segn, "unexpected detection");
 
   tloc.segn= segn;
   tloc.into= 0;
@@ -225,30 +214,65 @@ void safety_notify_detection(SegmentNum segn) {
   tra->foredetect= segn;
   tra->maxinto= trackloc_remaininseg(&tloc);
   
-  if (seg->autostop) {
-    seg->autostop= 0;
-    speedmanager_setspeed(tran, 0);
-    if (tra->maxinto > AUTOSTOP_UNCERTAINTY)
+  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;
+
+    seg->cm_autostop= 0;
+    setspeed(tran, 0);
   }
   tra->uncertainty= tra->maxinto;
     
-  ec= safety_train_changed(tran);
+  ec= lay_train(tran, 0);
   if (ec) {
     logmsg(tran, segn, "emergency stop");
-    actual_emergencystop(tran);
-    tra->justarrived= 0;
-    ec= safety_train_changed(s, tran);
-    if (ec) panic(segn, tran, "emergency stop insufficient!");
+    safety_emergencystop(tran);
   }
 }
 
+void safety_emergencystop(TranNum) {
+  ErrorCode ec;
 
-  
-    
-    
+  tra->speed= 0;
+  actual_emergencystop(tran);
+  ec= lay_train(tran, ESTOP_UNCERTAINTY);
+  if (ec) safety_panic(tran, segn, "emergency stop forbidden!");
+  speedmanager_speedchange_notify(tran, tra->backwards);
+}
+
+void safety_requestspeed(TrainNum tran, long newspeed) {
+  long oldspeed;
+  TrainState *tra = s->trains[tran];
 
-  own
+  oldspeed= tra->speed;
+  tra->speed= newspeed;
+
+  ec= lay_train(tran, 0);
+
+  if (ec) {
+    if (oldspeed && oldspeed < newspeed) {
+      logmsg(tran, NOTA(SegmentNum), "countermanded acceleration",
+	     " from %l to %l", oldspeed, newspeed);
+    } else if (oldspeed) {
+      safety_panic(tran, NOTA(SegmentNum), "deceleration forbidden!"
+		   " (from %l to %l", oldspeed, newspeed);
+    } else {
+      logmsg(tran, NOTA(SegmentNum), "countermanded motion start");
+    }
+    setspeed(tran, oldspeed);
+    ec= lay_train(tran, 0);
+    if (ec)
+      safety_panic(tran, NOTA(SegmentNum), "countermanding"
+		   " speed change insufficient!");
+    return;
+  }
+
+  setspeed(tran, newspeed);
+}
 			     
 
 int main(void) {
diff --git a/hostside/safety.h b/hostside/safety.h
index 31e2006..8cb7892 100644
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -3,6 +3,8 @@
 #ifndef SAFETY_H
 #define SAFETY_H
 
+/*========== basic types etc. ==========*/
+
 typedef unsigned short TrainNum;
 typedef unsigned short SegmentNum;
 typedef unsigned short LocationNum;
@@ -10,10 +12,12 @@ typedef short TimeInterval;
 typedef short Distance;
 typedef char Speed; /* non-negative, units of 4mm/s */
 
+/*========== state of the layout ==========*/
+
 typedef struct {
-  SegmentNum foredetect; /* train's detectable part is at most maxinto */
-  Distance maxinto, uncertainty; /* into foredetect but train may be */
-  unsigned                       /* uncertainty less far advanced */
+  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;
 } TrainState;
@@ -23,9 +27,11 @@ typedef struct {
     owned:1, /* this track segment is reserved for a train */
     tr_backwards:1, /* train's motion is (would be) backwards wrt track */
     pt_sideways:1, /* points are set to `alternative'. (no points?: 0) */
-    pt_moving:1, /* points have been told to change, sideways is new state */
-    autostop:1, /* owning train is slow and wants to stop on detection */
-    tr_updated:1; /* for use by safety_train_changed etc.; otherwise 0 */
+    pt_moving:1, /* points have been told to change to pt_sideways */
+    cm_autostop:1, /* train should stop on detection */
+    cm_pointchange:1, /* points should change when they can */
+    seg_inverted:1, /* polarity is inverted! */
+    tr_updated:1; /* for use by safety.c:lay_train etc.; otherwise 0 */
   TimeInterval until_here, /* ) nonnegative; */  /* ) always valid but */
     until_detect;          /* ) 0 if already */  /* )  only meaningful */
   TrainNum owner;                                /* )  iff owned       */
@@ -39,20 +45,90 @@ typedef struct {
 } SegmentLinkInfo;
 
 typedef struct {
+  unsigned invertible:1;
   SegmentLink backwards, forwards, sideways;
+  const char *pname;
 } SegmentInfo;
 
 typedef struct {
   Speed maxspeed;
   Distance tail, detectable, head;
+  const char *pname;
 } TrainInfo;
 
-extern const TrainInfo info_train[NUM_TRAINS];
-extern const SegmentInfo info_segment[NUM_SEGMENTS];
+extern const TrainInfo info_trains[NUM_TRAINS];
+extern const SegmentInfo info_segments[NUM_SEGMENTS];
 
 typedef struct {
-  TrainState tras[NUM_TRAINS];
-  SegmentState segs[NUM_SEGMENTS];
+  TrainState trains[NUM_TRAINS];
+  SegmentState segments[NUM_SEGMENTS];
 } State;
 
+extern State s;
+
+/*========== safety.c ==========*/
+/*
+ * safety.c is responsible for ensuring that things don't go
+ * physically wrong (eg, collisions, derailments, short circuits,
+ * etc.).
+ */
+
+void safety_emergencystop(TranNum);
+  /* Callable directly in response to application command. */
+
+void 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).
+   */
+
+void safety_notify_detection(SegmentNum segn);
+  /* To be called by actual.c when new train detection occurs. */
+
+/*========== speedmgr.c ==========*/
+
+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. */
+
+/*========== actual.c ==========*/
+/* actual.c should only be called from safety.c.
+ * It is responsible for communicating with the PICs, including
+ * repeating the NMRA commands and redacting detection information.
+ */
+
+void actual_setspeed(TrainNum tran);
+  
+/*
+ *
+ * Entrypoints are:                  Called from, and as a result of:
+ *   actual_setspeed                    safety.c
+ *   actual_emergencystop               safety.c
+
+
+/*========== utils.c ==========*/
+
+typedef struct TrackLocation TrackLocation;
+struct TrackLocation { /* transparent, and manipulable by trackloc_... fns */
+  SegmentNum segn; /* current segment */
+  long into; /* distance from start of segment */
+  unsigned backwards:1; /* if 1, into is positive and measured from end */
+};
+
+long trackloc_remaininseg(const TrackLocation *tloc);
+  /* Returns dist that tloc can advance before it goes into next segment. */
+
+void trackloc_further(TrackLocation *tloc, long *remain_io);
+  /* Advances tloc, decrementing *remain_io, until either
+   * *remain_io becomes zero, or tloc->segn changes. */
+
+void trackloc_reverse(TrackLocation *tloc);
+  /* Reverses tloc without changing its actual location. */
+
+const SegmentLinkInfo *trackloc_segmentlink_near(const TrackLocation *tloc);
+const SegmentLinkInfo *trackloc_segmentlink_far(const TrackLocation *tloc);
+
 #endif /*SAFETY_H*/
diff --git a/hostside/utils.c b/hostside/utils.c
new file mode 100644
index 0000000..13c5eb2
--- /dev/null
+++ b/hostside/utils.c
@@ -0,0 +1,57 @@
+const SegmentLinkInfo *trackloc_segmentlink_near(const TrackLocation *tloc) {
+  SegmentState *seg = s->segments[tloc->segn];
+  const SegmentInfo *segi= &safety_segis[tloc->segn];
+
+  return (tloc->backwards ? &segi->forwards :
+	  seg->pt_sideways ?  &segi->sideways :
+	  &segi->backwards);
+}
+  
+const SegmentLinkInfo *trackloc_segmentlink_far(const TrackLocation *tloc) {
+  SegmentState *seg = s->segments[tloc->segn];
+  const SegmentInfo *segi= &safety_segis[tloc->segn];
+
+  return (tloc->backwards ? &segi->backwards :
+	  seg->pt_sideways ?  &segi->sideways :
+	  &segi->forwards);
+}
+  
+long trackloc_remaininseg(const TrackLocation *tloc) {
+  State *s = &safety_state;
+  SegmentState *seg = s->segments[tloc->segn];
+  const SegmentInfo *segi= &safety_segis[tloc->segn];
+  const SegmentLinkInfo *lnki_near, *lnki_far;
+  long segment_len;
+  
+  lnki_near= trackloc_segmentlink_near(tloc);
+  lnki_far=  trackloc_segmentlink_far(tloc);
+  segment_len= linki_near->dist + link_far->dist;
+  assert(tloc->into <= segment_len);
+  return segment_len - tloc->info;
+}  
+
+void trackloc_further(TrackLocation *tloc, long *remain_io) {
+  State *s = &safety_state;
+  SegmentState *seg = s->segments[tloc->segn];
+  const SegmentInfo *segi= &safety_segis[tloc->segn];
+  const SegmentLinkInfo *lnki_far;
+
+  segment_remain= trackloc_remaininseg(tloc);
+
+  if (*remain_io <= segment_remain) {
+    tloc->into += *remain_io;
+    *remain_io= 0;
+  } else {
+    lnki_far= trackloc_segmentlink_far(tloc);
+    *remain_io -= segment_remain;
+    tloc->segn= lnki_far->next;
+    tloc->into= 0;
+    tloc->backwards ^= lnki_far->next_backwards;
+  }
+}
+
+void trackloc_reverse(TrackLocation *tloc) {
+  tloc->into= trackloc_remaininseg(tloc->into);
+  tloc->backwards ^= 1;
+}
+