diff --git a/dist/roverd b/dist/roverd index a75312a2..850b7117 100755 Binary files a/dist/roverd and b/dist/roverd differ diff --git a/dist/servoverifier b/dist/servoverifier index 0aacbdac..ef6e8c8f 100755 Binary files a/dist/servoverifier and b/dist/servoverifier differ diff --git a/pi/roverd/serial.go b/pi/roverd/serial.go index 22f1f37c..2cc0ad28 100644 --- a/pi/roverd/serial.go +++ b/pi/roverd/serial.go @@ -92,6 +92,10 @@ func (s *SerialAdapter) SendRaw(raw []byte) error { return s.write(raw) } +func (s *SerialAdapter) StartOI() error { + return s.write([]byte{128}) +} + func (s *SerialAdapter) SeekDock() error { return s.write([]byte{143}) } diff --git a/pi/roverd/serial_dummy.go b/pi/roverd/serial_dummy.go index 439fdc74..8b4c421b 100644 --- a/pi/roverd/serial_dummy.go +++ b/pi/roverd/serial_dummy.go @@ -52,6 +52,11 @@ func (s *SerialAdapter) SendRaw(raw []byte) error { return nil } +func (s *SerialAdapter) StartOI() error { + s.log.Printf("[dummy] start OI") + return nil +} + func (s *SerialAdapter) SeekDock() error { s.log.Printf("[dummy] seek dock") return nil diff --git a/pi/roverd/wsclient.go b/pi/roverd/wsclient.go index e1a2a3fb..aed1e1d2 100644 --- a/pi/roverd/wsclient.go +++ b/pi/roverd/wsclient.go @@ -6,6 +6,7 @@ import ( "encoding/json" "fmt" "log" + "sync" "time" "nhooyr.io/websocket" @@ -20,6 +21,8 @@ type WSClient struct { servo *CameraServo nightVision *NightVisionLight log *log.Logger + recoverMu sync.Mutex + recovering bool } func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events chan RoverEvent, media *MediaSupervisor, servo *CameraServo, nightVision *NightVisionLight, logger *log.Logger) *WSClient { @@ -126,12 +129,9 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error { return c.adapter.MotorPWM(main, side, vac) case msg.SensorStream != nil: if msg.SensorStream.Enable { - if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil { - return err - } - return c.adapter.PauseSensorStream(false) + return c.adapter.StartSensorStream(defaultStreamPackets) } - return c.adapter.PauseSensorStream(true) + return nil case msg.Raw != "" && len(msg.Raw) > 0: buf, err := base64.StdEncoding.DecodeString(msg.Raw) if err != nil { @@ -186,11 +186,50 @@ func (c *WSClient) handleServoCommand(payload *servoPayload) error { } func (c *WSClient) forwardSensors(ctx context.Context, conn *websocket.Conn) { + const ( + sensorSilenceTimeout = 5 * time.Second + sensorRecoveryCooldown = 3 * time.Second + sensorCommandPause = 50 * time.Millisecond + ) + + timer := time.NewTimer(sensorSilenceTimeout) + defer timer.Stop() + + resetTimer := func() { + if !timer.Stop() { + select { + case <-timer.C: + default: + } + } + timer.Reset(sensorSilenceTimeout) + } + + lastRecovery := time.Time{} + lastFrame := time.Now() + for { select { case <-ctx.Done(): return + case <-timer.C: + now := time.Now() + if !lastRecovery.IsZero() && now.Sub(lastRecovery) < sensorRecoveryCooldown { + resetTimer() + continue + } + + idleFor := now.Sub(lastFrame) + if idleFor < 0 { + idleFor = sensorSilenceTimeout + } + + c.recoverSensorStream(idleFor, sensorCommandPause) + lastRecovery = now + resetTimer() case frame := <-c.sensorFrames: + lastFrame = time.Now() + resetTimer() msg := sensorMessage{ Type: "sensor", Timestamp: time.Now().UnixMilli(), @@ -261,7 +300,46 @@ func (c *WSClient) ensureSensorStream() error { if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil { return err } - return c.adapter.PauseSensorStream(false) + return nil +} + +func (c *WSClient) recoverSensorStream(idleFor time.Duration, cmdPause time.Duration) { + c.recoverMu.Lock() + if c.recovering { + c.recoverMu.Unlock() + return + } + c.recovering = true + c.recoverMu.Unlock() + + defer func() { + c.recoverMu.Lock() + c.recovering = false + c.recoverMu.Unlock() + }() + + c.emitEvent("sensorWatchdog.restart", map[string]any{ + "idleMs": idleFor.Milliseconds(), + }) + + if err := c.adapter.StartOI(); err != nil { + c.log.Printf("watchdog start OI failed: %v", err) + c.emitEvent("sensorWatchdog.error", map[string]any{"error": err.Error()}) + return + } + if cmdPause > 0 { + time.Sleep(cmdPause) + } + + if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil { + c.log.Printf("watchdog start stream failed: %v", err) + c.emitEvent("sensorWatchdog.error", map[string]any{"error": err.Error()}) + return + } + + c.emitEvent("sensorWatchdog.ok", map[string]any{ + "idleMs": idleFor.Milliseconds(), + }) } func isModeOpcode(op byte) bool {