Track live input health and recover GPS failover

cereal/gps.go

@@ -2,29 +2,126 @@ package cereal
 
 import (
 	"log/slog"
+	"time"
 
 	"pfeifer.dev/mapd/cereal/log"
 	ms "pfeifer.dev/mapd/settings"
 )
 
+const (
+	gpsLocationExternalTimeout = time.Second
+	gpsLocationTimeout         = 10 * time.Second
+	gpsExternalPromotionDelay  = 2 * time.Second
+)
+
+type gpsSource uint8
+
+const (
+	gpsSourceNone gpsSource = iota
+	gpsSourceInternal
+	gpsSourceExternal
+)
+
+type gpsSample struct {
+	data       log.GpsLocationData
+	receivedAt time.Time
+	usable     bool
+}
+
+func (s gpsSample) Healthy(now time.Time, maxAge time.Duration) bool {
+	age := now.Sub(s.receivedAt)
+	return s.usable && age >= 0 && age < maxAge
+}
+
+func (s *gpsSample) Update(data log.GpsLocationData, status SubscriberStatus) {
+	s.receivedAt = status.ReceivedAt
+	s.usable = status.EventValid && (data.HasFix() || data.Flags()&1 != 0)
+	if s.usable {
+		s.data = data
+	}
+}
+
 type GpsSub struct {
-	gpsLocation         Subscriber[log.GpsLocationData]
-	gpsLocationExternal Subscriber[log.GpsLocationData]
-	useExt              bool
+	gpsLocation               Subscriber[log.GpsLocationData]
+	gpsLocationExternal       Subscriber[log.GpsLocationData]
+	gpsLocationSample         gpsSample
+	gpsLocationExternalSample gpsSample
+	now                       func() time.Time
+	source                    gpsSource
+	externalHealthySince      time.Time
 }
 
 func (s *GpsSub) Read() (locationData log.GpsLocationData, success bool) {
-	if s.useExt {
-		return s.gpsLocationExternal.Read()
-	} else {
-		locationData, success = s.gpsLocationExternal.Read()
-		if success {
-			s.useExt = true
-			slog.Info("Found gpsLocationExternal, switching to external GPS provider")
-			return locationData, success
+	internalData, internalUpdated := s.gpsLocation.Read()
+	if internalUpdated {
+		s.gpsLocationSample.Update(internalData, s.gpsLocation.Status())
+	}
+
+	externalData, externalUpdated := s.gpsLocationExternal.Read()
+	if externalUpdated {
+		s.gpsLocationExternalSample.Update(externalData, s.gpsLocationExternal.Status())
+	}
+
+	now := s.nowTime()
+	source := s.selectSource(now)
+
+	if source != s.source {
+		s.source = source
+		if source == gpsSourceExternal {
+			slog.Info("Switching to external GPS provider")
+		} else if source == gpsSourceInternal {
+			slog.Info("Switching to internal GPS provider")
 		}
 	}
-	return s.gpsLocation.Read()
+
+	switch source {
+	case gpsSourceExternal:
+		if externalUpdated && s.gpsLocationExternalSample.usable {
+			return s.gpsLocationExternalSample.data, true
+		}
+	case gpsSourceInternal:
+		if internalUpdated && s.gpsLocationSample.usable {
+			return s.gpsLocationSample.data, true
+		}
+	}
+	return locationData, false
+}
+
+func (s *GpsSub) selectSource(now time.Time) gpsSource {
+	externalHealthy := s.gpsLocationExternalSample.Healthy(now, gpsLocationExternalTimeout)
+	internalHealthy := s.gpsLocationSample.Healthy(now, gpsLocationTimeout)
+	if externalHealthy {
+		if s.externalHealthySince.IsZero() {
+			s.externalHealthySince = now
+		}
+	} else {
+		s.externalHealthySince = time.Time{}
+	}
+	if externalHealthy && (s.source == gpsSourceExternal || !internalHealthy || now.Sub(s.externalHealthySince) >= gpsExternalPromotionDelay) {
+		return gpsSourceExternal
+	}
+	if internalHealthy {
+		return gpsSourceInternal
+	}
+	return gpsSourceNone
+}
+
+func (s *GpsSub) Fresh(now time.Time) bool {
+	switch s.source {
+	case gpsSourceExternal:
+		return s.gpsLocationExternalSample.Healthy(now, gpsLocationExternalTimeout)
+	case gpsSourceInternal:
+		return s.gpsLocationSample.Healthy(now, gpsLocationTimeout)
+	default:
+		return false
+	}
+}
+
+func (s *GpsSub) nowTime() time.Time {
+	if s.now != nil {
+		return s.now()
+	}
+	return time.Now()
 }
 
 func (s *GpsSub) Close() {
@@ -36,6 +133,6 @@ func GetGpsSub() (gpsSub GpsSub) {
 	return GpsSub{
 		gpsLocation:         NewSubscriber("gpsLocation", GpsLocationReader, true, ms.Settings.SubscriberSettings.ShadowGpsLocation),
 		gpsLocationExternal: NewSubscriber("gpsLocationExternal", GpsLocationExternalReader, true, ms.Settings.SubscriberSettings.ShadowGpsLocationExternal),
-		useExt:              false,
+		now:                 time.Now,
 	}
 }

cereal/subscriber.go

@@ -2,6 +2,7 @@ package cereal
 
 import (
 	"math"
+	"time"
 
 	"capnproto.org/go/capnp/v3"
 	"github.com/pfeiferj/gomsgq"
@@ -11,16 +12,30 @@ import (
 
 type Reader[T any] func(log.Event) (T, error)
 
+type SubscriberStatus struct {
+	EventValid bool
+	ReceivedAt time.Time
+	Seen       bool
+}
+
+func (s SubscriberStatus) Healthy(now time.Time, maxAge time.Duration) bool {
+	age := now.Sub(s.ReceivedAt)
+	return s.Seen && s.EventValid && age >= 0 && age < maxAge
+}
+
 type Subscriber[T any] struct {
 	Sub    gomsgq.MsgqSubscriber
+	now    func() time.Time
 	reader Reader[T]
+	status SubscriberStatus
 }
 
 func (s *Subscriber[T]) Read() (obj T, success bool) {
 	data := s.Sub.Read()
 	if len(data) == 0 {
 		return obj, false
 	}
+	receivedAt := s.nowTime()
 	msg, err := capnp.Unmarshal(data)
 	if err != nil {
 		return obj, false
@@ -38,9 +53,25 @@ func (s *Subscriber[T]) Read() (obj T, success bool) {
 	if err != nil {
 		return obj, false
 	}
+	s.status = SubscriberStatus{
+		EventValid: event.Valid(),
+		ReceivedAt: receivedAt,
+		Seen:       true,
+	}
 	return obj, true
 }
 
+func (s *Subscriber[T]) Status() SubscriberStatus {
+	return s.status
+}
+
+func (s *Subscriber[T]) nowTime() time.Time {
+	if s.now != nil {
+		return s.now()
+	}
+	return time.Now()
+}
+
 func NewSubscriber[T any](name string, reader Reader[T], conflate bool, shadow bool) (subscriber Subscriber[T]) {
 	msgq := gomsgq.Msgq{}
 	err := msgq.Init(name, settings.GetSegmentSize(name))
@@ -53,6 +84,7 @@ func NewSubscriber[T any](name string, reader Reader[T], conflate bool, shadow b
 	sub.Init(msgq)
 
 	subscriber.Sub = sub
+	subscriber.now = time.Now
 	subscriber.reader = reader
 	return subscriber
 }

extended_state.go

@@ -41,6 +41,14 @@ func (s *ExtendedState) setPosition(out custom.MapdExtendedOut) {
 }
 
 func (s *ExtendedState) setPath(out custom.MapdExtendedOut) {
+	if !s.state.RouteUsable() {
+		_, err := out.NewPath(0)
+		if err != nil {
+			slog.Warn("failed to create path in extended state")
+		}
+		return
+	}
+
 	nodes := s.state.CurrentWay.Way.Nodes()
 	num_points := len(nodes)
 	all_nodes := [][]m.Position{nodes}

main.go

@@ -12,7 +12,12 @@ import (
 	ms "pfeifer.dev/mapd/settings"
 )
 
-const mapLoadRetryDelay = time.Second
+const (
+	mapLoadRetryDelay    = time.Second
+	carStateTimeout      = 100 * time.Millisecond
+	carStateStaleTimeout = 500 * time.Millisecond
+	modelV2Timeout       = 500 * time.Millisecond
+)
 
 func main() {
 	ms.Settings.Default()                // set defaults so settings not already in param are defaulted
@@ -57,7 +62,17 @@ func main() {
 	defer selfdriveState.Sub.Msgq.Close()
 
 	for {
-		err := state.Send() // send beginning of each loop to ensure it happens at the correct rate
+		now := time.Now()
+		carStatus := car.Status()
+		modelStatus := model.Status()
+		state.CarValid = carStatus.Healthy(now, carStateStaleTimeout)
+		state.GpsValid = gps.Fresh(now)
+		state.ModelValid = modelStatus.Healthy(now, modelV2Timeout)
+		if !state.GpsValid && state.RouteValid {
+			state.ClearRoute()
+		}
+
+		err := state.Send(state.CarValid) // send beginning of each loop to ensure it happens at the correct rate
 		if err != nil {
 			slog.Error("Failed to send update", "error", err)
 		}
@@ -83,13 +98,19 @@ func main() {
 		}
 
 		carData, carStateSuccess := car.Read()
-		if carStateSuccess {
+		if carStateSuccess && car.Status().EventValid {
+			if !carStatus.Healthy(car.Status().ReceivedAt, carStateTimeout) {
+				state.Car.UpdateTime.Rebase()
+			}
 			state.UpdateCarState(carData)
 			UpdateCurveSpeed(&state)
 		}
 
 		modelData, modelSuccess := model.Read()
-		if modelSuccess {
+		if modelSuccess && model.Status().EventValid {
+			if !modelStatus.Healthy(model.Status().ReceivedAt, modelV2Timeout) {
+				state.VisionCurveMA.Reset()
+			}
 			state.VisionCurveSpeed = calcVisionCurveSpeed(modelData, &state)
 		}
 
@@ -110,25 +131,43 @@ func main() {
 				lastMapLoadAttempt = mapLoadTime
 				if err != nil {
 					slog.Debug("", "error", errors.Wrap(err, "Could not find ways around location"))
+					state.MapValid = false
+					state.ClearRoute()
 					continue
 				}
 			}
+			state.MapValid = state.Data.Loaded
+			if !state.MapValid {
+				state.ClearRoute()
+				continue
+			}
 
 			state.CurrentWay, err = GetCurrentWay(state.CurrentWay, state.NextWays, &state.Data, location)
 			if err != nil {
 				slog.Debug("could not get current way", "error", err)
+				state.ClearRoute()
+				continue
 			}
+			state.RouteValid = true
 
 			state.NextWays, err = NextWays(location, state.CurrentWay, &state.Data, state.CurrentWay.OnWay.IsForward)
 			if err != nil {
 				slog.Debug("could not get next way", "error", err)
+				state.NextWays = nil
+				state.SpeedLimit.NextLimit.Reset()
+				state.NextAdvisorySpeed.Reset()
+				state.NextHazard.Reset()
 			}
 
 			state.Curvatures, err = GetStateCurvatures(&state)
 			if err != nil {
 				slog.Debug("could not get curvatures from current state", "error", err)
+				state.Curvatures = nil
+				state.TargetVelocities = nil
+				state.MapCurveSpeed = 0
+			} else {
+				state.TargetVelocities = GetTargetVelocities(state.Curvatures, state.TargetVelocities)
 			}
-			state.TargetVelocities = GetTargetVelocities(state.Curvatures, state.TargetVelocities)
 		}
 
 		// send at beginning of next loop