legop3/MultiRoombaRover
1
0
Fork 0
mirror of https://github.com/legop3/MultiRoombaRover.git synced 2026-09-15 17:12:59 -04:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

better sensor recovery

Browse Source
This commit is contained in:
legop3
2025-12-12 22:50:04 -05:00
parent c07e5971f5
commit 501bc234ee
3 changed files with 42 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dist/roverd
Vendored
BIN
View File
Binary file not shown.
dist/servoverifier
Vendored
BIN
View File
Binary file not shown.
pi/roverd/wsclient.go
+42 -15
View File
@@ -189,6 +189,7 @@ 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)
@@ -224,21 +225,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.emitEvent("sensorWatchdog.restart", map[string]any{
"idleMs": idleFor.Milliseconds(),
})
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 err := c.ensureSensorStream(); err != nil {
c.log.Printf("sensor stream restart failed: %v", err)
c.emitEvent("sensorWatchdog.streamRestart.error", map[string]any{"error": err.Error()})
} else {
c.emitEvent("sensorWatchdog.streamRestart.ok", map[string]any{
"idleMs": idleFor.Milliseconds(),
})
}
c.recoverSensorStream(idleFor, streamCommandPause)
lastRecovery = now
resetTimer()
case frame := <-c.sensorFrames:
@@ -317,6 +304,46 @@ func (c *WSClient) ensureSensorStream() error {
return c.adapter.PauseSensorStream(false)
}
func (c *WSClient) recoverSensorStream(idleFor time.Duration, cmdPause time.Duration) {
c.emitEvent("sensorWatchdog.restart", map[string]any{
"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 {
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 isModeOpcode(op byte) bool {
switch op {
case 128, 131, 132: