diff --git a/dist/roverd b/dist/roverd index 4d953b10..3aab6d44 100755 Binary files a/dist/roverd and b/dist/roverd differ diff --git a/dist/servoverifier b/dist/servoverifier index faac16ec..9cea6d1e 100755 Binary files a/dist/servoverifier and b/dist/servoverifier differ diff --git a/pi/roverd/wsclient.go b/pi/roverd/wsclient.go index 32d51c5e..2348294b 100644 --- a/pi/roverd/wsclient.go +++ b/pi/roverd/wsclient.go @@ -22,6 +22,12 @@ type WSClient struct { log *log.Logger } +const ( + sensorSilenceTimeout = 5 * time.Second + sensorRecoveryCooldown = 5 * time.Second + sensorCommandPause = 100 * time.Millisecond +) + func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events chan RoverEvent, media *MediaSupervisor, servo *CameraServo, nightVision *NightVisionLight, logger *log.Logger) *WSClient { return &WSClient{ cfg: cfg, @@ -126,12 +132,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.kickstartSensorStream(sensorCommandPause) } - 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,12 +189,6 @@ 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 - streamCommandPause = 75 * time.Millisecond - ) - timer := time.NewTimer(sensorSilenceTimeout) defer timer.Stop() @@ -225,7 +222,7 @@ func (c *WSClient) forwardSensors(ctx context.Context, conn *websocket.Conn) { } c.log.Printf("no sensor frames for %v; restarting OI and sensor stream", idleFor) - c.recoverSensorStream(idleFor, streamCommandPause) + c.recoverSensorStream(idleFor, sensorCommandPause) lastRecovery = now resetTimer() case frame := <-c.sensorFrames: @@ -298,10 +295,7 @@ func clamp(value, min, max int) int { } func (c *WSClient) ensureSensorStream() error { - if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil { - return err - } - return c.adapter.PauseSensorStream(false) + return c.kickstartSensorStream(sensorCommandPause) } func (c *WSClient) recoverSensorStream(idleFor time.Duration, cmdPause time.Duration) { @@ -309,41 +303,33 @@ func (c *WSClient) recoverSensorStream(idleFor time.Duration, cmdPause time.Dura "idleMs": idleFor.Milliseconds(), }) - if err := c.adapter.PauseSensorStream(true); err != nil { - c.log.Printf("sensor stream pause failed: %v", err) - } - if cmdPause > 0 { - time.Sleep(cmdPause) - } - - if err := c.adapter.StartOI(); err != nil { - c.log.Printf("start OI failed: %v", err) - c.emitEvent("sensorWatchdog.startOI.error", map[string]any{"error": err.Error()}) - } - if cmdPause > 0 { - time.Sleep(cmdPause) - } - - if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil { + if err := c.kickstartSensorStream(cmdPause); err != nil { c.log.Printf("sensor stream restart failed: %v", err) c.emitEvent("sensorWatchdog.streamRestart.error", map[string]any{"error": err.Error()}) return } - if cmdPause > 0 { - time.Sleep(cmdPause) - } - - if err := c.adapter.PauseSensorStream(false); err != nil { - c.log.Printf("sensor stream unpause failed: %v", err) - c.emitEvent("sensorWatchdog.streamRestart.error", map[string]any{"error": err.Error()}) - return - } c.emitEvent("sensorWatchdog.streamRestart.ok", map[string]any{ "idleMs": idleFor.Milliseconds(), }) } +func (c *WSClient) kickstartSensorStream(cmdPause time.Duration) error { + if err := c.adapter.StartOI(); err != nil { + return err + } + if cmdPause > 0 { + time.Sleep(cmdPause) + } + if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil { + return err + } + if cmdPause > 0 { + time.Sleep(cmdPause) + } + return nil +} + func isModeOpcode(op byte) bool { switch op { case 128, 131, 132: @@ -351,4 +337,4 @@ func isModeOpcode(op byte) bool { default: return false } -} +} \ No newline at end of file