diff --git a/cereal/gps.go b/cereal/gps.go index 2fd458d..525a5b4 100644 --- a/cereal/gps.go +++ b/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, } } diff --git a/cereal/subscriber.go b/cereal/subscriber.go index 7d68ccb..cc1fad3 100644 --- a/cereal/subscriber.go +++ b/cereal/subscriber.go @@ -2,6 +2,7 @@ package cereal import ( "math" + "time" "capnproto.org/go/capnp/v3" "github.com/pfeiferj/gomsgq" @@ -11,9 +12,22 @@ 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) { @@ -21,6 +35,7 @@ func (s *Subscriber[T]) Read() (obj T, success bool) { 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 } diff --git a/extended_state.go b/extended_state.go index 00befd5..07cd39e 100644 --- a/extended_state.go +++ b/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} diff --git a/main.go b/main.go index e817e80..9192111 100644 --- a/main.go +++ b/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 diff --git a/state.go b/state.go index 1e7915f..f65d7a3 100644 --- a/state.go +++ b/state.go @@ -17,6 +17,11 @@ type State struct { SpeedLimit SpeedLimitState NextWays []maps.NextWayResult Position m.Position + CarValid bool + GpsValid bool + MapValid bool + ModelValid bool + RouteValid bool Curvatures []m.Curvature TargetVelocities []Velocity DistanceSinceLastPosition float32 @@ -44,7 +49,7 @@ func (s *State) SuggestedSpeed() float32 { suggestedSpeed = slSuggestedSpeed } } - if ms.Settings.VisionCurveSpeedControlEnabled && s.VisionCurveSpeed > 0 && (s.VisionCurveSpeed < suggestedSpeed || suggestedSpeed == 0) && (!ms.Settings.VisionCurveUseEnableSpeed || s.Car.EnableSpeedActive) { + if s.ModelValid && ms.Settings.VisionCurveSpeedControlEnabled && s.VisionCurveSpeed > 0 && (s.VisionCurveSpeed < suggestedSpeed || suggestedSpeed == 0) && (!ms.Settings.VisionCurveUseEnableSpeed || s.Car.EnableSpeedActive) { suggestedSpeed = s.VisionCurveSpeed } if ms.Settings.MapCurveSpeedControlEnabled && s.MapCurveSpeed > 0 && (s.MapCurveSpeed < suggestedSpeed || suggestedSpeed == 0) && (!ms.Settings.MapCurveUseEnableSpeed || s.Car.EnableSpeedActive) { @@ -56,6 +61,22 @@ func (s *State) SuggestedSpeed() float32 { return suggestedSpeed } +func (s *State) RouteUsable() bool { + return s.GpsValid && s.MapValid && s.RouteValid +} + +func (s *State) ClearRoute() { + s.RouteValid = false + s.CurrentWay = CurrentWay{} + s.NextWays = nil + s.Curvatures = nil + s.TargetVelocities = nil + s.MapCurveSpeed = 0 + s.SpeedLimit.NextLimit.Reset() + s.NextAdvisorySpeed.Reset() + s.NextHazard.Reset() +} + func (s *State) UpdateCarState(carData car.CarState) { s.Car.Update(carData) s.DistanceSinceLastPosition += float32(s.Car.UpdateTime.DiffMA.Estimate) * s.Car.VEgo @@ -65,8 +86,12 @@ func (s *State) UpdateCarState(carData car.CarState) { s.SpeedLimit.Update(s.CurrentWay, s.Car) } -func (s *State) Send() error { - msg, output := s.Publisher.NewMessage(true) +func (s *State) Send(valid bool) error { + msg, output := s.Publisher.NewMessage(valid) + if !valid { + return s.Publisher.Send(msg) + } + id := s.CurrentWay.Way.Id() output.SetWayId(id) @@ -111,7 +136,9 @@ func (s *State) Send() error { output.SetRoadContext(custom.RoadContext(s.CurrentWay.Way.Context())) output.SetHighwayClass(custom.HighwayClass(s.CurrentWay.Way.HighwayClass())) output.SetEstimatedRoadWidth(s.CurrentWay.Way.Width()) - output.SetVisionCurveSpeed(s.VisionCurveSpeed) + if s.ModelValid { + output.SetVisionCurveSpeed(s.VisionCurveSpeed) + } output.SetMapCurveSpeed(s.MapCurveSpeed) output.SetSuggestedSpeed(s.SuggestedSpeed()) diff --git a/utils/update_tracker.go b/utils/update_tracker.go index 187df85..b9fb4df 100644 --- a/utils/update_tracker.go +++ b/utils/update_tracker.go @@ -10,6 +10,7 @@ type UpdateTracker struct { LastTime time.Time Time time.Time DiffMA m.MovingAverage + skipNext bool } func (u *UpdateTracker) Init(maLength int) { @@ -18,8 +19,19 @@ func (u *UpdateTracker) Init(maLength int) { u.DiffMA.Init(maLength) } +func (u *UpdateTracker) Rebase() { + now := time.Now() + u.LastTime = now + u.Time = now + u.skipNext = true +} + func (u *UpdateTracker) Update() { u.LastTime = u.Time u.Time = time.Now() + if u.skipNext { + u.skipNext = false + return + } u.DiffMA.Update(u.Time.Sub(u.LastTime).Seconds()) }