mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-11-16 19:52:17 -05:00
move from /pi to /rover because it isnt just pi anymore.
new json config stuff and ui
This commit is contained in:
@@ -0,0 +1,29 @@
|
||||
BIN_DIR ?= ../../dist
|
||||
ARCH ?= amd64
|
||||
GOARCH := $(ARCH)
|
||||
GOARM :=
|
||||
ifneq ($(filter armv6 armv7,$(ARCH)),)
|
||||
GOARCH := arm
|
||||
GOARM := $(patsubst armv%,%,$(ARCH))
|
||||
endif
|
||||
|
||||
.PHONY: build all armv6 armv7 arm64 amd64 dummy verifier-tools clean
|
||||
|
||||
# Binaries are committed to the repository; no release publishing is needed.
|
||||
build:
|
||||
CGO_ENABLED=0 GOOS=linux GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/roverd-linux-$(ARCH) ./cmd/roverd
|
||||
|
||||
all: armv6 armv7 arm64 amd64
|
||||
|
||||
armv6 armv7 arm64 amd64:
|
||||
$(MAKE) build ARCH=$@
|
||||
|
||||
verifier-tools:
|
||||
CGO_ENABLED=0 GOOS=linux GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/servoverifier-linux-$(ARCH) ./cmd/servoverifier
|
||||
CGO_ENABLED=0 GOOS=linux GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/hornverifier-linux-$(ARCH) ./cmd/hornverifier
|
||||
|
||||
dummy:
|
||||
CGO_ENABLED=0 GOOS=linux GOARCH=amd64 go build -tags dummy -o $(BIN_DIR)/roverd-dummy ./cmd/roverd
|
||||
|
||||
clean:
|
||||
rm -f $(BIN_DIR)/roverd-linux-* $(BIN_DIR)/roverd-dummy $(BIN_DIR)/servoverifier-linux-* $(BIN_DIR)/hornverifier-linux-*
|
||||
@@ -0,0 +1,117 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"math"
|
||||
"os/exec"
|
||||
)
|
||||
|
||||
type AudioLevels struct {
|
||||
HornGain float64
|
||||
TTSGain float64
|
||||
ForwardGain float64
|
||||
}
|
||||
|
||||
func clampAudioGain(v float64) float64 {
|
||||
if v < 0 {
|
||||
return 0
|
||||
}
|
||||
if v > 4 {
|
||||
return 4
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
func normalizeAudioLevels(v AudioLevels) AudioLevels {
|
||||
v.HornGain = clampAudioGain(v.HornGain)
|
||||
v.TTSGain = clampAudioGain(v.TTSGain)
|
||||
v.ForwardGain = clampAudioGain(v.ForwardGain)
|
||||
return v
|
||||
}
|
||||
|
||||
func (c *WSClient) getAudioLevels() AudioLevels {
|
||||
c.audioMu.RLock()
|
||||
defer c.audioMu.RUnlock()
|
||||
return c.audioLevels
|
||||
}
|
||||
|
||||
func (c *WSClient) setAudioLevels(next AudioLevels) {
|
||||
normalized := normalizeAudioLevels(next)
|
||||
c.audioMu.Lock()
|
||||
c.audioLevels = normalized
|
||||
c.audioMu.Unlock()
|
||||
c.applyAudioLevelsToMixer(normalized)
|
||||
}
|
||||
|
||||
func (c *WSClient) handleAudioLevels(payload *audioLevelsPayload) error {
|
||||
if payload == nil {
|
||||
return nil
|
||||
}
|
||||
levels := c.getAudioLevels()
|
||||
if payload.HornGain != nil {
|
||||
levels.HornGain = clampAudioGain(*payload.HornGain)
|
||||
}
|
||||
if payload.TTSGain != nil {
|
||||
levels.TTSGain = clampAudioGain(*payload.TTSGain)
|
||||
}
|
||||
if payload.ForwardGain != nil {
|
||||
levels.ForwardGain = clampAudioGain(*payload.ForwardGain)
|
||||
}
|
||||
c.setAudioLevels(levels)
|
||||
return nil
|
||||
}
|
||||
|
||||
func (c *WSClient) applyAudioLevelsToMixer(levels AudioLevels) {
|
||||
c.applyMixerGain("HornMaster", levels.HornGain)
|
||||
c.applyMixerGain("TTSMaster", levels.TTSGain)
|
||||
c.applyMixerGain("ForwardMaster", levels.ForwardGain)
|
||||
}
|
||||
|
||||
func (c *WSClient) applyMixerGain(control string, gain float64) {
|
||||
normalized := clampAudioGain(gain)
|
||||
if normalized <= 0 {
|
||||
if err := c.trySetMixerControl(control, "0%"); err != nil {
|
||||
c.log.Printf("audio-levels: amixer mute %s failed: %v", control, err)
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
// Convert linear gain to dB, matching softvol max_dB=12.0 in /etc/asound.conf.
|
||||
db := 20.0 * math.Log10(normalized)
|
||||
if db > 12.0 {
|
||||
db = 12.0
|
||||
}
|
||||
if db < -60.0 {
|
||||
db = -60.0
|
||||
}
|
||||
|
||||
// amixer treats a leading "-" value as an option; set via percent to avoid getopt ambiguity.
|
||||
percent := int(math.Round((db + 60.0) / 72.0 * 100.0))
|
||||
if percent < 0 {
|
||||
percent = 0
|
||||
}
|
||||
if percent > 100 {
|
||||
percent = 100
|
||||
}
|
||||
percentArg := fmt.Sprintf("%d%%", percent)
|
||||
if err := c.trySetMixerControl(control, percentArg); err != nil {
|
||||
c.log.Printf("audio-levels: amixer set %s=%s failed: %v", control, percentArg, err)
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) trySetMixerControl(control, value string) error {
|
||||
// Prefer the active ALSA default route; fall back to card index for compatibility.
|
||||
candidates := [][]string{
|
||||
{"-q", "-D", "default", "sset", control, value},
|
||||
{"-q", "-c", "0", "sset", control, value},
|
||||
}
|
||||
var lastErr error
|
||||
for _, args := range candidates {
|
||||
out, err := exec.Command("amixer", args...).CombinedOutput()
|
||||
if err == nil {
|
||||
return nil
|
||||
}
|
||||
lastErr = fmt.Errorf("%w (%s)", err, string(out))
|
||||
}
|
||||
return lastErr
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"log"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
const (
|
||||
autoChargeTimeout = 10 * time.Second
|
||||
autoChargeCooldown = 0 * time.Minute
|
||||
sourceHomeBase = 1 << 1
|
||||
)
|
||||
|
||||
type AutoChargeController struct {
|
||||
mu sync.Mutex
|
||||
paused bool
|
||||
adapter *SerialAdapter
|
||||
events chan<- RoverEvent
|
||||
logger *log.Logger
|
||||
timerStart time.Time
|
||||
cooldownUntil time.Time
|
||||
lastState byte
|
||||
lastSources byte
|
||||
}
|
||||
|
||||
func NewAutoChargeController(adapter *SerialAdapter, events chan<- RoverEvent, logger *log.Logger) *AutoChargeController {
|
||||
return &AutoChargeController{
|
||||
adapter: adapter,
|
||||
events: events,
|
||||
logger: logger,
|
||||
}
|
||||
}
|
||||
|
||||
func (a *AutoChargeController) Run(ctx context.Context, samples <-chan SensorSample) {
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case sample := <-samples:
|
||||
a.processSample(sample)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (a *AutoChargeController) processSample(sample SensorSample) {
|
||||
a.mu.Lock()
|
||||
defer a.mu.Unlock()
|
||||
if a.paused {
|
||||
return
|
||||
}
|
||||
now := time.Now()
|
||||
docked := sample.ChargeSources&sourceHomeBase != 0
|
||||
charging := isCharging(sample.ChargingState)
|
||||
|
||||
if !docked || charging {
|
||||
if !a.timerStart.IsZero() {
|
||||
a.emitEvent("autoCharge.timerCleared", map[string]any{
|
||||
"durationMs": time.Since(a.timerStart).Milliseconds(),
|
||||
})
|
||||
}
|
||||
a.timerStart = time.Time{}
|
||||
a.lastState = sample.ChargingState
|
||||
a.lastSources = sample.ChargeSources
|
||||
return
|
||||
}
|
||||
|
||||
// docked but not charging
|
||||
if a.cooldownUntil.After(now) {
|
||||
return
|
||||
}
|
||||
|
||||
if a.timerStart.IsZero() {
|
||||
a.timerStart = now
|
||||
a.emitEvent("autoCharge.timerStarted", map[string]any{
|
||||
"chargingState": sample.ChargingState,
|
||||
})
|
||||
return
|
||||
}
|
||||
|
||||
if now.Sub(a.timerStart) >= autoChargeTimeout {
|
||||
if err := a.adapter.SeekDock(); err != nil {
|
||||
a.emitEvent("autoCharge.seekDockError", map[string]any{"error": err.Error()})
|
||||
} else {
|
||||
a.emitEvent("autoCharge.seekDockIssued", map[string]any{
|
||||
"waitingMs": autoChargeTimeout.Milliseconds(),
|
||||
})
|
||||
}
|
||||
a.timerStart = time.Time{}
|
||||
a.cooldownUntil = now.Add(autoChargeCooldown)
|
||||
}
|
||||
}
|
||||
|
||||
func isCharging(state byte) bool {
|
||||
switch state {
|
||||
case 1, 2, 3, 4:
|
||||
return true
|
||||
default:
|
||||
return false
|
||||
}
|
||||
}
|
||||
|
||||
func (a *AutoChargeController) emitEvent(event string, data map[string]any) {
|
||||
if a.events == nil {
|
||||
return
|
||||
}
|
||||
select {
|
||||
case a.events <- RoverEvent{
|
||||
Type: "event",
|
||||
Event: event,
|
||||
Ts: time.Now().UnixMilli(),
|
||||
Data: data,
|
||||
}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// Pausing waits for any current dock attempt to finish before management stops
|
||||
// motion, preventing the automatic timer from issuing a later movement command.
|
||||
func (a *AutoChargeController) SetPaused(paused bool) {
|
||||
a.mu.Lock()
|
||||
defer a.mu.Unlock()
|
||||
a.paused = paused
|
||||
a.timerStart = time.Time{}
|
||||
}
|
||||
@@ -0,0 +1,87 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
gpiocdev "github.com/warthog618/go-gpiocdev"
|
||||
)
|
||||
|
||||
type BRCPulser struct {
|
||||
cfg BRCConfig
|
||||
logger *log.Logger
|
||||
line *gpiocdev.Line
|
||||
done chan struct{}
|
||||
}
|
||||
|
||||
func NewBRCPulser(cfg BRCConfig, logger *log.Logger) (*BRCPulser, error) {
|
||||
chip := cfg.GPIOChip
|
||||
if chip == "" {
|
||||
chip = "gpiochip0"
|
||||
}
|
||||
|
||||
line, err := gpiocdev.RequestLine(
|
||||
chip,
|
||||
cfg.GPIOPin,
|
||||
gpiocdev.AsOutput(1),
|
||||
gpiocdev.WithConsumer("roverd-brc"),
|
||||
)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
return &BRCPulser{cfg: cfg, logger: logger, line: line, done: make(chan struct{})}, nil
|
||||
}
|
||||
|
||||
func (b *BRCPulser) Close() {
|
||||
// The owner cancels the pulse loop before closing its GPIO line.
|
||||
<-b.done
|
||||
if b.line != nil {
|
||||
_ = b.line.SetValue(1)
|
||||
b.line.Close()
|
||||
}
|
||||
}
|
||||
|
||||
func (b *BRCPulser) Start(ctx context.Context) {
|
||||
go func() {
|
||||
defer close(b.done)
|
||||
ticker := time.NewTicker(b.cfg.PulseEvery.Duration)
|
||||
defer ticker.Stop()
|
||||
|
||||
for {
|
||||
if ctx.Err() != nil {
|
||||
return
|
||||
}
|
||||
b.pulseOnce(ctx)
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-ticker.C:
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (b *BRCPulser) pulseOnce(ctx context.Context) {
|
||||
if b.line == nil {
|
||||
return
|
||||
}
|
||||
if err := b.line.SetValue(0); err != nil {
|
||||
b.logger.Printf("brc pulse low: %v", err)
|
||||
return
|
||||
}
|
||||
// Cancellation ends a pulse promptly before peripheral shutdown releases
|
||||
// the line, even when a long pulse width was configured.
|
||||
timer := time.NewTimer(b.cfg.PulseWidth.Duration)
|
||||
defer timer.Stop()
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
case <-timer.C:
|
||||
}
|
||||
if err := b.line.SetValue(1); err != nil {
|
||||
b.logger.Printf("brc pulse high: %v", err)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"log"
|
||||
)
|
||||
|
||||
type BRCPulser struct{}
|
||||
|
||||
func NewBRCPulser(cfg BRCConfig, logger *log.Logger) (*BRCPulser, error) {
|
||||
logger.Printf("[dummy] BRC configured on pin %d", cfg.GPIOPin)
|
||||
return &BRCPulser{}, nil
|
||||
}
|
||||
|
||||
func (b *BRCPulser) Close() {}
|
||||
|
||||
func (b *BRCPulser) Start(ctx context.Context) {}
|
||||
@@ -0,0 +1,233 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
rpio "github.com/stianeikeland/go-rpio/v4"
|
||||
)
|
||||
|
||||
type CameraServo struct {
|
||||
cfg CameraServoConfig
|
||||
logger *log.Logger
|
||||
pin rpio.Pin
|
||||
mu sync.Mutex
|
||||
currentAngle float64
|
||||
desiredAngle float64
|
||||
lastMove time.Time
|
||||
moving bool
|
||||
stopCh chan struct{}
|
||||
closed bool
|
||||
}
|
||||
|
||||
func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) {
|
||||
if !cfg.Enabled {
|
||||
return nil, fmt.Errorf("camera servo disabled")
|
||||
}
|
||||
if err := rpio.Open(); err != nil {
|
||||
return nil, fmt.Errorf("open gpio: %w", err)
|
||||
}
|
||||
|
||||
pin := rpio.Pin(cfg.Pin)
|
||||
pin.Mode(rpio.Pwm)
|
||||
targetClock := cfg.FreqHz * cfg.CycleLen
|
||||
pin.Freq(targetClock)
|
||||
|
||||
servo := &CameraServo{
|
||||
cfg: cfg,
|
||||
logger: logger,
|
||||
pin: pin,
|
||||
stopCh: make(chan struct{}),
|
||||
}
|
||||
if err := servo.setAngleLocked(cfg.HomeAngle); err != nil {
|
||||
rpio.Close()
|
||||
return nil, err
|
||||
}
|
||||
logger.Printf("camera servo initialized on GPIO %d (%.1f..%.1f deg, %d..%d us, invert=%v)", cfg.Pin, cfg.MinAngle, cfg.MaxAngle, cfg.MinPulseUs, cfg.MaxPulseUs, cfg.Invert)
|
||||
return servo, nil
|
||||
}
|
||||
|
||||
func (s *CameraServo) Close() {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
if s.closed {
|
||||
return
|
||||
}
|
||||
s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle))
|
||||
rpio.Close()
|
||||
if s.stopCh != nil {
|
||||
close(s.stopCh)
|
||||
s.stopCh = nil
|
||||
}
|
||||
s.closed = true
|
||||
}
|
||||
|
||||
func (s *CameraServo) SetAngle(angle float64) error {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
return s.setAngleLocked(angle)
|
||||
}
|
||||
|
||||
func (s *CameraServo) setAngleLocked(angle float64) error {
|
||||
if s.closed {
|
||||
return fmt.Errorf("servo closed")
|
||||
}
|
||||
clamped := clampFloat(angle, s.cfg.MinAngle, s.cfg.MaxAngle)
|
||||
s.desiredAngle = clamped
|
||||
limited := s.rateLimitAngleLocked(clamped)
|
||||
s.applyPulseLocked(s.angleToPulse(limited))
|
||||
s.currentAngle = limited
|
||||
if math.Abs(limited-s.desiredAngle) > servoAngleEpsilon {
|
||||
s.startMoveLoopLocked()
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *CameraServo) Nudge(delta float64) error {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
if delta == 0 {
|
||||
delta = s.cfg.NudgeDegrees
|
||||
}
|
||||
target := s.currentAngle + delta
|
||||
return s.setAngleLocked(target)
|
||||
}
|
||||
|
||||
func (s *CameraServo) SetPulseWidth(micros int) error {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
if s.closed {
|
||||
return fmt.Errorf("servo closed")
|
||||
}
|
||||
if !s.cfg.AllowRawPulse {
|
||||
return fmt.Errorf("raw pulse commands disabled")
|
||||
}
|
||||
if micros <= 0 {
|
||||
return fmt.Errorf("pulse width must be > 0")
|
||||
}
|
||||
clampedPulse := clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs)
|
||||
targetAngle := s.pulseToAngle(clampedPulse)
|
||||
s.desiredAngle = targetAngle
|
||||
limited := s.rateLimitAngleLocked(targetAngle)
|
||||
s.applyPulseLocked(s.angleToPulse(limited))
|
||||
s.currentAngle = limited
|
||||
if math.Abs(limited-s.desiredAngle) > servoAngleEpsilon {
|
||||
s.startMoveLoopLocked()
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *CameraServo) CurrentAngle() float64 {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
return s.currentAngle
|
||||
}
|
||||
|
||||
// Configuration reports the effective public behavior advertised to the
|
||||
// server. The native implementation simply returns its validated YAML config.
|
||||
func (s *CameraServo) Configuration() CameraServoConfig {
|
||||
return s.cfg
|
||||
}
|
||||
|
||||
func (s *CameraServo) BackendDescription() string {
|
||||
return "native GPIO"
|
||||
}
|
||||
|
||||
func (s *CameraServo) applyPulseLocked(micros int) {
|
||||
micros = clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs)
|
||||
s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen))
|
||||
}
|
||||
|
||||
func (s *CameraServo) startMoveLoopLocked() {
|
||||
if s.moving || s.stopCh == nil {
|
||||
return
|
||||
}
|
||||
s.moving = true
|
||||
go func() {
|
||||
ticker := time.NewTicker(servoStepInterval)
|
||||
defer ticker.Stop()
|
||||
for {
|
||||
select {
|
||||
case <-ticker.C:
|
||||
s.mu.Lock()
|
||||
if s.closed {
|
||||
s.moving = false
|
||||
s.mu.Unlock()
|
||||
return
|
||||
}
|
||||
if math.Abs(s.currentAngle-s.desiredAngle) <= servoAngleEpsilon {
|
||||
s.moving = false
|
||||
s.mu.Unlock()
|
||||
return
|
||||
}
|
||||
limited := s.rateLimitAngleLocked(s.desiredAngle)
|
||||
s.applyPulseLocked(s.angleToPulse(limited))
|
||||
s.currentAngle = limited
|
||||
s.mu.Unlock()
|
||||
case <-s.stopCh:
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (s *CameraServo) rateLimitAngleLocked(target float64) float64 {
|
||||
now := time.Now()
|
||||
if s.lastMove.IsZero() {
|
||||
s.lastMove = now
|
||||
}
|
||||
elapsed := now.Sub(s.lastMove).Seconds()
|
||||
if elapsed <= 0 {
|
||||
s.lastMove = now
|
||||
return s.currentAngle
|
||||
}
|
||||
maxElapsed := servoStepInterval.Seconds()
|
||||
if elapsed > maxElapsed {
|
||||
elapsed = maxElapsed
|
||||
}
|
||||
maxDelta := maxServoDegPerSec * elapsed
|
||||
delta := target - s.currentAngle
|
||||
if math.Abs(delta) <= maxDelta {
|
||||
s.lastMove = now
|
||||
return target
|
||||
}
|
||||
if delta > 0 {
|
||||
target = s.currentAngle + maxDelta
|
||||
} else {
|
||||
target = s.currentAngle - maxDelta
|
||||
}
|
||||
s.lastMove = now
|
||||
return target
|
||||
}
|
||||
|
||||
func (s *CameraServo) angleToPulse(angle float64) int {
|
||||
totalRange := s.cfg.MaxAngle - s.cfg.MinAngle
|
||||
if totalRange == 0 {
|
||||
return s.cfg.MinPulseUs
|
||||
}
|
||||
norm := (angle - s.cfg.MinAngle) / totalRange
|
||||
norm = math.Max(0, math.Min(1, norm))
|
||||
if s.cfg.Invert {
|
||||
norm = 1 - norm
|
||||
}
|
||||
pulseRange := s.cfg.MaxPulseUs - s.cfg.MinPulseUs
|
||||
return s.cfg.MinPulseUs + int(math.Round(norm*float64(pulseRange)))
|
||||
}
|
||||
|
||||
func (s *CameraServo) pulseToAngle(pulse int) float64 {
|
||||
pulseRange := s.cfg.MaxPulseUs - s.cfg.MinPulseUs
|
||||
if pulseRange == 0 {
|
||||
return s.cfg.MinAngle
|
||||
}
|
||||
norm := float64(pulse-s.cfg.MinPulseUs) / float64(pulseRange)
|
||||
norm = math.Max(0, math.Min(1, norm))
|
||||
if s.cfg.Invert {
|
||||
norm = 1 - norm
|
||||
}
|
||||
return s.cfg.MinAngle + norm*(s.cfg.MaxAngle-s.cfg.MinAngle)
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
)
|
||||
|
||||
type CameraServo struct{}
|
||||
|
||||
func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) {
|
||||
return nil, fmt.Errorf("camera servo not supported in dummy build")
|
||||
}
|
||||
|
||||
func (c *CameraServo) Close() {}
|
||||
|
||||
func (c *CameraServo) SetAngle(angle float64) error {
|
||||
return fmt.Errorf("camera servo disabled")
|
||||
}
|
||||
|
||||
func (c *CameraServo) Nudge(delta float64) error {
|
||||
return fmt.Errorf("camera servo disabled")
|
||||
}
|
||||
|
||||
func (c *CameraServo) SetPulseWidth(micros int) error {
|
||||
return fmt.Errorf("camera servo disabled")
|
||||
}
|
||||
|
||||
func (c *CameraServo) CurrentAngle() float64 {
|
||||
return 0
|
||||
}
|
||||
|
||||
func (c *CameraServo) Configuration() CameraServoConfig {
|
||||
return CameraServoConfig{}
|
||||
}
|
||||
|
||||
func (c *CameraServo) BackendDescription() string {
|
||||
return "native GPIO"
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"context"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"os"
|
||||
"os/exec"
|
||||
"sync"
|
||||
)
|
||||
|
||||
const chromeTTSDaemonPath = "/usr/local/bin/chromegtts-daemon"
|
||||
|
||||
type chromeTTSDaemon struct {
|
||||
log *log.Logger
|
||||
mu sync.Mutex
|
||||
cmd *exec.Cmd
|
||||
stdin io.WriteCloser
|
||||
scanner *bufio.Scanner
|
||||
}
|
||||
|
||||
func NewChromeTTSDaemon(logger *log.Logger) *chromeTTSDaemon {
|
||||
return &chromeTTSDaemon{log: logger}
|
||||
}
|
||||
|
||||
func (d *chromeTTSDaemon) Start(ctx context.Context) error {
|
||||
d.mu.Lock()
|
||||
defer d.mu.Unlock()
|
||||
return d.startLocked(ctx)
|
||||
}
|
||||
|
||||
func (d *chromeTTSDaemon) Speak(ctx context.Context, text, voice string, pitch, speed float64) error {
|
||||
d.mu.Lock()
|
||||
defer d.mu.Unlock()
|
||||
|
||||
if err := d.startLocked(ctx); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
payload := map[string]any{
|
||||
"text": text,
|
||||
"voice": voice,
|
||||
"pitch": pitch,
|
||||
"speed": speed,
|
||||
}
|
||||
encoded, err := json.Marshal(payload)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
if _, err := d.stdin.Write(append(encoded, '\n')); err != nil {
|
||||
d.stopLocked()
|
||||
return fmt.Errorf("write chromegtts request: %w", err)
|
||||
}
|
||||
|
||||
type result struct {
|
||||
response chromeTTSResponse
|
||||
err error
|
||||
}
|
||||
done := make(chan result, 1)
|
||||
go func(scanner *bufio.Scanner) {
|
||||
var response chromeTTSResponse
|
||||
if !scanner.Scan() {
|
||||
done <- result{err: fmt.Errorf("chromegtts daemon stopped")}
|
||||
return
|
||||
}
|
||||
if err := json.Unmarshal(scanner.Bytes(), &response); err != nil {
|
||||
done <- result{err: fmt.Errorf("decode chromegtts response: %w", err)}
|
||||
return
|
||||
}
|
||||
done <- result{response: response}
|
||||
}(d.scanner)
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
d.stopLocked()
|
||||
return ctx.Err()
|
||||
case res := <-done:
|
||||
if res.err != nil {
|
||||
d.stopLocked()
|
||||
return res.err
|
||||
}
|
||||
if !res.response.OK {
|
||||
return fmt.Errorf("chromegtts failed: %s", res.response.Error)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
}
|
||||
|
||||
func (d *chromeTTSDaemon) Shutdown() {
|
||||
d.mu.Lock()
|
||||
defer d.mu.Unlock()
|
||||
d.stopLocked()
|
||||
}
|
||||
|
||||
type chromeTTSResponse struct {
|
||||
OK bool `json:"ok"`
|
||||
Ready bool `json:"ready,omitempty"`
|
||||
Error string `json:"error,omitempty"`
|
||||
}
|
||||
|
||||
func (d *chromeTTSDaemon) startLocked(ctx context.Context) error {
|
||||
if d.cmd != nil && d.cmd.ProcessState == nil {
|
||||
return nil
|
||||
}
|
||||
|
||||
cmd := exec.Command(chromeTTSDaemonPath)
|
||||
stdin, err := cmd.StdinPipe()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
stdout, err := cmd.StdoutPipe()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
cmd.Stderr = os.Stderr
|
||||
if err := cmd.Start(); err != nil {
|
||||
return fmt.Errorf("start chromegtts daemon: %w", err)
|
||||
}
|
||||
|
||||
scanner := bufio.NewScanner(stdout)
|
||||
var ready chromeTTSResponse
|
||||
if !scanner.Scan() {
|
||||
_ = cmd.Process.Kill()
|
||||
_ = cmd.Wait()
|
||||
return fmt.Errorf("chromegtts daemon exited before ready")
|
||||
}
|
||||
if err := json.Unmarshal(scanner.Bytes(), &ready); err != nil {
|
||||
_ = cmd.Process.Kill()
|
||||
_ = cmd.Wait()
|
||||
return fmt.Errorf("decode chromegtts ready: %w", err)
|
||||
}
|
||||
if !ready.OK {
|
||||
_ = cmd.Process.Kill()
|
||||
_ = cmd.Wait()
|
||||
return fmt.Errorf("chromegtts daemon not ready: %s", ready.Error)
|
||||
}
|
||||
|
||||
d.cmd = cmd
|
||||
d.stdin = stdin
|
||||
d.scanner = scanner
|
||||
if d.log != nil {
|
||||
d.log.Printf("chromegtts daemon started")
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (d *chromeTTSDaemon) stopLocked() {
|
||||
if d.stdin != nil {
|
||||
_ = d.stdin.Close()
|
||||
d.stdin = nil
|
||||
}
|
||||
if d.cmd != nil && d.cmd.ProcessState == nil && d.cmd.Process != nil {
|
||||
_ = d.cmd.Process.Kill()
|
||||
_ = d.cmd.Wait()
|
||||
}
|
||||
d.cmd = nil
|
||||
d.scanner = nil
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
package roverd
|
||||
|
||||
func clampInt(value, min, max int) int {
|
||||
if value < min {
|
||||
return min
|
||||
}
|
||||
if value > max {
|
||||
return max
|
||||
}
|
||||
return value
|
||||
}
|
||||
@@ -0,0 +1,187 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/binary"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
"os/exec"
|
||||
"strconv"
|
||||
"strings"
|
||||
"time"
|
||||
)
|
||||
|
||||
const twoPi = 2 * math.Pi
|
||||
|
||||
func main() {
|
||||
var (
|
||||
device = flag.String("device", "", "ALSA device (empty = default)")
|
||||
rate = flag.Int("rate", 48000, "Sample rate in Hz")
|
||||
channels = flag.Int("channels", 1, "Number of audio channels")
|
||||
duration = flag.Duration("duration", 2*time.Second, "Total horn duration")
|
||||
freqsRaw = flag.String("freqs", "440,550,660", "Comma-separated frequencies in Hz")
|
||||
volume = flag.Float64("volume", 0.25, "Output volume 0.0-1.0")
|
||||
attack = flag.Duration("attack", 20*time.Millisecond, "Attack time")
|
||||
release = flag.Duration("release", 60*time.Millisecond, "Release time")
|
||||
)
|
||||
flag.Parse()
|
||||
|
||||
if *rate <= 0 {
|
||||
log.Fatalf("rate must be > 0 (got %d)", *rate)
|
||||
}
|
||||
if *channels <= 0 {
|
||||
log.Fatalf("channels must be > 0 (got %d)", *channels)
|
||||
}
|
||||
if *duration <= 0 {
|
||||
log.Fatalf("duration must be > 0 (got %s)", *duration)
|
||||
}
|
||||
if *volume <= 0 || *volume > 1.0 {
|
||||
log.Fatalf("volume must be within (0,1] (got %.3f)", *volume)
|
||||
}
|
||||
if *attack < 0 || *release < 0 {
|
||||
log.Fatalf("attack/release must be >= 0")
|
||||
}
|
||||
|
||||
freqs, err := parseFreqs(*freqsRaw)
|
||||
if err != nil {
|
||||
log.Fatalf("parse freqs: %v", err)
|
||||
}
|
||||
if len(freqs) == 0 {
|
||||
log.Fatal("no frequencies provided")
|
||||
}
|
||||
|
||||
if *attack+*release > *duration {
|
||||
log.Fatalf("attack+release must be <= duration (%s + %s > %s)", *attack, *release, *duration)
|
||||
}
|
||||
|
||||
args := []string{"-q", "-f", "S16_LE", "-c", fmt.Sprintf("%d", *channels), "-r", fmt.Sprintf("%d", *rate), "-t", "raw"}
|
||||
if *device != "" {
|
||||
args = append(args, "-D", *device)
|
||||
}
|
||||
cmd := exec.Command("aplay", args...)
|
||||
stdin, err := cmd.StdinPipe()
|
||||
if err != nil {
|
||||
log.Fatalf("aplay stdin: %v", err)
|
||||
}
|
||||
if err := cmd.Start(); err != nil {
|
||||
log.Fatalf("start aplay: %v", err)
|
||||
}
|
||||
|
||||
writer := bufio.NewWriterSize(stdin, 32*1024)
|
||||
if err := synthChord(writer, freqs, *rate, *channels, *duration, *volume, *attack, *release); err != nil {
|
||||
_ = stdin.Close()
|
||||
_ = cmd.Wait()
|
||||
log.Fatalf("synth: %v", err)
|
||||
}
|
||||
if err := writer.Flush(); err != nil {
|
||||
_ = stdin.Close()
|
||||
_ = cmd.Wait()
|
||||
log.Fatalf("flush: %v", err)
|
||||
}
|
||||
if err := stdin.Close(); err != nil {
|
||||
_ = cmd.Wait()
|
||||
log.Fatalf("close stdin: %v", err)
|
||||
}
|
||||
if err := cmd.Wait(); err != nil {
|
||||
log.Fatalf("aplay failed: %v", err)
|
||||
}
|
||||
log.Print("Horn verification complete")
|
||||
}
|
||||
|
||||
func parseFreqs(raw string) ([]float64, error) {
|
||||
trimmed := strings.TrimSpace(raw)
|
||||
if trimmed == "" {
|
||||
return nil, nil
|
||||
}
|
||||
parts := strings.Split(trimmed, ",")
|
||||
freqs := make([]float64, 0, len(parts))
|
||||
for _, part := range parts {
|
||||
part = strings.TrimSpace(part)
|
||||
if part == "" {
|
||||
continue
|
||||
}
|
||||
value, err := strconv.ParseFloat(part, 64)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("invalid freq %q", part)
|
||||
}
|
||||
if value <= 0 {
|
||||
return nil, fmt.Errorf("freq must be > 0 (got %.3f)", value)
|
||||
}
|
||||
freqs = append(freqs, value)
|
||||
}
|
||||
return freqs, nil
|
||||
}
|
||||
|
||||
func synthChord(writer *bufio.Writer, freqs []float64, rate, channels int, duration time.Duration, volume float64, attack, release time.Duration) error {
|
||||
totalFrames := int(float64(rate) * duration.Seconds())
|
||||
if totalFrames <= 0 {
|
||||
return fmt.Errorf("duration too short")
|
||||
}
|
||||
|
||||
phase := make([]float64, len(freqs))
|
||||
increment := make([]float64, len(freqs))
|
||||
for i, f := range freqs {
|
||||
increment[i] = twoPi * f / float64(rate)
|
||||
}
|
||||
|
||||
attackFrames := int(float64(rate) * attack.Seconds())
|
||||
releaseFrames := int(float64(rate) * release.Seconds())
|
||||
steadyFrames := totalFrames - attackFrames - releaseFrames
|
||||
|
||||
framesPerChunk := 512
|
||||
buf := make([]byte, framesPerChunk*channels*2)
|
||||
sampleIndex := 0
|
||||
scale := volume / float64(len(freqs))
|
||||
|
||||
for framesLeft := totalFrames; framesLeft > 0; {
|
||||
framesNow := framesPerChunk
|
||||
if framesLeft < framesNow {
|
||||
framesNow = framesLeft
|
||||
}
|
||||
for i := 0; i < framesNow; i++ {
|
||||
env := envelope(sampleIndex, attackFrames, steadyFrames, releaseFrames)
|
||||
sample := 0.0
|
||||
for j := range freqs {
|
||||
sample += sawFromPhase(phase[j])
|
||||
phase[j] += increment[j]
|
||||
if phase[j] > twoPi {
|
||||
phase[j] -= twoPi
|
||||
}
|
||||
}
|
||||
sample *= scale * env
|
||||
if sample > 1.0 {
|
||||
sample = 1.0
|
||||
} else if sample < -1.0 {
|
||||
sample = -1.0
|
||||
}
|
||||
intSample := int16(sample * math.MaxInt16)
|
||||
offset := i * channels * 2
|
||||
for ch := 0; ch < channels; ch++ {
|
||||
binary.LittleEndian.PutUint16(buf[offset+ch*2:], uint16(intSample))
|
||||
}
|
||||
sampleIndex++
|
||||
}
|
||||
if _, err := writer.Write(buf[:framesNow*channels*2]); err != nil {
|
||||
return err
|
||||
}
|
||||
framesLeft -= framesNow
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func envelope(sampleIndex, attackFrames, steadyFrames, releaseFrames int) float64 {
|
||||
if attackFrames > 0 && sampleIndex < attackFrames {
|
||||
return float64(sampleIndex) / float64(attackFrames)
|
||||
}
|
||||
if releaseFrames > 0 && sampleIndex >= attackFrames+steadyFrames {
|
||||
relIndex := sampleIndex - (attackFrames + steadyFrames)
|
||||
return float64(releaseFrames-relIndex) / float64(releaseFrames)
|
||||
}
|
||||
return 1.0
|
||||
}
|
||||
|
||||
func sawFromPhase(phase float64) float64 {
|
||||
return 2.0*(phase/twoPi) - 1.0
|
||||
}
|
||||
@@ -0,0 +1,151 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
roverd "multiroombarover/rover/roverd"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var portName string
|
||||
var baud int
|
||||
var timeout time.Duration
|
||||
var startupWait time.Duration
|
||||
var controlID string
|
||||
var rawValue string
|
||||
|
||||
flag.StringVar(&portName, "port", "", "serial device, for example /dev/ttyUSB0 or /dev/ttyACM0")
|
||||
flag.IntVar(&baud, "baud", 115200, "Firmata serial baud rate")
|
||||
flag.DurationVar(&timeout, "timeout", 5*time.Second, "timeout for each Firmata response")
|
||||
flag.DurationVar(&startupWait, "startup-wait", 2*time.Second, "time allowed for boards that reset when the port opens")
|
||||
flag.StringVar(&controlID, "control", "", "optional declared control ID to exercise")
|
||||
flag.StringVar(&rawValue, "value", "", "JSON value for -control, such as 90, true, or \"hello\"")
|
||||
flag.Parse()
|
||||
|
||||
if portName == "" {
|
||||
log.Fatal("-port is required")
|
||||
}
|
||||
if (controlID == "") != (rawValue == "") {
|
||||
log.Fatal("-control and -value must be provided together")
|
||||
}
|
||||
|
||||
port, err := serial.OpenPort(&serial.Config{
|
||||
Name: portName,
|
||||
Baud: baud,
|
||||
ReadTimeout: 100 * time.Millisecond,
|
||||
})
|
||||
if err != nil {
|
||||
log.Fatalf("open %s: %v", portName, err)
|
||||
}
|
||||
defer port.Close()
|
||||
|
||||
// CH340 and native-USB development boards may reset when the host opens the
|
||||
// port. Waiting here makes the same probe work with both connection styles
|
||||
// without baking that diagnostic delay into the production Firmata client.
|
||||
time.Sleep(startupWait)
|
||||
|
||||
rootContext, cancelRoot := context.WithCancel(context.Background())
|
||||
defer cancelRoot()
|
||||
client := roverd.NewFirmataClient(port)
|
||||
client.Start(rootContext)
|
||||
|
||||
firmware, err := withTimeout(timeout, client.QueryFirmware)
|
||||
if err != nil {
|
||||
log.Fatalf("query firmware: %v", err)
|
||||
}
|
||||
fmt.Printf("Firmata firmware: %s %d.%d\n", firmware.Name, firmware.Major, firmware.Minor)
|
||||
|
||||
capabilities, err := withTimeout(timeout, client.QueryCapabilities)
|
||||
if err != nil {
|
||||
log.Fatalf("query capabilities: %v", err)
|
||||
}
|
||||
fmt.Printf("Firmata pins described: %d\n", len(capabilities))
|
||||
|
||||
description, err := withTimeout(timeout, client.Describe)
|
||||
if err != nil {
|
||||
log.Fatalf("describe rover peripheral: %v", err)
|
||||
}
|
||||
formatted, err := json.MarshalIndent(description, "", " ")
|
||||
if err != nil {
|
||||
log.Fatalf("format description: %v", err)
|
||||
}
|
||||
fmt.Printf("Peripheral description:\n%s\n", formatted)
|
||||
|
||||
if controlID != "" {
|
||||
if err := exerciseControl(client, description, controlID, json.RawMessage(rawValue)); err != nil {
|
||||
log.Fatalf("exercise control %q: %v", controlID, err)
|
||||
}
|
||||
fmt.Fprintf(os.Stdout, "Control %q accepted.\n", controlID)
|
||||
}
|
||||
}
|
||||
|
||||
// withTimeout gives every boot-time exchange its own deadline. A missing board
|
||||
// therefore reports the exact handshake stage that failed instead of consuming
|
||||
// one shared timeout and obscuring which response was absent.
|
||||
func withTimeout[T any](timeout time.Duration, operation func(context.Context) (T, error)) (T, error) {
|
||||
ctx, cancel := context.WithTimeout(context.Background(), timeout)
|
||||
defer cancel()
|
||||
return operation(ctx)
|
||||
}
|
||||
|
||||
func exerciseControl(client *roverd.FirmataClient, description roverd.PeripheralDescription, controlID string, rawValue json.RawMessage) error {
|
||||
var selected *roverd.PeripheralControl
|
||||
for index := range description.Controls {
|
||||
if description.Controls[index].ID == controlID {
|
||||
selected = &description.Controls[index]
|
||||
break
|
||||
}
|
||||
}
|
||||
if selected == nil {
|
||||
return errors.New("control is not present in the device description")
|
||||
}
|
||||
|
||||
var value any
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return fmt.Errorf("parse -value as JSON: %w", err)
|
||||
}
|
||||
|
||||
// Standard outputs deliberately use standard Firmata commands. Only custom
|
||||
// callbacks use the rover-peripheral CONTROL operation, which is the central
|
||||
// distinction the probe is intended to validate on real hardware.
|
||||
switch selected.Output.Type {
|
||||
case "custom":
|
||||
return client.SendPeripheralControl(selected.ID, value)
|
||||
case "digital":
|
||||
enabled, ok := value.(bool)
|
||||
if !ok {
|
||||
return errors.New("digital control value must be true or false")
|
||||
}
|
||||
if selected.Output.ActiveLow {
|
||||
enabled = !enabled
|
||||
}
|
||||
if err := client.SetPinMode(byte(*selected.Output.Pin), roverd.FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
return client.SetDigitalPin(byte(*selected.Output.Pin), enabled)
|
||||
case "pwm", "servo":
|
||||
number, ok := value.(float64)
|
||||
if !ok || number != float64(int(number)) {
|
||||
return errors.New("PWM and servo control values must be whole numbers")
|
||||
}
|
||||
mode := roverd.FirmataPinModePWM
|
||||
if selected.Output.Type == "servo" {
|
||||
mode = roverd.FirmataPinModeServo
|
||||
}
|
||||
if err := client.SetPinMode(byte(*selected.Output.Pin), mode); err != nil {
|
||||
return err
|
||||
}
|
||||
return client.ExtendedAnalog(byte(*selected.Output.Pin), int(number))
|
||||
default:
|
||||
return fmt.Errorf("unsupported output %q", selected.Output.Type)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,177 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"flag"
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"os/signal"
|
||||
"sync/atomic"
|
||||
"syscall"
|
||||
"time"
|
||||
|
||||
roverd "multiroombarover/rover/roverd"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var cfgPath string
|
||||
flag.StringVar(&cfgPath, "config", "/etc/roverd.json", "path to roverd JSON configuration")
|
||||
printDefaults := flag.Bool("print-default-config", false, "print a complete initial JSON configuration and exit")
|
||||
printSchema := flag.Bool("print-config-schema", false, "print the configuration JSON Schema and exit")
|
||||
checkConfig := flag.Bool("check-config", false, "validate configuration without opening hardware or connecting")
|
||||
flag.Parse()
|
||||
if *printDefaults {
|
||||
data, _ := json.MarshalIndent(roverd.DefaultConfig(), "", " ")
|
||||
fmt.Println(string(data))
|
||||
return
|
||||
}
|
||||
if *printSchema {
|
||||
fmt.Println(string(roverd.ConfigurationSchema()))
|
||||
return
|
||||
}
|
||||
cfg, err := roverd.LoadConfig(cfgPath)
|
||||
if err != nil {
|
||||
log.Fatalf("load config: %v", err)
|
||||
}
|
||||
if *checkConfig {
|
||||
fmt.Println("Configuration valid")
|
||||
return
|
||||
}
|
||||
if run(cfg, cfgPath) {
|
||||
executable, err := os.Executable()
|
||||
if err != nil {
|
||||
log.Fatal(err)
|
||||
}
|
||||
if err := syscall.Exec(executable, os.Args, os.Environ()); err != nil {
|
||||
log.Fatalf("restart roverd: %v", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func run(cfg *roverd.Config, cfgPath string) bool {
|
||||
ctx, cancel := signal.NotifyContext(context.Background(), syscall.SIGINT, syscall.SIGTERM)
|
||||
defer cancel()
|
||||
logger := log.New(os.Stdout, "roverd: ", log.LstdFlags|log.Lmicroseconds|log.LUTC)
|
||||
events := make(chan roverd.RoverEvent, 128)
|
||||
console := roverd.NewConsoleNotifier(logger)
|
||||
console.SetEvents(events)
|
||||
var restarting atomic.Bool
|
||||
management := roverd.NewManagementClient(cfg, cfgPath, events, logger, func() { restarting.Store(true); cancel() })
|
||||
managementDone := make(chan struct{})
|
||||
go func() { defer close(managementDone); management.Run(ctx) }()
|
||||
// Hardware failures leave management connected until a corrected config
|
||||
// requests restart. runHardware unwinds its own resources on failure.
|
||||
media := roverd.NewMediaSupervisor(cfg.Media, logger)
|
||||
management.SetMedia(media)
|
||||
if media != nil {
|
||||
media.Start(ctx)
|
||||
}
|
||||
if err := runHardware(ctx, cfg, events, management, media, logger, console); err != nil {
|
||||
console.Notify(fmt.Sprintf("Hardware unavailable: %v. Management remains connected.", err))
|
||||
}
|
||||
<-ctx.Done()
|
||||
<-managementDone
|
||||
return restarting.Load()
|
||||
}
|
||||
|
||||
func runHardware(ctx context.Context, cfg *roverd.Config, eventStream chan roverd.RoverEvent, management *roverd.ManagementClient, mediaSupervisor *roverd.MediaSupervisor, logger *log.Logger, console *roverd.ConsoleNotifier) error {
|
||||
ctx, cancel := context.WithCancel(ctx)
|
||||
defer cancel()
|
||||
serialPort, err := roverd.OpenSerial(cfg.Serial)
|
||||
if err != nil {
|
||||
return fmt.Errorf("open serial: %w", err)
|
||||
}
|
||||
defer serialPort.Close()
|
||||
|
||||
// Peripheral discovery is intentionally a boot-time operation. The manager
|
||||
// keeps successful USB ports open across server WebSocket reconnects and is
|
||||
// rebuilt only when the roverd process itself restarts.
|
||||
peripherals, err := roverd.DiscoverPeripheralManager(ctx, cfg.Serial.Device, logger)
|
||||
if err != nil {
|
||||
console.Notify(fmt.Sprintf("Rover peripheral startup failed: %v", err))
|
||||
return fmt.Errorf("discover rover peripherals: %w", err)
|
||||
}
|
||||
defer peripherals.Close()
|
||||
|
||||
sensorFrames := make(chan []byte, 8)
|
||||
sensorSamples := make(chan roverd.SensorSample, 8)
|
||||
|
||||
streamer := roverd.NewSensorStreamer(serialPort, sensorFrames, sensorSamples, logger)
|
||||
go streamer.Run(ctx)
|
||||
|
||||
adapter := roverd.NewSerialAdapter(serialPort, logger)
|
||||
|
||||
// Resolve roles across external and internal peripherals without transport priority.
|
||||
hardwareControllers, err := roverd.ResolveRoverHardwareControllers(cfg, peripherals, logger)
|
||||
if err != nil {
|
||||
console.Notify(fmt.Sprintf("Rover peripheral startup failed while selecting hardware: %v", err))
|
||||
return fmt.Errorf("resolve rover hardware controllers: %w", err)
|
||||
}
|
||||
defer hardwareControllers.Close()
|
||||
if err := peripherals.StartInternal(ctx); err != nil {
|
||||
logger.Printf("internal peripheral: %v", err)
|
||||
console.Notify(fmt.Sprintf("Pi GPIO background function unavailable: %v", err))
|
||||
}
|
||||
for _, message := range peripherals.StartupBroadcasts() {
|
||||
console.Notify(message)
|
||||
}
|
||||
|
||||
for _, message := range hardwareControllers.StartupBroadcasts() {
|
||||
console.Notify(message)
|
||||
}
|
||||
|
||||
// A peripheral is never hot-reconnected. Report the first terminal serial
|
||||
// failure for each discovered board and tell the local operator exactly what
|
||||
// recovery action the fixed boot-time lifecycle requires.
|
||||
go func() {
|
||||
for {
|
||||
select {
|
||||
case failure := <-peripherals.Failures():
|
||||
console.Notify(fmt.Sprintf(
|
||||
"Rover peripheral %q (%s) disconnected: %v. Reconnect it and restart roverd.",
|
||||
failure.Name,
|
||||
failure.ID,
|
||||
failure.Err,
|
||||
))
|
||||
case <-ctx.Done():
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
|
||||
autoCharge := roverd.NewAutoChargeController(adapter, eventStream, logger)
|
||||
|
||||
client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, hardwareControllers.CameraServo, hardwareControllers.Headlight, hardwareControllers.Laser, peripherals, logger, console)
|
||||
client.SetAutoCharge(autoCharge)
|
||||
go autoCharge.Run(ctx, sensorSamples)
|
||||
if !management.SetRuntime(client, mediaSupervisor) {
|
||||
return nil
|
||||
}
|
||||
defer client.PrepareShutdown()
|
||||
|
||||
// The control loop is ready after startup resolution. Individual unavailable
|
||||
// peripheral roles have already been reported and do not prevent connection.
|
||||
console.Notify("roverd started and hardware initialization completed.")
|
||||
defer console.Notify("roverd stopped.")
|
||||
|
||||
retryDelay := time.Second
|
||||
for ctx.Err() == nil {
|
||||
if err := client.Run(ctx); err != nil {
|
||||
logger.Printf("websocket loop ended: %v", err)
|
||||
}
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return nil
|
||||
case <-time.After(retryDelay):
|
||||
}
|
||||
|
||||
if retryDelay < 30*time.Second {
|
||||
retryDelay *= 2
|
||||
}
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"flag"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
rpio "github.com/stianeikeland/go-rpio/v4"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var (
|
||||
pinNum = flag.Int("pin", 19, "BCM pin connected to the servo signal line")
|
||||
freqHz = flag.Int("freq", 50, "Servo PWM frequency in Hz")
|
||||
cycleLen = flag.Int("cycle", 20000, "PWM cycle length (counts per period)")
|
||||
minPulse = flag.Int("min", 900, "Minimum pulse width in microseconds")
|
||||
maxPulse = flag.Int("max", 2100, "Maximum pulse width in microseconds")
|
||||
stepPulse = flag.Int("step", 100, "Pulse width increment in microseconds when sweeping")
|
||||
sweeps = flag.Int("sweeps", 2, "How many full min→max→min sweeps to perform")
|
||||
pause = flag.Duration("pause", 150*time.Millisecond, "Delay between pulse adjustments")
|
||||
holdPulse = flag.Int("hold", 0, "Pulse width to hold before exiting (0 = midpoint of min/max)")
|
||||
)
|
||||
flag.Parse()
|
||||
|
||||
if *freqHz <= 0 || *cycleLen <= 0 {
|
||||
log.Fatalf("invalid freq (%d) or cycle (%d)", *freqHz, *cycleLen)
|
||||
}
|
||||
if *minPulse <= 0 || *maxPulse <= 0 || *minPulse >= *maxPulse {
|
||||
log.Fatalf("invalid min/max pulses (%d/%d)", *minPulse, *maxPulse)
|
||||
}
|
||||
if *stepPulse <= 0 {
|
||||
log.Fatalf("step must be > 0 (got %d)", *stepPulse)
|
||||
}
|
||||
if *pause <= 0 {
|
||||
log.Fatalf("pause must be > 0 (got %s)", pause)
|
||||
}
|
||||
if *sweeps < 0 {
|
||||
log.Fatalf("sweeps must be >= 0 (got %d)", *sweeps)
|
||||
}
|
||||
|
||||
if err := rpio.Open(); err != nil {
|
||||
log.Fatalf("open gpio: %v", err)
|
||||
}
|
||||
defer rpio.Close()
|
||||
|
||||
pin := rpio.Pin(*pinNum)
|
||||
pin.Mode(rpio.Pwm)
|
||||
|
||||
targetClock := *freqHz * *cycleLen
|
||||
pin.Freq(targetClock)
|
||||
log.Printf("Configured PWM pin %d at %d Hz (clock=%d Hz, cycle=%d)", *pinNum, *freqHz, targetClock, *cycleLen)
|
||||
|
||||
setPulse := func(us int) {
|
||||
clamped := clamp(us, *minPulse, *maxPulse)
|
||||
pin.DutyCycle(uint32(clamped), uint32(*cycleLen))
|
||||
log.Printf("pulse -> %dµs", clamped)
|
||||
}
|
||||
|
||||
mid := (*minPulse + *maxPulse) / 2
|
||||
setPulse(mid)
|
||||
|
||||
runSweep := func() {
|
||||
for pulse := *minPulse; pulse <= *maxPulse; pulse += *stepPulse {
|
||||
setPulse(pulse)
|
||||
time.Sleep(*pause)
|
||||
}
|
||||
for pulse := *maxPulse - *stepPulse; pulse >= *minPulse; pulse -= *stepPulse {
|
||||
setPulse(pulse)
|
||||
time.Sleep(*pause)
|
||||
}
|
||||
}
|
||||
|
||||
for i := 0; i < *sweeps; i++ {
|
||||
log.Printf("Sweep %d/%d", i+1, *sweeps)
|
||||
runSweep()
|
||||
}
|
||||
|
||||
finalPulse := *holdPulse
|
||||
if finalPulse <= 0 {
|
||||
finalPulse = mid
|
||||
}
|
||||
setPulse(finalPulse)
|
||||
log.Printf("Holding at %dµs", clamp(finalPulse, *minPulse, *maxPulse))
|
||||
log.Print("Servo verification complete")
|
||||
}
|
||||
|
||||
func clamp(value, min, max int) int {
|
||||
if value < min {
|
||||
return min
|
||||
}
|
||||
if value > max {
|
||||
return max
|
||||
}
|
||||
return value
|
||||
}
|
||||
@@ -0,0 +1,132 @@
|
||||
package roverd
|
||||
|
||||
import "encoding/json"
|
||||
|
||||
type helloMessage struct {
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Description string `json:"description,omitempty"`
|
||||
Color string `json:"color,omitempty"`
|
||||
Battery BatteryConfig `json:"battery"`
|
||||
MaxWheelSpeed int `json:"maxWheelSpeed"`
|
||||
Media any `json:"media"`
|
||||
CameraServo CameraServoConfig `json:"cameraServo"`
|
||||
Audio AudioConfig `json:"audio"`
|
||||
Horn HornConfig `json:"horn"`
|
||||
Headlight GPIOToggleConfig `json:"headlight"`
|
||||
Laser GPIOToggleConfig `json:"laser"`
|
||||
Peripherals []RoverPeripheralMetadata `json:"peripherals,omitempty"`
|
||||
Private PrivateConfig `json:"private"`
|
||||
}
|
||||
|
||||
type sensorMessage struct {
|
||||
Type string `json:"type"`
|
||||
Timestamp int64 `json:"ts"`
|
||||
Data string `json:"data"`
|
||||
}
|
||||
|
||||
// hostStatsMessage is intentionally separate from sensorMessage because sensor
|
||||
// frames are raw Roomba Open Interface data, while these values describe the
|
||||
// Raspberry Pi host that is running roverd.
|
||||
type hostStatsMessage struct {
|
||||
Type string `json:"type"`
|
||||
Timestamp int64 `json:"ts"`
|
||||
Stats HostStats `json:"stats"`
|
||||
}
|
||||
|
||||
type inboundMessage struct {
|
||||
Type string `json:"type"`
|
||||
ID string `json:"id"`
|
||||
DriveDirect *driveDirectPayload `json:"driveDirect,omitempty"`
|
||||
MotorPWM *motorPWMPayload `json:"motorPwm,omitempty"`
|
||||
Raw string `json:"raw,omitempty"`
|
||||
SensorStream *sensorStreamPayload `json:"sensorStream,omitempty"`
|
||||
Servo *servoPayload `json:"servo,omitempty"`
|
||||
TTS *ttsPayload `json:"tts,omitempty"`
|
||||
Horn *hornPayload `json:"horn,omitempty"`
|
||||
AudioLevels *audioLevelsPayload `json:"audioLevels,omitempty"`
|
||||
Headlight *togglePayload `json:"headlight,omitempty"`
|
||||
Laser *togglePayload `json:"laser,omitempty"`
|
||||
Peripheral *peripheralPayload `json:"peripheral,omitempty"`
|
||||
Song *songPayload `json:"song,omitempty"`
|
||||
}
|
||||
|
||||
type driveDirectPayload struct {
|
||||
Left int `json:"left"`
|
||||
Right int `json:"right"`
|
||||
}
|
||||
|
||||
type motorPWMPayload struct {
|
||||
Main int `json:"main"`
|
||||
Side int `json:"side"`
|
||||
Vacuum int `json:"vacuum"`
|
||||
}
|
||||
|
||||
type sensorStreamPayload struct {
|
||||
Enable bool `json:"enable"`
|
||||
}
|
||||
|
||||
type mediaCommand struct {
|
||||
Action string `json:"action"`
|
||||
}
|
||||
|
||||
type servoPayload struct {
|
||||
Angle *float64 `json:"angle,omitempty"`
|
||||
Nudge *float64 `json:"nudge,omitempty"`
|
||||
PulseUs *int `json:"pulseUs,omitempty"`
|
||||
}
|
||||
|
||||
type ttsPayload struct {
|
||||
Text string `json:"text"`
|
||||
Engine string `json:"engine,omitempty"`
|
||||
Voice string `json:"voice,omitempty"`
|
||||
Pitch float64 `json:"pitch,omitempty"`
|
||||
Speed float64 `json:"speed,omitempty"`
|
||||
Speak bool `json:"speak,omitempty"`
|
||||
}
|
||||
|
||||
type hornPayload struct {
|
||||
Action string `json:"action"`
|
||||
Waveform string `json:"waveform,omitempty"`
|
||||
Freqs []float64 `json:"freqs,omitempty"`
|
||||
}
|
||||
|
||||
type audioLevelsPayload struct {
|
||||
HornGain *float64 `json:"hornGain,omitempty"`
|
||||
TTSGain *float64 `json:"ttsGain,omitempty"`
|
||||
ForwardGain *float64 `json:"forwardGain,omitempty"`
|
||||
}
|
||||
|
||||
type togglePayload struct {
|
||||
Action string `json:"action"`
|
||||
}
|
||||
|
||||
type peripheralPayload struct {
|
||||
ID string `json:"id"`
|
||||
Control string `json:"control"`
|
||||
Value json.RawMessage `json:"value"`
|
||||
}
|
||||
|
||||
type songPayload struct {
|
||||
Slot *int `json:"slot,omitempty"`
|
||||
Notes []songNote `json:"notes"`
|
||||
Loop bool `json:"loop,omitempty"`
|
||||
}
|
||||
|
||||
type songNote struct {
|
||||
Note int `json:"note"`
|
||||
Duration int `json:"duration"`
|
||||
}
|
||||
|
||||
type rebootPayload struct {
|
||||
DelayMs int `json:"delayMs,omitempty"`
|
||||
}
|
||||
|
||||
type updatePayload struct{}
|
||||
|
||||
type ackMessage struct {
|
||||
Type string `json:"type"`
|
||||
ID string `json:"id"`
|
||||
Status string `json:"status"`
|
||||
Error string `json:"error,omitempty"`
|
||||
}
|
||||
@@ -0,0 +1,678 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"fmt"
|
||||
"net"
|
||||
"net/url"
|
||||
"os"
|
||||
"regexp"
|
||||
"strconv"
|
||||
"strings"
|
||||
"time"
|
||||
|
||||
"bytes"
|
||||
"encoding/json"
|
||||
"io"
|
||||
)
|
||||
|
||||
type SerialConfig struct {
|
||||
Device string `json:"device"`
|
||||
Baud int `json:"baud" jsonschema:"minimum=1"`
|
||||
}
|
||||
|
||||
type Duration struct {
|
||||
time.Duration
|
||||
}
|
||||
|
||||
func (d *Duration) UnmarshalJSON(data []byte) error {
|
||||
var raw string
|
||||
if err := json.Unmarshal(data, &raw); err != nil {
|
||||
return err
|
||||
}
|
||||
parsed, err := time.ParseDuration(raw)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
d.Duration = parsed
|
||||
return nil
|
||||
}
|
||||
|
||||
func (d Duration) MarshalJSON() ([]byte, error) { return json.Marshal(d.Duration.String()) }
|
||||
|
||||
type BRCConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
GPIOPin int `json:"gpioPin"`
|
||||
GPIOChip string `json:"gpioChip"`
|
||||
PulseEvery Duration `json:"pulseEvery"`
|
||||
PulseWidth Duration `json:"pulseWidth"`
|
||||
}
|
||||
|
||||
type BatteryConfig struct {
|
||||
Full int `json:"full"`
|
||||
Warn int `json:"warn"`
|
||||
Urgent int `json:"urgent"`
|
||||
}
|
||||
|
||||
type AudioConfig struct {
|
||||
// AudioConfig is only for sounds generated by roverd itself. Live capture
|
||||
// and playback are media-publisher concerns, so those settings live under
|
||||
// MediaConfig where they can be written directly into media.env.
|
||||
TTSEnabled bool `json:"ttsEnabled"`
|
||||
DefaultEngine string `json:"defaultEngine"`
|
||||
DefaultVoice string `json:"defaultVoice"`
|
||||
DefaultPitch int `json:"defaultPitch"`
|
||||
}
|
||||
|
||||
type HornConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
Volume float64 `json:"volume"`
|
||||
SampleRate int `json:"sampleRate"`
|
||||
Channels int `json:"channels"`
|
||||
Device string `json:"device"`
|
||||
SineGain float64 `json:"sineGain"`
|
||||
SawGain float64 `json:"sawGain"`
|
||||
MaxDuration Duration `json:"maxDuration"`
|
||||
}
|
||||
|
||||
type MediaConfig struct {
|
||||
// RTSPPort is shared by the derived video, microphone, and forwarded-audio
|
||||
// RTSP URLs. Keeping it at this level prevents each nested block from needing
|
||||
// to repeat the same server port when the common MediaMTX listener is used.
|
||||
RTSPPort int `json:"rtspPort" jsonschema:"minimum=1,maximum=65535"`
|
||||
Manage bool `json:"manage"`
|
||||
HealthURL string `json:"healthUrl"`
|
||||
HealthInterval Duration `json:"healthInterval"`
|
||||
Video VideoMediaConfig `json:"video"`
|
||||
AudioCapture AudioCaptureConfig `json:"audioCapture"`
|
||||
AudioPlayback AudioPlaybackConfig `json:"audioPlayback"`
|
||||
}
|
||||
|
||||
type VideoMediaConfig struct {
|
||||
// Publisher selects the installed publisher script/pipeline family. The
|
||||
// first pass uses pi-libcamera for current rovers; laptop-v4l2 can be added
|
||||
// without changing the server-facing media shape again.
|
||||
Enabled bool `json:"enabled"`
|
||||
Service string `json:"service"`
|
||||
Publisher string `json:"publisher"`
|
||||
// PublishURL is derived during validation. It remains in rover metadata for server-side
|
||||
// consumers through MediaMetadata, but is never persisted in configuration.
|
||||
PublishURL string `json:"-"`
|
||||
Device string `json:"device"`
|
||||
InputFormat string `json:"inputFormat"`
|
||||
Width int `json:"width"`
|
||||
Height int `json:"height"`
|
||||
FPS int `json:"fps"`
|
||||
Bitrate int `json:"bitrate"`
|
||||
Inverted bool `json:"inverted"`
|
||||
SensorMode string `json:"sensorMode"`
|
||||
}
|
||||
|
||||
type AudioCaptureConfig struct {
|
||||
// AudioCapture describes the rover microphone stream that browsers can
|
||||
// subscribe to as "<rover>-audio". A disabled capture block still has
|
||||
// normalized defaults so enabling it only requires flipping enabled: true.
|
||||
Enabled bool `json:"enabled"`
|
||||
Service string `json:"service"`
|
||||
// PublishURL follows the same derived-only contract as the video path.
|
||||
PublishURL string `json:"-"`
|
||||
Device string `json:"device"`
|
||||
SampleRate int `json:"sampleRate"`
|
||||
Channels int `json:"channels"`
|
||||
Bitrate int `json:"bitrate"`
|
||||
}
|
||||
|
||||
type AudioPlaybackConfig struct {
|
||||
// AudioPlayback describes the reverse stream that VIP users publish into
|
||||
// MediaMTX for playback on the rover speaker. The URL is a request/read URL
|
||||
// for the rover listener, while the server converts it to publish mode when
|
||||
// it needs to inject audio.
|
||||
Enabled bool `json:"enabled"`
|
||||
Service string `json:"service"`
|
||||
// ForwardURL is derived because the server and rover must agree on the exact -fwd path.
|
||||
ForwardURL string `json:"-"`
|
||||
Device string `json:"device"`
|
||||
Normalize bool `json:"normalize"`
|
||||
NormalizeFilter string `json:"normalizeFilter"`
|
||||
}
|
||||
|
||||
type CameraServoConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
Pin int `json:"pin"`
|
||||
FreqHz int `json:"freqHz"`
|
||||
CycleLen int `json:"cycleLen"`
|
||||
MinPulseUs int `json:"minPulseUs"`
|
||||
MaxPulseUs int `json:"maxPulseUs"`
|
||||
MinAngle float64 `json:"minAngle"`
|
||||
MaxAngle float64 `json:"maxAngle"`
|
||||
HomeAngle float64 `json:"homeAngle"`
|
||||
NudgeDegrees float64 `json:"nudgeDegrees"`
|
||||
AllowRawPulse bool `json:"allowRawPulse"`
|
||||
Invert bool `json:"invert"`
|
||||
}
|
||||
|
||||
type GPIOToggleConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
GPIOPin int `json:"gpioPin"`
|
||||
GPIOChip string `json:"gpioChip"`
|
||||
InitialOn bool `json:"initialOn"`
|
||||
ActiveLow bool `json:"activeLow"`
|
||||
}
|
||||
|
||||
func (g GPIOToggleConfig) LogicalToGPIO(on bool) int {
|
||||
// activeLow is the single hardware-inversion point for GPIO toggles. The
|
||||
// rest of the daemon, server, and web UI can use normal logical on/off
|
||||
// semantics without knowing whether the driver is active-high or active-low.
|
||||
if on == g.ActiveLow {
|
||||
return 0
|
||||
}
|
||||
return 1
|
||||
}
|
||||
|
||||
type AutoSideBrushConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
Speed int `json:"speed"`
|
||||
}
|
||||
|
||||
type PrivateConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
Safety PrivateSafetyConfig `json:"safety"`
|
||||
}
|
||||
|
||||
type PrivateSafetyConfig struct {
|
||||
SpeedLimitEnabled bool `json:"speedLimitEnabled"`
|
||||
SpeedLimitMaxWheelMMs int `json:"speedLimitMaxWheelSpeed"`
|
||||
HardOvercurrentEnabled bool `json:"hardOvercurrentEnabled"`
|
||||
OvercurrentStopMs int `json:"overcurrentStopMs"`
|
||||
HardBumpEnabled bool `json:"hardBumpEnabled"`
|
||||
BumpBackoffSpeed int `json:"bumpBackoffSpeed"`
|
||||
BumpBackoffMs int `json:"bumpBackoffMs"`
|
||||
CliffEnabled bool `json:"cliffEnabled"`
|
||||
CliffBackoffSpeed int `json:"cliffBackoffSpeed"`
|
||||
CliffBackoffMs int `json:"cliffBackoffMs"`
|
||||
VirtualWallEnabled bool `json:"virtualWallEnabled"`
|
||||
VirtualWallBackoffSpeed int `json:"virtualWallBackoffSpeed"`
|
||||
VirtualWallBackoffMs int `json:"virtualWallBackoffMs"`
|
||||
TriggerCooldownMs int `json:"triggerCooldownMs"`
|
||||
}
|
||||
|
||||
type PiGPIOConfig struct {
|
||||
Enabled bool `json:"enabled"`
|
||||
BRC BRCConfig `json:"brc"`
|
||||
CameraServo CameraServoConfig `json:"cameraServo"`
|
||||
Headlight GPIOToggleConfig `json:"headlight"`
|
||||
Laser GPIOToggleConfig `json:"laser"`
|
||||
}
|
||||
|
||||
type Config struct {
|
||||
PiGPIO PiGPIOConfig `json:"piGpio"`
|
||||
Name string `json:"name" jsonschema:"minLength=1"`
|
||||
Description string `json:"description"`
|
||||
Color string `json:"color"`
|
||||
ServerURL string `json:"serverUrl" jsonschema:"minLength=1"`
|
||||
Serial SerialConfig `json:"serial"`
|
||||
Battery BatteryConfig `json:"battery"`
|
||||
MaxWheelMMs int `json:"maxWheelSpeed" jsonschema:"minimum=1,maximum=500"`
|
||||
Media MediaConfig `json:"media"`
|
||||
Audio AudioConfig `json:"audio"`
|
||||
Horn HornConfig `json:"horn"`
|
||||
AutoSideBrush AutoSideBrushConfig `json:"autoSideBrush"`
|
||||
Private PrivateConfig `json:"private"`
|
||||
}
|
||||
|
||||
// DefaultConfig supplies initial settings; persisted and remotely submitted
|
||||
// configuration is always a complete object.
|
||||
func DefaultConfig() Config {
|
||||
cfg := Config{
|
||||
Name: "rover", ServerURL: "ws://localhost:8080/rover",
|
||||
Serial: SerialConfig{Device: "/dev/ttyUSB0", Baud: 115200},
|
||||
Battery: BatteryConfig{Full: 2068, Warn: 1700, Urgent: 1650},
|
||||
PiGPIO: PiGPIOConfig{
|
||||
BRC: BRCConfig{
|
||||
Enabled: true,
|
||||
GPIOPin: 4,
|
||||
GPIOChip: "gpiochip0",
|
||||
PulseEvery: Duration{
|
||||
Duration: time.Minute,
|
||||
},
|
||||
PulseWidth: Duration{
|
||||
Duration: time.Second,
|
||||
},
|
||||
},
|
||||
CameraServo: CameraServoConfig{
|
||||
Pin: 12,
|
||||
FreqHz: 50,
|
||||
CycleLen: 20000,
|
||||
MinPulseUs: 900,
|
||||
MaxPulseUs: 2100,
|
||||
MinAngle: -15,
|
||||
MaxAngle: 30,
|
||||
HomeAngle: 0,
|
||||
NudgeDegrees: 2,
|
||||
},
|
||||
Headlight: GPIOToggleConfig{
|
||||
Enabled: true,
|
||||
GPIOPin: 22,
|
||||
GPIOChip: "gpiochip0",
|
||||
InitialOn: false,
|
||||
ActiveLow: true,
|
||||
},
|
||||
Laser: GPIOToggleConfig{
|
||||
Enabled: false,
|
||||
GPIOPin: 27,
|
||||
GPIOChip: "gpiochip0",
|
||||
InitialOn: false,
|
||||
ActiveLow: false,
|
||||
},
|
||||
},
|
||||
MaxWheelMMs: 500,
|
||||
Media: MediaConfig{
|
||||
RTSPPort: 8554,
|
||||
HealthInterval: Duration{Duration: 30 * time.Second},
|
||||
Video: VideoMediaConfig{
|
||||
Enabled: true,
|
||||
Service: "video-publisher.service",
|
||||
Publisher: "pi-libcamera",
|
||||
Width: 640,
|
||||
Height: 480,
|
||||
FPS: 30,
|
||||
Bitrate: 2000000,
|
||||
Inverted: true,
|
||||
SensorMode: "1296:972",
|
||||
},
|
||||
AudioCapture: AudioCaptureConfig{
|
||||
Enabled: false,
|
||||
Service: "audio-only-publisher.service",
|
||||
Device: "hw:0,0",
|
||||
SampleRate: 48000,
|
||||
Channels: 2,
|
||||
Bitrate: 510000,
|
||||
},
|
||||
AudioPlayback: AudioPlaybackConfig{
|
||||
Enabled: true,
|
||||
Service: "audio-forward-listener.service",
|
||||
Device: "forward",
|
||||
Normalize: true,
|
||||
NormalizeFilter: "dynaudnorm=f=75:g=15:m=10:p=0.9,alimiter=limit=0.85:level=disabled",
|
||||
},
|
||||
},
|
||||
Audio: AudioConfig{
|
||||
TTSEnabled: false,
|
||||
DefaultEngine: "flite",
|
||||
DefaultVoice: "rms",
|
||||
DefaultPitch: 50,
|
||||
},
|
||||
Horn: HornConfig{
|
||||
Enabled: false,
|
||||
Volume: 0.25,
|
||||
SampleRate: 48000,
|
||||
Channels: 1,
|
||||
SineGain: 1.0,
|
||||
SawGain: 0.7,
|
||||
MaxDuration: Duration{Duration: 10000 * time.Millisecond},
|
||||
},
|
||||
AutoSideBrush: AutoSideBrushConfig{
|
||||
Enabled: true,
|
||||
Speed: 20,
|
||||
},
|
||||
Private: PrivateConfig{
|
||||
Enabled: false,
|
||||
Safety: PrivateSafetyConfig{
|
||||
SpeedLimitEnabled: false,
|
||||
SpeedLimitMaxWheelMMs: 250,
|
||||
HardOvercurrentEnabled: false,
|
||||
OvercurrentStopMs: 300,
|
||||
HardBumpEnabled: false,
|
||||
BumpBackoffSpeed: 250,
|
||||
BumpBackoffMs: 350,
|
||||
CliffEnabled: false,
|
||||
CliffBackoffSpeed: 250,
|
||||
CliffBackoffMs: 500,
|
||||
VirtualWallEnabled: true,
|
||||
VirtualWallBackoffSpeed: 250,
|
||||
VirtualWallBackoffMs: 500,
|
||||
TriggerCooldownMs: 800,
|
||||
},
|
||||
},
|
||||
}
|
||||
return cfg
|
||||
}
|
||||
|
||||
func LoadConfig(path string) (*Config, error) {
|
||||
data, err := os.ReadFile(path)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
return DecodeConfig(data)
|
||||
}
|
||||
|
||||
func DecodeConfig(data []byte) (*Config, error) {
|
||||
if err := ValidateConfigurationSchema(data); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
var cfg Config
|
||||
decoder := json.NewDecoder(bytes.NewReader(data))
|
||||
decoder.DisallowUnknownFields()
|
||||
if err := decoder.Decode(&cfg); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
if err := decoder.Decode(new(any)); err != io.EOF {
|
||||
return nil, errors.New("expected one JSON object")
|
||||
}
|
||||
if err := validateConfig(&cfg); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
return &cfg, nil
|
||||
}
|
||||
|
||||
func validateConfig(cfg *Config) error {
|
||||
if cfg.Name == "" {
|
||||
return errors.New("missing name")
|
||||
}
|
||||
normalizedColor, err := normalizeHexColor(cfg.Color)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
cfg.Color = normalizedColor
|
||||
server, err := url.Parse(cfg.ServerURL)
|
||||
if err != nil || server.Hostname() == "" || (server.Scheme != "ws" && server.Scheme != "wss") || server.Path != "/rover" || server.RawQuery != "" || server.Fragment != "" {
|
||||
return errors.New("serverUrl must be ws://host[:port]/rover or wss://host[:port]/rover")
|
||||
}
|
||||
if cfg.Serial.Device == "" || cfg.Serial.Baud == 0 {
|
||||
return errors.New("serial device/baud required")
|
||||
}
|
||||
if cfg.Battery.Full == 0 {
|
||||
return errors.New("battery thresholds required")
|
||||
}
|
||||
if cfg.MaxWheelMMs <= 0 || cfg.MaxWheelMMs > 500 {
|
||||
return fmt.Errorf("maxWheelSpeed must be 1-500, got %d", cfg.MaxWheelMMs)
|
||||
}
|
||||
if cfg.PiGPIO.BRC.GPIOChip == "" {
|
||||
cfg.PiGPIO.BRC.GPIOChip = "gpiochip0"
|
||||
}
|
||||
if cfg.Media.RTSPPort <= 0 {
|
||||
cfg.Media.RTSPPort = 8554
|
||||
}
|
||||
if err := validateMediaConfig(&cfg.Media, cfg.ServerURL, cfg.Name); err != nil {
|
||||
return fmt.Errorf("media: %w", err)
|
||||
}
|
||||
if cfg.PiGPIO.Enabled {
|
||||
if err := validateServoConfig(&cfg.PiGPIO.CameraServo); err != nil {
|
||||
return fmt.Errorf("cameraServo: %w", err)
|
||||
}
|
||||
if err := validateGPIOToggleConfig(&cfg.PiGPIO.Headlight); err != nil {
|
||||
return fmt.Errorf("headlight: %w", err)
|
||||
}
|
||||
if err := validateGPIOToggleConfig(&cfg.PiGPIO.Laser); err != nil {
|
||||
return fmt.Errorf("laser: %w", err)
|
||||
}
|
||||
if cfg.PiGPIO.BRC.Enabled && (cfg.PiGPIO.BRC.GPIOPin < 0 || cfg.PiGPIO.BRC.PulseEvery.Duration <= 0 || cfg.PiGPIO.BRC.PulseWidth.Duration <= 0) {
|
||||
return errors.New("piGpio.brc: nonnegative pin and positive pulse durations required")
|
||||
}
|
||||
}
|
||||
validateAudioConfig(&cfg.Audio)
|
||||
validateHornConfig(&cfg.Horn)
|
||||
validateAutoSideBrushConfig(&cfg.AutoSideBrush)
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateServoConfig(cfg *CameraServoConfig) error {
|
||||
if !cfg.Enabled {
|
||||
return nil
|
||||
}
|
||||
if cfg.Pin <= 0 {
|
||||
return errors.New("pin must be > 0")
|
||||
}
|
||||
if cfg.FreqHz <= 0 {
|
||||
return errors.New("freqHz must be > 0")
|
||||
}
|
||||
if cfg.CycleLen <= 0 {
|
||||
return errors.New("cycleLen must be > 0")
|
||||
}
|
||||
if cfg.MinPulseUs <= 0 || cfg.MaxPulseUs <= 0 {
|
||||
return errors.New("minPulseUs/maxPulseUs invalid")
|
||||
}
|
||||
if cfg.MinPulseUs == cfg.MaxPulseUs {
|
||||
return errors.New("minPulseUs/maxPulseUs cannot be equal")
|
||||
}
|
||||
if cfg.MinPulseUs > cfg.MaxPulseUs {
|
||||
cfg.MinPulseUs, cfg.MaxPulseUs = cfg.MaxPulseUs, cfg.MinPulseUs
|
||||
cfg.Invert = !cfg.Invert
|
||||
}
|
||||
if cfg.MinAngle >= cfg.MaxAngle {
|
||||
return errors.New("minAngle must be less than maxAngle")
|
||||
}
|
||||
cfg.HomeAngle = clampFloat(cfg.HomeAngle, cfg.MinAngle, cfg.MaxAngle)
|
||||
if cfg.NudgeDegrees <= 0 {
|
||||
cfg.NudgeDegrees = 2
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func clampFloat(value, min, max float64) float64 {
|
||||
if value < min {
|
||||
return min
|
||||
}
|
||||
if value > max {
|
||||
return max
|
||||
}
|
||||
return value
|
||||
}
|
||||
|
||||
func validateAudioConfig(cfg *AudioConfig) {
|
||||
if cfg.DefaultEngine == "" {
|
||||
cfg.DefaultEngine = "flite"
|
||||
}
|
||||
if cfg.DefaultVoice == "" {
|
||||
cfg.DefaultVoice = "rms"
|
||||
}
|
||||
if cfg.DefaultPitch <= 0 {
|
||||
cfg.DefaultPitch = 50
|
||||
}
|
||||
}
|
||||
|
||||
func validateMediaConfig(cfg *MediaConfig, serverURL string, roverName string) error {
|
||||
/*
|
||||
The nested media config is the source of truth for the publisher scripts,
|
||||
so defaults that used to live in shell are normalized here before any env
|
||||
file is written. This keeps the Pi behavior stable while making laptop
|
||||
and future publisher variants explicit configuration choices.
|
||||
*/
|
||||
if cfg.RTSPPort <= 0 {
|
||||
cfg.RTSPPort = 8554
|
||||
}
|
||||
if cfg.HealthInterval.Duration <= 0 {
|
||||
cfg.HealthInterval = Duration{Duration: 30 * time.Second}
|
||||
}
|
||||
if err := validateVideoMediaConfig(&cfg.Video, serverURL, roverName, cfg.RTSPPort); err != nil {
|
||||
return fmt.Errorf("video: %w", err)
|
||||
}
|
||||
if err := validateAudioCaptureConfig(&cfg.AudioCapture, serverURL, roverName, cfg.RTSPPort); err != nil {
|
||||
return fmt.Errorf("audioCapture: %w", err)
|
||||
}
|
||||
if err := validateAudioPlaybackConfig(&cfg.AudioPlayback, serverURL, roverName, cfg.RTSPPort); err != nil {
|
||||
return fmt.Errorf("audioPlayback: %w", err)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateVideoMediaConfig(cfg *VideoMediaConfig, serverURL string, roverName string, rtspPort int) error {
|
||||
if cfg.Service == "" {
|
||||
cfg.Service = "video-publisher.service"
|
||||
}
|
||||
if cfg.Publisher == "" {
|
||||
cfg.Publisher = "pi-libcamera"
|
||||
}
|
||||
// V4L2 input formats are consumed by ffmpeg as lowercase names such as
|
||||
// mjpeg or yuyv422. Normalizing here keeps the publisher script simple and
|
||||
// makes hand-edited Debian laptop configs less sensitive to capitalization.
|
||||
cfg.InputFormat = strings.ToLower(strings.TrimSpace(cfg.InputFormat))
|
||||
if cfg.Width <= 0 {
|
||||
cfg.Width = 640
|
||||
}
|
||||
if cfg.Height <= 0 {
|
||||
cfg.Height = 480
|
||||
}
|
||||
if cfg.FPS <= 0 {
|
||||
cfg.FPS = 30
|
||||
}
|
||||
if cfg.Bitrate <= 0 {
|
||||
cfg.Bitrate = 2000000
|
||||
}
|
||||
if cfg.SensorMode == "" && cfg.Publisher == "pi-libcamera" {
|
||||
cfg.SensorMode = "1296:972"
|
||||
}
|
||||
/*
|
||||
Always derive this endpoint. Older rover configs can contain an explicit SRT publishUrl;
|
||||
honoring it after a binary update would silently leave that rover on the old transport.
|
||||
*/
|
||||
derived, err := derivePublishURL(serverURL, roverName, rtspPort)
|
||||
if err != nil {
|
||||
return fmt.Errorf("derive publishUrl: %w", err)
|
||||
}
|
||||
cfg.PublishURL = derived
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateAudioCaptureConfig(cfg *AudioCaptureConfig, serverURL string, roverName string, rtspPort int) error {
|
||||
if cfg.Service == "" {
|
||||
cfg.Service = "audio-only-publisher.service"
|
||||
}
|
||||
if cfg.Device == "" {
|
||||
cfg.Device = "hw:0,0"
|
||||
}
|
||||
if cfg.SampleRate <= 0 {
|
||||
cfg.SampleRate = 48000
|
||||
}
|
||||
if cfg.Channels <= 0 {
|
||||
cfg.Channels = 2
|
||||
}
|
||||
if cfg.Bitrate <= 0 {
|
||||
cfg.Bitrate = 510000
|
||||
}
|
||||
derived, err := derivePublishURL(serverURL, roverName+"-audio", rtspPort)
|
||||
if err != nil {
|
||||
return fmt.Errorf("derive publishUrl: %w", err)
|
||||
}
|
||||
cfg.PublishURL = derived
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateAudioPlaybackConfig(cfg *AudioPlaybackConfig, serverURL string, roverName string, rtspPort int) error {
|
||||
if cfg.Service == "" {
|
||||
cfg.Service = "audio-forward-listener.service"
|
||||
}
|
||||
if cfg.Device == "" {
|
||||
cfg.Device = "forward"
|
||||
}
|
||||
if cfg.NormalizeFilter == "" {
|
||||
cfg.NormalizeFilter = "dynaudnorm=f=75:g=15:m=10:p=0.9,alimiter=limit=0.85:level=disabled"
|
||||
}
|
||||
derived, err := deriveReadURL(serverURL, roverName+"-fwd", rtspPort)
|
||||
if err != nil {
|
||||
return fmt.Errorf("derive forwardUrl: %w", err)
|
||||
}
|
||||
cfg.ForwardURL = derived
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateHornConfig(cfg *HornConfig) {
|
||||
if cfg.Volume <= 0 {
|
||||
cfg.Volume = 0.25
|
||||
}
|
||||
if cfg.Volume > 1 {
|
||||
cfg.Volume = 1
|
||||
}
|
||||
if cfg.SampleRate <= 0 {
|
||||
cfg.SampleRate = 48000
|
||||
}
|
||||
if cfg.Channels <= 0 {
|
||||
cfg.Channels = 1
|
||||
}
|
||||
if cfg.SineGain <= 0 {
|
||||
cfg.SineGain = 1.0
|
||||
}
|
||||
if cfg.SawGain <= 0 {
|
||||
cfg.SawGain = 0.7
|
||||
}
|
||||
if cfg.MaxDuration.Duration <= 0 {
|
||||
cfg.MaxDuration = Duration{Duration: 1200 * time.Millisecond}
|
||||
}
|
||||
}
|
||||
|
||||
func validateGPIOToggleConfig(cfg *GPIOToggleConfig) error {
|
||||
if !cfg.Enabled {
|
||||
return nil
|
||||
}
|
||||
if cfg.GPIOPin <= 0 {
|
||||
return errors.New("gpioPin must be > 0")
|
||||
}
|
||||
if cfg.GPIOChip == "" {
|
||||
cfg.GPIOChip = "gpiochip0"
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateAutoSideBrushConfig(cfg *AutoSideBrushConfig) {
|
||||
if cfg.Speed == 0 {
|
||||
return
|
||||
}
|
||||
cfg.Speed = clampInt(cfg.Speed, -127, 127)
|
||||
}
|
||||
|
||||
func derivePublishURL(serverURL, streamName string, port int) (string, error) {
|
||||
return deriveRTSPURL(serverURL, streamName, port)
|
||||
}
|
||||
|
||||
func deriveReadURL(serverURL, streamName string, port int) (string, error) {
|
||||
return deriveRTSPURL(serverURL, streamName, port)
|
||||
}
|
||||
|
||||
func deriveRTSPURL(serverURL, streamName string, port int) (string, error) {
|
||||
if streamName == "" {
|
||||
return "", errors.New("missing stream name for publishUrl")
|
||||
}
|
||||
parsed, err := url.Parse(serverURL)
|
||||
if err != nil {
|
||||
return "", err
|
||||
}
|
||||
host := parsed.Hostname()
|
||||
if host == "" {
|
||||
return "", errors.New("serverUrl missing host")
|
||||
}
|
||||
if port <= 0 {
|
||||
port = 8554
|
||||
}
|
||||
/*
|
||||
JoinHostPort handles both ordinary hostnames and bracketed IPv6 addresses. The rover name
|
||||
is a MediaMTX path, so it is escaped independently instead of interpolated into the host.
|
||||
RTSP distinguishes publishing from reading through protocol methods, which is why both
|
||||
directions intentionally use the same URL shape.
|
||||
*/
|
||||
escaped := url.PathEscape(streamName)
|
||||
return fmt.Sprintf("rtsp://%s/%s", net.JoinHostPort(host, strconv.Itoa(port)), escaped), nil
|
||||
}
|
||||
|
||||
var hexColorRe = regexp.MustCompile(`^#[0-9A-Fa-f]{6}$`)
|
||||
|
||||
func normalizeHexColor(raw string) (string, error) {
|
||||
trimmed := strings.TrimSpace(raw)
|
||||
if trimmed == "" {
|
||||
return "", nil
|
||||
}
|
||||
if !hexColorRe.MatchString(trimmed) {
|
||||
return "", fmt.Errorf("color must be #RRGGBB, got %q", raw)
|
||||
}
|
||||
return strings.ToUpper(trimmed), nil
|
||||
}
|
||||
|
||||
// MediaMetadata adds derived endpoints only to the control hello.
|
||||
func (cfg *Config) MediaMetadata() map[string]any {
|
||||
data, _ := json.Marshal(cfg.Media)
|
||||
var media map[string]any
|
||||
json.Unmarshal(data, &media)
|
||||
media["video"].(map[string]any)["publishUrl"] = cfg.Media.Video.PublishURL
|
||||
media["audioCapture"].(map[string]any)["publishUrl"] = cfg.Media.AudioCapture.PublishURL
|
||||
media["audioPlayback"].(map[string]any)["forwardUrl"] = cfg.Media.AudioPlayback.ForwardURL
|
||||
return media
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"os"
|
||||
"path/filepath"
|
||||
"sync"
|
||||
|
||||
"github.com/invopop/jsonschema"
|
||||
validator "github.com/santhosh-tekuri/jsonschema/v6"
|
||||
)
|
||||
|
||||
func (Duration) JSONSchema() *jsonschema.Schema {
|
||||
return &jsonschema.Schema{Type: "string", Description: "Duration, for example 250ms, 1s, or 1m."}
|
||||
}
|
||||
|
||||
var configurationSchema = sync.OnceValues(func() (json.RawMessage, *validator.Schema) {
|
||||
r := jsonschema.Reflector{Anonymous: true, DoNotReference: true}
|
||||
schema := r.Reflect(&Config{})
|
||||
// The existing React form validator uses draft 7. This schema contains no
|
||||
// references or newer keywords, so both ends use the same simple dialect.
|
||||
schema.Version = "http://json-schema.org/draft-07/schema#"
|
||||
schema.Title = "Rover configuration"
|
||||
data, err := json.Marshal(schema)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
var document any
|
||||
if err := json.Unmarshal(data, &document); err != nil {
|
||||
panic(err)
|
||||
}
|
||||
compiler := validator.NewCompiler()
|
||||
const location = "urn:roverd:configuration"
|
||||
if err := compiler.AddResource(location, document); err != nil {
|
||||
panic(err)
|
||||
}
|
||||
compiled, err := compiler.Compile(location)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
return data, compiled
|
||||
})
|
||||
|
||||
func ConfigurationSchema() json.RawMessage {
|
||||
data, _ := configurationSchema()
|
||||
return data
|
||||
}
|
||||
|
||||
func ValidateConfigurationSchema(data []byte) error {
|
||||
var value any
|
||||
if err := json.Unmarshal(data, &value); err != nil {
|
||||
return err
|
||||
}
|
||||
_, schema := configurationSchema()
|
||||
return schema.Validate(value)
|
||||
}
|
||||
|
||||
// SaveConfig replaces one complete object, never merging or keeping revisions.
|
||||
// A temporary file on the same filesystem prevents partial JSON after a crash.
|
||||
func SaveConfig(path string, cfg *Config) error {
|
||||
data, err := json.MarshalIndent(cfg, "", " ")
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
file, err := os.CreateTemp(filepath.Dir(path), ".roverd-config-*")
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer os.Remove(file.Name())
|
||||
defer file.Close()
|
||||
if _, err = file.Write(append(data, '\n')); err != nil {
|
||||
return err
|
||||
}
|
||||
if err = file.Sync(); err != nil {
|
||||
return err
|
||||
}
|
||||
if err = file.Close(); err != nil {
|
||||
return err
|
||||
}
|
||||
if err = os.Rename(file.Name(), path); err != nil {
|
||||
return fmt.Errorf("replace configuration: %w", err)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
const roverConsolePath = "/dev/tty1"
|
||||
|
||||
// ConsoleNotifier writes the small set of rover lifecycle events that must be
|
||||
// visible even when nobody is logged in. This intentionally targets tty1
|
||||
// directly instead of using wall: wall discovers recipients through utmp, so
|
||||
// it does not reliably reach a virtual console that is only showing a login
|
||||
// prompt.
|
||||
type ConsoleNotifier struct {
|
||||
events chan<- RoverEvent
|
||||
path string
|
||||
logger *log.Logger
|
||||
mu sync.Mutex
|
||||
}
|
||||
|
||||
// NewConsoleNotifier returns the production notifier for the rover's primary
|
||||
// local virtual console. Keeping the path inside the notifier also gives tests
|
||||
// a way to substitute a regular temporary file without touching a real TTY.
|
||||
func NewConsoleNotifier(logger *log.Logger) *ConsoleNotifier {
|
||||
return newConsoleNotifier(roverConsolePath, logger)
|
||||
}
|
||||
|
||||
func newConsoleNotifier(path string, logger *log.Logger) *ConsoleNotifier {
|
||||
return &ConsoleNotifier{path: path, logger: logger}
|
||||
}
|
||||
|
||||
// Notify appends one self-contained alert to the console. Console output is a
|
||||
// diagnostic convenience rather than part of rover control, so an unavailable
|
||||
// tty is logged but never allowed to stop startup, reconnection, docking, or
|
||||
// reboot behavior.
|
||||
func (n *ConsoleNotifier) Notify(message string) {
|
||||
if n == nil {
|
||||
return
|
||||
}
|
||||
|
||||
n.mu.Lock()
|
||||
defer n.mu.Unlock()
|
||||
|
||||
if n.events != nil {
|
||||
select {
|
||||
case n.events <- RoverEvent{Type: "event", Event: "daemon.notice", Ts: time.Now().UnixMilli(), Data: map[string]any{"message": message}}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
console, err := os.OpenFile(n.path, os.O_WRONLY|os.O_APPEND, 0)
|
||||
if err != nil {
|
||||
n.logFailure("open", err)
|
||||
return
|
||||
}
|
||||
defer console.Close()
|
||||
|
||||
// Leading and trailing CRLFs keep the alert separate from an agetty login
|
||||
// prompt, while plain text avoids leaving an unknown terminal in a modified
|
||||
// color or cursor state.
|
||||
timestamp := time.Now().UTC().Format("2006-01-02 15:04:05 UTC")
|
||||
if _, err := fmt.Fprintf(console, "\r\n*** rover alert - %s ***\r\n%s\r\n", timestamp, message); err != nil {
|
||||
n.logFailure("write", err)
|
||||
}
|
||||
}
|
||||
|
||||
func (n *ConsoleNotifier) logFailure(operation string, err error) {
|
||||
if n.logger != nil {
|
||||
n.logger.Printf("console notification %s failed for %s: %v", operation, n.path, err)
|
||||
}
|
||||
}
|
||||
|
||||
func (n *ConsoleNotifier) SetEvents(events chan<- RoverEvent) {
|
||||
n.mu.Lock()
|
||||
defer n.mu.Unlock()
|
||||
n.events = events
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"io"
|
||||
"log"
|
||||
"os"
|
||||
"path/filepath"
|
||||
"strings"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestConsoleNotifierWritesVisibleAlert(t *testing.T) {
|
||||
path := filepath.Join(t.TempDir(), "tty1")
|
||||
if err := os.WriteFile(path, nil, 0o600); err != nil {
|
||||
t.Fatalf("create fake console: %v", err)
|
||||
}
|
||||
|
||||
notifier := newConsoleNotifier(path, log.New(io.Discard, "", 0))
|
||||
notifier.Notify("control server connection lost")
|
||||
|
||||
contents, err := os.ReadFile(path)
|
||||
if err != nil {
|
||||
t.Fatalf("read fake console: %v", err)
|
||||
}
|
||||
output := string(contents)
|
||||
if !strings.Contains(output, "*** rover alert - ") {
|
||||
t.Fatalf("alert header missing from %q", output)
|
||||
}
|
||||
if !strings.Contains(output, "control server connection lost") {
|
||||
t.Fatalf("alert message missing from %q", output)
|
||||
}
|
||||
}
|
||||
|
||||
func TestConsoleNotifierTreatsMissingConsoleAsNonfatal(t *testing.T) {
|
||||
// A missing TTY is normal on some headless or containerized hosts. The
|
||||
// contract is therefore simply that Notify returns instead of escalating a
|
||||
// display failure into a rover-process failure.
|
||||
notifier := newConsoleNotifier(filepath.Join(t.TempDir(), "missing"), log.New(io.Discard, "", 0))
|
||||
notifier.Notify("roverd started")
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
package roverd
|
||||
|
||||
type RoverEvent struct {
|
||||
Type string `json:"type"`
|
||||
Event string `json:"event"`
|
||||
Ts int64 `json:"ts"`
|
||||
Data map[string]any `json:"data,omitempty"`
|
||||
}
|
||||
@@ -0,0 +1,664 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"sync"
|
||||
)
|
||||
|
||||
// Firmata command and mode constants are kept here instead of scattering raw
|
||||
// bytes through the peripheral code. The values come directly from the Firmata
|
||||
// protocol, so captures from a rover can be compared with the specification.
|
||||
const (
|
||||
firmataReportVersion byte = 0xF9
|
||||
firmataSetPinMode byte = 0xF4
|
||||
firmataSetDigitalPin byte = 0xF5
|
||||
firmataStartSysex byte = 0xF0
|
||||
firmataEndSysex byte = 0xF7
|
||||
firmataReportFirmware byte = 0x79
|
||||
firmataCapabilityQuery byte = 0x6B
|
||||
firmataCapabilityReply byte = 0x6C
|
||||
firmataExtendedAnalog byte = 0x6F
|
||||
firmataServoConfig byte = 0x70
|
||||
firmataPeripheralFeature byte = 0x01
|
||||
|
||||
firmataPeripheralDescribe byte = 0x00
|
||||
firmataPeripheralDescription byte = 0x01
|
||||
firmataPeripheralControl byte = 0x02
|
||||
firmataMaximumSysexDataBytes = 252
|
||||
|
||||
FirmataPinModeOutput byte = 0x01
|
||||
FirmataPinModePWM byte = 0x03
|
||||
FirmataPinModeServo byte = 0x04
|
||||
)
|
||||
|
||||
// FirmataMessage is the transport-neutral result of parsing one complete
|
||||
// Firmata message. For SysEx messages Command is the SysEx feature byte and
|
||||
// Data is everything between that feature byte and END_SYSEX.
|
||||
type FirmataMessage struct {
|
||||
Command byte
|
||||
Data []byte
|
||||
Sysex bool
|
||||
}
|
||||
|
||||
// FirmataParser incrementally parses a byte stream. USB serial reads may split
|
||||
// a message anywhere or combine several messages, so parsing whole Read calls
|
||||
// as though they were packets would intermittently corrupt valid traffic.
|
||||
type FirmataParser struct {
|
||||
inSysex bool
|
||||
sysex []byte
|
||||
command byte
|
||||
data []byte
|
||||
expected int
|
||||
}
|
||||
|
||||
// Feed accepts any fragment of the serial stream and returns every complete
|
||||
// message found in it, preserving wire order.
|
||||
func (p *FirmataParser) Feed(fragment []byte) ([]FirmataMessage, error) {
|
||||
var messages []FirmataMessage
|
||||
|
||||
for _, value := range fragment {
|
||||
if p.inSysex {
|
||||
switch {
|
||||
case value == firmataEndSysex:
|
||||
if len(p.sysex) == 0 {
|
||||
p.resetSysex()
|
||||
return messages, errors.New("Firmata SysEx message is missing a feature byte")
|
||||
}
|
||||
messages = append(messages, FirmataMessage{
|
||||
Command: p.sysex[0],
|
||||
Data: append([]byte(nil), p.sysex[1:]...),
|
||||
Sysex: true,
|
||||
})
|
||||
p.resetSysex()
|
||||
case value&0x80 != 0:
|
||||
// Bytes inside SysEx must be seven-bit clean. Reset immediately so
|
||||
// a damaged frame cannot consume every later message on the port.
|
||||
p.resetSysex()
|
||||
return messages, fmt.Errorf("invalid 8-bit value 0x%02x inside Firmata SysEx", value)
|
||||
default:
|
||||
p.sysex = append(p.sysex, value)
|
||||
}
|
||||
continue
|
||||
}
|
||||
|
||||
if value == firmataStartSysex {
|
||||
p.inSysex = true
|
||||
p.sysex = p.sysex[:0]
|
||||
p.resetFixed()
|
||||
continue
|
||||
}
|
||||
|
||||
if value&0x80 != 0 {
|
||||
p.command = value
|
||||
p.data = p.data[:0]
|
||||
p.expected = firmataDataLength(value)
|
||||
if p.expected == 0 {
|
||||
messages = append(messages, FirmataMessage{Command: value})
|
||||
p.resetFixed()
|
||||
}
|
||||
continue
|
||||
}
|
||||
|
||||
// Stray data before a status byte is harmless serial noise. Firmata
|
||||
// has no framing information that could assign it to a command.
|
||||
if p.expected == 0 {
|
||||
continue
|
||||
}
|
||||
p.data = append(p.data, value)
|
||||
if len(p.data) == p.expected {
|
||||
messages = append(messages, FirmataMessage{
|
||||
Command: p.command,
|
||||
Data: append([]byte(nil), p.data...),
|
||||
})
|
||||
p.resetFixed()
|
||||
}
|
||||
}
|
||||
|
||||
return messages, nil
|
||||
}
|
||||
|
||||
func (p *FirmataParser) resetSysex() {
|
||||
p.inSysex = false
|
||||
p.sysex = p.sysex[:0]
|
||||
}
|
||||
|
||||
func (p *FirmataParser) resetFixed() {
|
||||
p.command = 0
|
||||
p.data = p.data[:0]
|
||||
p.expected = 0
|
||||
}
|
||||
|
||||
// firmataDataLength returns the number of seven-bit data bytes used by the
|
||||
// fixed-length messages relevant to normal Firmata traffic. Unknown system
|
||||
// commands are treated as single-byte messages so they cannot stall parsing of
|
||||
// the rover-peripheral SysEx frames that follow them.
|
||||
func firmataDataLength(command byte) int {
|
||||
switch command {
|
||||
case firmataReportVersion, firmataSetPinMode, firmataSetDigitalPin:
|
||||
return 2
|
||||
}
|
||||
|
||||
switch command & 0xF0 {
|
||||
case 0x80, 0x90, 0xA0, 0xE0:
|
||||
return 2
|
||||
case 0xC0, 0xD0:
|
||||
return 1
|
||||
default:
|
||||
return 0
|
||||
}
|
||||
}
|
||||
|
||||
// EncodeFirmata7Bit converts arbitrary bytes into the two-byte representation
|
||||
// required inside Firmata SysEx. Keeping this transform below the JSON layer
|
||||
// means firmware authors and UI code never need to think about wire encoding.
|
||||
func EncodeFirmata7Bit(raw []byte) []byte {
|
||||
encoded := make([]byte, 0, len(raw)*2)
|
||||
for _, value := range raw {
|
||||
encoded = append(encoded, value&0x7F, (value>>7)&0x01)
|
||||
}
|
||||
return encoded
|
||||
}
|
||||
|
||||
// DecodeFirmata7Bit reverses EncodeFirmata7Bit and rejects malformed pairs.
|
||||
func DecodeFirmata7Bit(encoded []byte) ([]byte, error) {
|
||||
if len(encoded)%2 != 0 {
|
||||
return nil, fmt.Errorf("Firmata 7-bit payload has odd length %d", len(encoded))
|
||||
}
|
||||
|
||||
decoded := make([]byte, 0, len(encoded)/2)
|
||||
for index := 0; index < len(encoded); index += 2 {
|
||||
low, high := encoded[index], encoded[index+1]
|
||||
if low&0x80 != 0 || high > 1 {
|
||||
return nil, fmt.Errorf("invalid Firmata 7-bit pair at byte %d", index)
|
||||
}
|
||||
decoded = append(decoded, low|(high<<7))
|
||||
}
|
||||
return decoded, nil
|
||||
}
|
||||
|
||||
// PeripheralDescription is generated by the ESP32 at boot. Controls is a slice
|
||||
// intentionally: registration order is part of the UI contract and must never
|
||||
// be replaced by map iteration or alphabetical sorting.
|
||||
type PeripheralDescription struct {
|
||||
Name string `json:"name"`
|
||||
RoverControls PeripheralRoverControls `json:"roverControls,omitempty"`
|
||||
Controls []PeripheralControl `json:"controls"`
|
||||
}
|
||||
|
||||
type PeripheralRoverControls struct {
|
||||
CameraServo *PeripheralCameraServo `json:"cameraServo,omitempty"`
|
||||
Headlight *PeripheralDigitalRole `json:"headlight,omitempty"`
|
||||
Laser *PeripheralDigitalRole `json:"laser,omitempty"`
|
||||
}
|
||||
|
||||
type PeripheralCameraServo struct {
|
||||
Pin int `json:"pin"`
|
||||
MinimumAngleDegrees float64 `json:"minimumAngleDegrees"`
|
||||
MaximumAngleDegrees float64 `json:"maximumAngleDegrees"`
|
||||
HomeAngleDegrees float64 `json:"homeAngleDegrees"`
|
||||
NudgeDegrees float64 `json:"nudgeDegrees"`
|
||||
MinimumPulseMicroseconds int `json:"minimumPulseMicroseconds"`
|
||||
MaximumPulseMicroseconds int `json:"maximumPulseMicroseconds"`
|
||||
AllowRawPulse bool `json:"allowRawPulse"`
|
||||
Inverted bool `json:"inverted"`
|
||||
}
|
||||
|
||||
type PeripheralDigitalRole struct {
|
||||
Pin int `json:"pin"`
|
||||
ActiveLow bool `json:"activeLow"`
|
||||
InitiallyOn bool `json:"initiallyOn"`
|
||||
}
|
||||
|
||||
type PeripheralControl struct {
|
||||
ID string `json:"id"`
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Mode string `json:"mode,omitempty"`
|
||||
Minimum *int `json:"min,omitempty"`
|
||||
Maximum *int `json:"max,omitempty"`
|
||||
MaximumLength *int `json:"maxLength,omitempty"`
|
||||
Output PeripheralOutput `json:"output"`
|
||||
}
|
||||
|
||||
type PeripheralOutput struct {
|
||||
Type string `json:"type"`
|
||||
Pin *int `json:"pin,omitempty"`
|
||||
ActiveLow bool `json:"activeLow,omitempty"`
|
||||
}
|
||||
|
||||
// Validate catches authoring mistakes at connection time, where the error can
|
||||
// name the offending peripheral, instead of allowing a malformed declaration
|
||||
// to turn into a confusing no-op later when a driver uses the control.
|
||||
func (description PeripheralDescription) Validate() error {
|
||||
if description.Name == "" {
|
||||
return errors.New("peripheral description requires a name")
|
||||
}
|
||||
if camera := description.RoverControls.CameraServo; camera != nil {
|
||||
if err := validateFirmataPin("cameraServo", camera.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
if camera.MinimumAngleDegrees >= camera.MaximumAngleDegrees {
|
||||
return errors.New("cameraServo angle range must be increasing")
|
||||
}
|
||||
if camera.HomeAngleDegrees < camera.MinimumAngleDegrees || camera.HomeAngleDegrees > camera.MaximumAngleDegrees {
|
||||
return errors.New("cameraServo home angle must be inside its angle range")
|
||||
}
|
||||
if camera.NudgeDegrees <= 0 {
|
||||
return errors.New("cameraServo nudge must be positive")
|
||||
}
|
||||
if camera.MinimumPulseMicroseconds <= 0 || camera.MaximumPulseMicroseconds <= camera.MinimumPulseMicroseconds {
|
||||
return errors.New("cameraServo pulse range must be positive and increasing")
|
||||
}
|
||||
}
|
||||
if role := description.RoverControls.Headlight; role != nil {
|
||||
if err := validateFirmataPin("headlight", role.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
if role := description.RoverControls.Laser; role != nil {
|
||||
if err := validateFirmataPin("laser", role.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
seen := make(map[string]struct{}, len(description.Controls))
|
||||
for index, control := range description.Controls {
|
||||
if control.ID == "" || control.Name == "" {
|
||||
return fmt.Errorf("control %d requires both id and name", index)
|
||||
}
|
||||
if _, exists := seen[control.ID]; exists {
|
||||
return fmt.Errorf("control id %q is duplicated", control.ID)
|
||||
}
|
||||
seen[control.ID] = struct{}{}
|
||||
|
||||
switch control.Type {
|
||||
case "slider", "number":
|
||||
if control.Minimum == nil || control.Maximum == nil || *control.Minimum > *control.Maximum {
|
||||
return fmt.Errorf("control %q requires a valid min and max", control.ID)
|
||||
}
|
||||
case "button":
|
||||
if control.Mode != "toggle" && control.Mode != "momentary" {
|
||||
return fmt.Errorf("button %q requires toggle or momentary mode", control.ID)
|
||||
}
|
||||
case "text":
|
||||
if control.MaximumLength == nil || *control.MaximumLength <= 0 {
|
||||
return fmt.Errorf("text control %q requires a positive maxLength", control.ID)
|
||||
}
|
||||
default:
|
||||
return fmt.Errorf("control %q has unsupported type %q", control.ID, control.Type)
|
||||
}
|
||||
|
||||
switch control.Output.Type {
|
||||
case "digital":
|
||||
if control.Output.Pin == nil {
|
||||
return fmt.Errorf("control %q output %q requires a pin", control.ID, control.Output.Type)
|
||||
}
|
||||
if err := validateFirmataPin("control "+control.ID, *control.Output.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
if control.Type != "button" {
|
||||
return fmt.Errorf("digital output control %q must be a button", control.ID)
|
||||
}
|
||||
case "pwm", "servo":
|
||||
if control.Output.Pin == nil {
|
||||
return fmt.Errorf("control %q output %q requires a pin", control.ID, control.Output.Type)
|
||||
}
|
||||
if err := validateFirmataPin("control "+control.ID, *control.Output.Pin); err != nil {
|
||||
return err
|
||||
}
|
||||
if control.Type != "slider" && control.Type != "number" {
|
||||
return fmt.Errorf("%s output control %q must be a slider or number", control.Output.Type, control.ID)
|
||||
}
|
||||
case "custom":
|
||||
default:
|
||||
return fmt.Errorf("control %q has unsupported output %q", control.ID, control.Output.Type)
|
||||
}
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func validateFirmataPin(owner string, pin int) error {
|
||||
// Firmata represents pin numbers with one seven-bit byte. Rejecting values
|
||||
// outside that wire range avoids silently wrapping a declaration when it is
|
||||
// converted to a byte for output commands.
|
||||
if pin < 0 || pin > 127 {
|
||||
return fmt.Errorf("%s pin must be between 0 and 127", owner)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
// FirmataFirmware identifies the implementation answering the standard
|
||||
// REPORT_FIRMWARE query. It is diagnostic metadata, not a protocol gate.
|
||||
type FirmataFirmware struct {
|
||||
Major int
|
||||
Minor int
|
||||
Name string
|
||||
}
|
||||
|
||||
// FirmataPinCapability is one mode/resolution pair from CAPABILITY_RESPONSE.
|
||||
type FirmataPinCapability struct {
|
||||
Mode byte
|
||||
Resolution byte
|
||||
}
|
||||
|
||||
// FirmataClient owns one already-open serial connection. Its reader goroutine
|
||||
// separates arbitrary USB read boundaries from request/response handling while
|
||||
// writeMu prevents two commands from interleaving on the byte stream.
|
||||
type FirmataClient struct {
|
||||
connection io.ReadWriteCloser
|
||||
parser FirmataParser
|
||||
messages chan FirmataMessage
|
||||
errors chan error
|
||||
writeMu sync.Mutex
|
||||
requestMu sync.Mutex
|
||||
stateMu sync.RWMutex
|
||||
terminalErr error
|
||||
// terminalErrorHandler is invoked only for the first non-timeout read
|
||||
// failure while the client context remains active. PeripheralManager uses it
|
||||
// to turn an unexpected USB loss into one operator-facing broadcast.
|
||||
terminalErrorHandler func(error)
|
||||
}
|
||||
|
||||
func NewFirmataClient(connection io.ReadWriteCloser) *FirmataClient {
|
||||
return &FirmataClient{
|
||||
connection: connection,
|
||||
messages: make(chan FirmataMessage, 16),
|
||||
errors: make(chan error, 1),
|
||||
}
|
||||
}
|
||||
|
||||
// Start begins consuming the serial stream. The caller still owns the port and
|
||||
// closes it during shutdown; this makes the client usable with both real serial
|
||||
// ports and deterministic in-memory test connections.
|
||||
func (client *FirmataClient) Start(ctx context.Context) {
|
||||
go client.readLoop(ctx)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) readLoop(ctx context.Context) {
|
||||
buffer := make([]byte, 256)
|
||||
for {
|
||||
count, err := client.connection.Read(buffer)
|
||||
if count > 0 {
|
||||
messages, parseErr := client.parser.Feed(buffer[:count])
|
||||
if parseErr != nil {
|
||||
client.publishError(ctx, parseErr)
|
||||
return
|
||||
}
|
||||
for _, message := range messages {
|
||||
select {
|
||||
case client.messages <- message:
|
||||
case <-ctx.Done():
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
if err != nil {
|
||||
if errors.Is(err, io.EOF) {
|
||||
// tarm/serial represents an ordinary ReadTimeout with io.EOF. A
|
||||
// Firmata connection is expected to be quiet between commands, so
|
||||
// treating that timeout as a closed device kills the reader before
|
||||
// the next request can receive its reply. A real USB removal is
|
||||
// reported by the serial driver as a non-EOF error.
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
continue
|
||||
}
|
||||
}
|
||||
client.publishError(ctx, err)
|
||||
return
|
||||
}
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) publishError(ctx context.Context, err error) {
|
||||
firstTerminalError, handler := client.recordTerminalError(err)
|
||||
if firstTerminalError && handler != nil && ctx.Err() == nil {
|
||||
handler(err)
|
||||
}
|
||||
|
||||
select {
|
||||
case client.errors <- err:
|
||||
case <-ctx.Done():
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) recordTerminalError(err error) (bool, func(error)) {
|
||||
client.stateMu.Lock()
|
||||
defer client.stateMu.Unlock()
|
||||
firstTerminalError := client.terminalErr == nil
|
||||
if client.terminalErr == nil {
|
||||
client.terminalErr = err
|
||||
}
|
||||
handler := client.terminalErrorHandler
|
||||
return firstTerminalError, handler
|
||||
}
|
||||
|
||||
// SetTerminalErrorHandler registers the one-shot observer used after a device
|
||||
// has completed discovery. If the connection already failed, the observer is
|
||||
// called immediately so a narrow handshake-to-registration race is not lost.
|
||||
func (client *FirmataClient) SetTerminalErrorHandler(handler func(error)) {
|
||||
client.stateMu.Lock()
|
||||
client.terminalErrorHandler = handler
|
||||
terminalErr := client.terminalErr
|
||||
client.stateMu.Unlock()
|
||||
if terminalErr != nil && handler != nil {
|
||||
handler(terminalErr)
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) write(message []byte) error {
|
||||
client.writeMu.Lock()
|
||||
defer client.writeMu.Unlock()
|
||||
client.stateMu.RLock()
|
||||
terminalErr := client.terminalErr
|
||||
client.stateMu.RUnlock()
|
||||
if terminalErr != nil {
|
||||
return fmt.Errorf("Firmata connection unavailable: %w", terminalErr)
|
||||
}
|
||||
|
||||
written, err := client.connection.Write(message)
|
||||
if err != nil {
|
||||
firstTerminalError, handler := client.recordTerminalError(err)
|
||||
if firstTerminalError && handler != nil {
|
||||
handler(err)
|
||||
}
|
||||
return err
|
||||
}
|
||||
if written != len(message) {
|
||||
err := fmt.Errorf("short Firmata write %d/%d", written, len(message))
|
||||
firstTerminalError, handler := client.recordTerminalError(err)
|
||||
if firstTerminalError && handler != nil {
|
||||
handler(err)
|
||||
}
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) writeSysex(command byte, data []byte) error {
|
||||
message := make([]byte, 0, len(data)+3)
|
||||
message = append(message, firmataStartSysex, command)
|
||||
message = append(message, data...)
|
||||
message = append(message, firmataEndSysex)
|
||||
return client.write(message)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) waitFor(ctx context.Context, match func(FirmataMessage) bool) (FirmataMessage, error) {
|
||||
for {
|
||||
select {
|
||||
case message := <-client.messages:
|
||||
if match(message) {
|
||||
return message, nil
|
||||
}
|
||||
case err := <-client.errors:
|
||||
return FirmataMessage{}, err
|
||||
case <-ctx.Done():
|
||||
return FirmataMessage{}, ctx.Err()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (client *FirmataClient) QueryFirmware(ctx context.Context) (FirmataFirmware, error) {
|
||||
client.requestMu.Lock()
|
||||
defer client.requestMu.Unlock()
|
||||
|
||||
if err := client.writeSysex(firmataReportFirmware, nil); err != nil {
|
||||
return FirmataFirmware{}, err
|
||||
}
|
||||
message, err := client.waitFor(ctx, func(message FirmataMessage) bool {
|
||||
return message.Sysex && message.Command == firmataReportFirmware
|
||||
})
|
||||
if err != nil {
|
||||
return FirmataFirmware{}, err
|
||||
}
|
||||
if len(message.Data) < 2 {
|
||||
return FirmataFirmware{}, errors.New("Firmata firmware response is missing version bytes")
|
||||
}
|
||||
name, err := DecodeFirmata7Bit(message.Data[2:])
|
||||
if err != nil {
|
||||
return FirmataFirmware{}, fmt.Errorf("decode Firmata firmware name: %w", err)
|
||||
}
|
||||
return FirmataFirmware{Major: int(message.Data[0]), Minor: int(message.Data[1]), Name: string(name)}, nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) QueryCapabilities(ctx context.Context) ([][]FirmataPinCapability, error) {
|
||||
client.requestMu.Lock()
|
||||
defer client.requestMu.Unlock()
|
||||
|
||||
if err := client.writeSysex(firmataCapabilityQuery, nil); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
message, err := client.waitFor(ctx, func(message FirmataMessage) bool {
|
||||
return message.Sysex && message.Command == firmataCapabilityReply
|
||||
})
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
return parseFirmataCapabilities(message.Data)
|
||||
}
|
||||
|
||||
func parseFirmataCapabilities(data []byte) ([][]FirmataPinCapability, error) {
|
||||
var pins [][]FirmataPinCapability
|
||||
var pin []FirmataPinCapability
|
||||
for index := 0; index < len(data); {
|
||||
if data[index] == 0x7F {
|
||||
pins = append(pins, pin)
|
||||
pin = nil
|
||||
index++
|
||||
continue
|
||||
}
|
||||
if index+1 >= len(data) {
|
||||
return nil, errors.New("Firmata capability response ends inside a mode pair")
|
||||
}
|
||||
pin = append(pin, FirmataPinCapability{Mode: data[index], Resolution: data[index+1]})
|
||||
index += 2
|
||||
}
|
||||
if pin != nil {
|
||||
return nil, errors.New("Firmata capability response is missing its final pin separator")
|
||||
}
|
||||
return pins, nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) Describe(ctx context.Context) (PeripheralDescription, error) {
|
||||
client.requestMu.Lock()
|
||||
defer client.requestMu.Unlock()
|
||||
|
||||
if err := client.writeSysex(firmataPeripheralFeature, []byte{firmataPeripheralDescribe}); err != nil {
|
||||
return PeripheralDescription{}, err
|
||||
}
|
||||
message, err := client.waitFor(ctx, func(message FirmataMessage) bool {
|
||||
return message.Sysex && message.Command == firmataPeripheralFeature && len(message.Data) > 0 && message.Data[0] == firmataPeripheralDescription
|
||||
})
|
||||
if err != nil {
|
||||
return PeripheralDescription{}, err
|
||||
}
|
||||
|
||||
raw, err := DecodeFirmata7Bit(message.Data[1:])
|
||||
if err != nil {
|
||||
return PeripheralDescription{}, fmt.Errorf("decode peripheral description: %w", err)
|
||||
}
|
||||
var description PeripheralDescription
|
||||
if err := json.Unmarshal(raw, &description); err != nil {
|
||||
return PeripheralDescription{}, fmt.Errorf("parse peripheral description: %w", err)
|
||||
}
|
||||
if err := description.Validate(); err != nil {
|
||||
return PeripheralDescription{}, fmt.Errorf("validate peripheral description: %w", err)
|
||||
}
|
||||
return description, nil
|
||||
}
|
||||
|
||||
func (client *FirmataClient) SetPinMode(pin, mode byte) error {
|
||||
return client.write([]byte{firmataSetPinMode, pin & 0x7F, mode & 0x7F})
|
||||
}
|
||||
|
||||
func (client *FirmataClient) SetDigitalPin(pin byte, enabled bool) error {
|
||||
value := byte(0)
|
||||
if enabled {
|
||||
value = 1
|
||||
}
|
||||
return client.write([]byte{firmataSetDigitalPin, pin & 0x7F, value})
|
||||
}
|
||||
|
||||
func (client *FirmataClient) ExtendedAnalog(pin byte, value int) error {
|
||||
if value < 0 {
|
||||
return fmt.Errorf("Firmata analog value cannot be negative: %d", value)
|
||||
}
|
||||
|
||||
payload := []byte{pin & 0x7F}
|
||||
// Firmata encodes integers as many seven-bit chunks as necessary. Zero
|
||||
// still needs one value byte so the receiver can distinguish it from a
|
||||
// message that contains only the pin.
|
||||
for {
|
||||
payload = append(payload, byte(value&0x7F))
|
||||
value >>= 7
|
||||
if value == 0 {
|
||||
break
|
||||
}
|
||||
}
|
||||
return client.writeSysex(firmataExtendedAnalog, payload)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) ConfigureServo(pin byte, minimumPulseMicroseconds, maximumPulseMicroseconds int) error {
|
||||
if minimumPulseMicroseconds <= 0 || maximumPulseMicroseconds <= minimumPulseMicroseconds {
|
||||
return errors.New("servo pulse range must be positive and increasing")
|
||||
}
|
||||
payload := []byte{
|
||||
pin & 0x7F,
|
||||
byte(minimumPulseMicroseconds & 0x7F), byte((minimumPulseMicroseconds >> 7) & 0x7F),
|
||||
byte(maximumPulseMicroseconds & 0x7F), byte((maximumPulseMicroseconds >> 7) & 0x7F),
|
||||
}
|
||||
return client.writeSysex(firmataServoConfig, payload)
|
||||
}
|
||||
|
||||
func (client *FirmataClient) SendPeripheralControl(controlID string, value any) error {
|
||||
payload, err := json.Marshal(struct {
|
||||
Control string `json:"control"`
|
||||
Value any `json:"value"`
|
||||
}{Control: controlID, Value: value})
|
||||
if err != nil {
|
||||
return fmt.Errorf("encode peripheral control: %w", err)
|
||||
}
|
||||
data := append([]byte{firmataPeripheralControl}, EncodeFirmata7Bit(payload)...)
|
||||
// ConfigurableFirmata on ESP32 stores at most 252 bytes including the SysEx
|
||||
// feature byte. Refuse a value that the board would otherwise discard as an
|
||||
// incomplete frame; this is a transport constraint, not an application-level
|
||||
// text policy.
|
||||
if len(data)+1 > firmataMaximumSysexDataBytes {
|
||||
return fmt.Errorf("peripheral control needs %d SysEx data bytes; Firmata accepts at most %d", len(data)+1, firmataMaximumSysexDataBytes)
|
||||
}
|
||||
return client.writeSysex(firmataPeripheralFeature, data)
|
||||
}
|
||||
@@ -0,0 +1,304 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
// FirmataCameraServo preserves the established logical camera movement model
|
||||
// while replacing only the final physical write. The ESP32 receives ordinary
|
||||
// Firmata servo configuration and angle messages, regardless of rover host.
|
||||
type FirmataCameraServo struct {
|
||||
cfg CameraServoConfig
|
||||
client *FirmataClient
|
||||
pin byte
|
||||
peripheralID string
|
||||
mu sync.Mutex
|
||||
currentAngle float64
|
||||
desiredAngle float64
|
||||
lastMove time.Time
|
||||
moving bool
|
||||
stopCh chan struct{}
|
||||
closed bool
|
||||
}
|
||||
|
||||
func newFirmataCameraServo(peripheral *managedPeripheral, declaration PeripheralCameraServo, logger *log.Logger) (*FirmataCameraServo, error) {
|
||||
cfg := CameraServoConfig{
|
||||
Enabled: true,
|
||||
Pin: declaration.Pin,
|
||||
FreqHz: 50,
|
||||
CycleLen: 20000,
|
||||
MinPulseUs: declaration.MinimumPulseMicroseconds,
|
||||
MaxPulseUs: declaration.MaximumPulseMicroseconds,
|
||||
MinAngle: declaration.MinimumAngleDegrees,
|
||||
MaxAngle: declaration.MaximumAngleDegrees,
|
||||
HomeAngle: declaration.HomeAngleDegrees,
|
||||
NudgeDegrees: declaration.NudgeDegrees,
|
||||
AllowRawPulse: declaration.AllowRawPulse,
|
||||
Invert: declaration.Inverted,
|
||||
}
|
||||
servo := &FirmataCameraServo{
|
||||
cfg: cfg,
|
||||
client: peripheral.client,
|
||||
pin: byte(declaration.Pin),
|
||||
peripheralID: peripheral.metadata.ID,
|
||||
stopCh: make(chan struct{}),
|
||||
}
|
||||
|
||||
// SERVO_CONFIG establishes the peripheral-owned pulse calibration before
|
||||
// selecting servo mode. This is standard Firmata, not a rover extension.
|
||||
if err := servo.client.ConfigureServo(servo.pin, cfg.MinPulseUs, cfg.MaxPulseUs); err != nil {
|
||||
return nil, fmt.Errorf("configure Firmata servo: %w", err)
|
||||
}
|
||||
if err := servo.client.SetPinMode(servo.pin, FirmataPinModeServo); err != nil {
|
||||
return nil, fmt.Errorf("select Firmata servo mode: %w", err)
|
||||
}
|
||||
if err := servo.setAngleLocked(cfg.HomeAngle); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
logger.Printf("camera servo using ESP32 %s pin %d (%.1f..%.1f deg)", peripheral.metadata.ID, declaration.Pin, cfg.MinAngle, cfg.MaxAngle)
|
||||
return servo, nil
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) SetAngle(angle float64) error {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
return servo.setAngleLocked(angle)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) setAngleLocked(angle float64) error {
|
||||
if servo.closed {
|
||||
return errorsNewControllerClosed("camera servo")
|
||||
}
|
||||
servo.desiredAngle = clampFloat(angle, servo.cfg.MinAngle, servo.cfg.MaxAngle)
|
||||
limited := servo.rateLimitAngleLocked(servo.desiredAngle)
|
||||
if err := servo.writeAngleLocked(limited); err != nil {
|
||||
return err
|
||||
}
|
||||
servo.currentAngle = limited
|
||||
if math.Abs(limited-servo.desiredAngle) > servoAngleEpsilon {
|
||||
servo.startMoveLoopLocked()
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) Nudge(delta float64) error {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
if delta == 0 {
|
||||
delta = servo.cfg.NudgeDegrees
|
||||
}
|
||||
return servo.setAngleLocked(servo.currentAngle + delta)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) SetPulseWidth(micros int) error {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
if !servo.cfg.AllowRawPulse {
|
||||
return fmt.Errorf("raw pulse commands disabled")
|
||||
}
|
||||
if micros <= 0 {
|
||||
return fmt.Errorf("pulse width must be > 0")
|
||||
}
|
||||
pulse := clampInt(micros, servo.cfg.MinPulseUs, servo.cfg.MaxPulseUs)
|
||||
return servo.setAngleLocked(servo.pulseToAngle(pulse))
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) CurrentAngle() float64 {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
return servo.currentAngle
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) Configuration() CameraServoConfig {
|
||||
return servo.cfg
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) BackendDescription() string {
|
||||
return "ESP32 " + servo.peripheralID
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) Close() {
|
||||
servo.mu.Lock()
|
||||
defer servo.mu.Unlock()
|
||||
if servo.closed {
|
||||
return
|
||||
}
|
||||
// Returning home matches the native Pi implementation. Any write failure is
|
||||
// ignored during shutdown because the serial connection may already be gone.
|
||||
_ = servo.writeAngleLocked(servo.cfg.HomeAngle)
|
||||
close(servo.stopCh)
|
||||
servo.closed = true
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) writeAngleLocked(angle float64) error {
|
||||
rangeDegrees := servo.cfg.MaxAngle - servo.cfg.MinAngle
|
||||
normalized := (angle - servo.cfg.MinAngle) / rangeDegrees
|
||||
normalized = math.Max(0, math.Min(1, normalized))
|
||||
if servo.cfg.Invert {
|
||||
normalized = 1 - normalized
|
||||
}
|
||||
// Standard Firmata servo values are positions from 0 through 180. Pulse
|
||||
// calibration was already supplied through SERVO_CONFIG above.
|
||||
position := int(math.Round(normalized * 180))
|
||||
return servo.client.ExtendedAnalog(servo.pin, position)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) pulseToAngle(pulse int) float64 {
|
||||
normalized := float64(pulse-servo.cfg.MinPulseUs) / float64(servo.cfg.MaxPulseUs-servo.cfg.MinPulseUs)
|
||||
if servo.cfg.Invert {
|
||||
normalized = 1 - normalized
|
||||
}
|
||||
return servo.cfg.MinAngle + normalized*(servo.cfg.MaxAngle-servo.cfg.MinAngle)
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) rateLimitAngleLocked(target float64) float64 {
|
||||
now := time.Now()
|
||||
if servo.lastMove.IsZero() {
|
||||
servo.lastMove = now
|
||||
}
|
||||
elapsed := now.Sub(servo.lastMove).Seconds()
|
||||
if elapsed > servoStepInterval.Seconds() {
|
||||
elapsed = servoStepInterval.Seconds()
|
||||
}
|
||||
maximumDelta := maxServoDegPerSec * elapsed
|
||||
delta := target - servo.currentAngle
|
||||
if math.Abs(delta) <= maximumDelta {
|
||||
servo.lastMove = now
|
||||
return target
|
||||
}
|
||||
servo.lastMove = now
|
||||
if delta > 0 {
|
||||
return servo.currentAngle + maximumDelta
|
||||
}
|
||||
return servo.currentAngle - maximumDelta
|
||||
}
|
||||
|
||||
func (servo *FirmataCameraServo) startMoveLoopLocked() {
|
||||
if servo.moving || servo.closed {
|
||||
return
|
||||
}
|
||||
servo.moving = true
|
||||
go func() {
|
||||
ticker := time.NewTicker(servoStepInterval)
|
||||
defer ticker.Stop()
|
||||
for {
|
||||
select {
|
||||
case <-ticker.C:
|
||||
servo.mu.Lock()
|
||||
if servo.closed || math.Abs(servo.currentAngle-servo.desiredAngle) <= servoAngleEpsilon {
|
||||
servo.moving = false
|
||||
servo.mu.Unlock()
|
||||
return
|
||||
}
|
||||
limited := servo.rateLimitAngleLocked(servo.desiredAngle)
|
||||
if err := servo.writeAngleLocked(limited); err != nil {
|
||||
// A failed serial write makes further automatic steps pointless.
|
||||
// The next user command returns the connection error normally.
|
||||
servo.moving = false
|
||||
servo.mu.Unlock()
|
||||
return
|
||||
}
|
||||
servo.currentAngle = limited
|
||||
servo.mu.Unlock()
|
||||
case <-servo.stopCh:
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
// FirmataToggle owns logical state exactly like GPIOToggle but sends the final
|
||||
// electrical level through Firmata's standard digital-pin command.
|
||||
type FirmataToggle struct {
|
||||
cfg GPIOToggleConfig
|
||||
name string
|
||||
client *FirmataClient
|
||||
pin byte
|
||||
peripheralID string
|
||||
mu sync.Mutex
|
||||
on bool
|
||||
closed bool
|
||||
}
|
||||
|
||||
func newFirmataToggle(name string, peripheral *managedPeripheral, declaration PeripheralDigitalRole, logger *log.Logger) (*FirmataToggle, error) {
|
||||
cfg := GPIOToggleConfig{Enabled: true, GPIOPin: declaration.Pin, InitialOn: declaration.InitiallyOn, ActiveLow: declaration.ActiveLow}
|
||||
toggle := &FirmataToggle{
|
||||
cfg: cfg,
|
||||
name: name,
|
||||
client: peripheral.client,
|
||||
pin: byte(declaration.Pin),
|
||||
peripheralID: peripheral.metadata.ID,
|
||||
on: cfg.InitialOn,
|
||||
}
|
||||
if err := toggle.client.SetPinMode(toggle.pin, FirmataPinModeOutput); err != nil {
|
||||
return nil, fmt.Errorf("select Firmata output mode: %w", err)
|
||||
}
|
||||
if err := toggle.writeLocked(toggle.on); err != nil {
|
||||
return nil, fmt.Errorf("initialize Firmata output: %w", err)
|
||||
}
|
||||
logger.Printf("%s using ESP32 %s pin %d (initial=%v activeLow=%v)", name, peripheral.metadata.ID, declaration.Pin, cfg.InitialOn, cfg.ActiveLow)
|
||||
return toggle, nil
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) HandleAction(action string) error {
|
||||
toggle.mu.Lock()
|
||||
defer toggle.mu.Unlock()
|
||||
if toggle.closed {
|
||||
return errorsNewControllerClosed(toggle.name)
|
||||
}
|
||||
switch strings.ToLower(strings.TrimSpace(action)) {
|
||||
case "", "toggle":
|
||||
return toggle.setLocked(!toggle.on)
|
||||
case "on":
|
||||
return toggle.setLocked(true)
|
||||
case "off":
|
||||
return toggle.setLocked(false)
|
||||
default:
|
||||
return fmt.Errorf("unknown action %q", action)
|
||||
}
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) setLocked(on bool) error {
|
||||
if err := toggle.writeLocked(on); err != nil {
|
||||
return err
|
||||
}
|
||||
toggle.on = on
|
||||
return nil
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) writeLocked(on bool) error {
|
||||
physicalHigh := on
|
||||
if toggle.cfg.ActiveLow {
|
||||
physicalHigh = !physicalHigh
|
||||
}
|
||||
return toggle.client.SetDigitalPin(toggle.pin, physicalHigh)
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) On() bool {
|
||||
toggle.mu.Lock()
|
||||
defer toggle.mu.Unlock()
|
||||
return toggle.on
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) Configuration() GPIOToggleConfig {
|
||||
return toggle.cfg
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) BackendDescription() string {
|
||||
return "ESP32 " + toggle.peripheralID
|
||||
}
|
||||
|
||||
func (toggle *FirmataToggle) Close() {
|
||||
toggle.mu.Lock()
|
||||
defer toggle.mu.Unlock()
|
||||
toggle.closed = true
|
||||
}
|
||||
|
||||
func errorsNewControllerClosed(name string) error {
|
||||
return fmt.Errorf("%s controller closed", name)
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"context"
|
||||
"log"
|
||||
"strings"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestDisabledNativeRolesResolveToFirmataOnEveryHostBuild(t *testing.T) {
|
||||
description := PeripheralDescription{
|
||||
Name: "Rover GPIO",
|
||||
RoverControls: PeripheralRoverControls{
|
||||
CameraServo: &PeripheralCameraServo{
|
||||
Pin: 14, MinimumAngleDegrees: -15, MaximumAngleDegrees: 30,
|
||||
HomeAngleDegrees: 0, NudgeDegrees: 2,
|
||||
MinimumPulseMicroseconds: 900, MaximumPulseMicroseconds: 2100,
|
||||
},
|
||||
Headlight: &PeripheralDigitalRole{Pin: 18, ActiveLow: true, InitiallyOn: true},
|
||||
Laser: &PeripheralDigitalRole{Pin: 16, ActiveLow: false, InitiallyOn: false},
|
||||
},
|
||||
Controls: []PeripheralControl{},
|
||||
}
|
||||
connection := scriptedPeripheralConnection(t, description)
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/rover-gpio"}, map[string]*scriptedConnection{"/dev/rover-gpio": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
// The parent switch suppresses every native role even with saved child
|
||||
// settings enabled. No GPIO provider may displace the external peripheral.
|
||||
baseline := len(connection.Bytes())
|
||||
controllers, err := ResolveRoverHardwareControllers(&Config{PiGPIO: PiGPIOConfig{
|
||||
CameraServo: CameraServoConfig{Enabled: true}, Headlight: GPIOToggleConfig{Enabled: true}, Laser: GPIOToggleConfig{Enabled: true},
|
||||
}}, manager, discardLogger())
|
||||
if err != nil {
|
||||
t.Fatalf("resolve: %v", err)
|
||||
}
|
||||
defer controllers.Close()
|
||||
if controllers.CameraServo == nil || controllers.Headlight == nil || controllers.Laser == nil {
|
||||
t.Fatalf("missing Firmata controller: %#v", controllers)
|
||||
}
|
||||
if !controllers.CameraServo.Configuration().Enabled || !controllers.Headlight.Configuration().Enabled || !controllers.Laser.Configuration().Enabled {
|
||||
t.Fatal("ESP32-backed roles were not advertised as enabled")
|
||||
}
|
||||
wantHardwareBroadcast := "Rover hardware ready: camera servo via ESP32 firmata-0, headlight via ESP32 firmata-0, laser via ESP32 firmata-0."
|
||||
if messages := controllers.StartupBroadcasts(); len(messages) != 1 || messages[0] != wantHardwareBroadcast {
|
||||
t.Fatalf("hardware broadcasts = %#v, want %q", messages, wantHardwareBroadcast)
|
||||
}
|
||||
|
||||
// Initialization uses only standard Firmata: servo calibration and mode,
|
||||
// followed by the home position and digital initial states. The active-low
|
||||
// headlight starts logically on, so its physical output is low.
|
||||
writes := connection.Bytes()[baseline:]
|
||||
wantPrefix := []byte{
|
||||
firmataStartSysex, firmataServoConfig, 14, 4, 7, 52, 16, firmataEndSysex,
|
||||
firmataSetPinMode, 14, FirmataPinModeServo,
|
||||
firmataStartSysex, firmataExtendedAnalog, 14, 60, firmataEndSysex,
|
||||
firmataSetPinMode, 18, FirmataPinModeOutput,
|
||||
firmataSetDigitalPin, 18, 0,
|
||||
firmataSetPinMode, 16, FirmataPinModeOutput,
|
||||
firmataSetDigitalPin, 16, 0,
|
||||
}
|
||||
if !bytes.Equal(writes, wantPrefix) {
|
||||
t.Fatalf("initial controller bytes = %v, want %v", writes, wantPrefix)
|
||||
}
|
||||
|
||||
baseline = len(connection.Bytes())
|
||||
if err := controllers.Headlight.HandleAction("off"); err != nil {
|
||||
t.Fatalf("turn headlight off: %v", err)
|
||||
}
|
||||
if controllers.Headlight.On() {
|
||||
t.Fatal("headlight remained logically on")
|
||||
}
|
||||
// Active-low means logical off becomes a high electrical output.
|
||||
if got, want := connection.Bytes()[baseline:], []byte{firmataSetDigitalPin, 18, 1}; !bytes.Equal(got, want) {
|
||||
t.Fatalf("headlight bytes = %v, want %v", got, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestMissingNativeAndFirmataRolesRemainDisabled(t *testing.T) {
|
||||
manager := &PeripheralManager{byID: make(map[string]*managedPeripheral)}
|
||||
controllers, err := ResolveRoverHardwareControllers(&Config{}, manager, discardLogger())
|
||||
if err != nil {
|
||||
t.Fatalf("resolve: %v", err)
|
||||
}
|
||||
if controllers.CameraServo != nil || controllers.Headlight != nil || controllers.Laser != nil {
|
||||
t.Fatalf("unexpected controllers without providers: %#v", controllers)
|
||||
}
|
||||
}
|
||||
|
||||
func TestConflictingRolesRemainUnavailableWithoutInitializingGPIO(t *testing.T) {
|
||||
roleDescription := PeripheralDescription{RoverControls: PeripheralRoverControls{
|
||||
CameraServo: &PeripheralCameraServo{}, Headlight: &PeripheralDigitalRole{}, Laser: &PeripheralDigitalRole{},
|
||||
}}
|
||||
manager := &PeripheralManager{peripherals: []*managedPeripheral{
|
||||
{metadata: RoverPeripheralMetadata{ID: "firmata-0"}, description: roleDescription},
|
||||
}}
|
||||
cfg := &Config{PiGPIO: PiGPIOConfig{Enabled: true,
|
||||
CameraServo: CameraServoConfig{Enabled: true},
|
||||
Headlight: GPIOToggleConfig{Enabled: true}, Laser: GPIOToggleConfig{Enabled: true},
|
||||
}}
|
||||
factories := nativeHardwareControllerFactories{
|
||||
newCameraServo: func(CameraServoConfig, *log.Logger) (CameraServoController, error) {
|
||||
t.Fatal("conflicting GPIO servo must not initialize")
|
||||
return nil, nil
|
||||
},
|
||||
newToggle: func(string, GPIOToggleConfig, *log.Logger) (ToggleController, error) {
|
||||
t.Fatal("conflicting GPIO toggle must not initialize")
|
||||
return nil, nil
|
||||
},
|
||||
}
|
||||
controllers, err := resolveRoverHardwareControllers(cfg, manager, discardLogger(), factories)
|
||||
if err != nil {
|
||||
t.Fatal(err)
|
||||
}
|
||||
if controllers.CameraServo != nil || controllers.Headlight != nil || controllers.Laser != nil {
|
||||
t.Fatal("conflicting roles must remain unavailable")
|
||||
}
|
||||
if len(controllers.issues) != 3 {
|
||||
t.Fatalf("expected three role conflicts, got %v", controllers.issues)
|
||||
}
|
||||
for _, issue := range controllers.issues {
|
||||
if !strings.Contains(issue, "firmata-0, pi-gpio") {
|
||||
t.Fatalf("missing provider identities: %s", issue)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
type testCameraServoController struct {
|
||||
cfg CameraServoConfig
|
||||
}
|
||||
|
||||
func (controller *testCameraServoController) SetAngle(float64) error { return nil }
|
||||
func (controller *testCameraServoController) Nudge(float64) error { return nil }
|
||||
func (controller *testCameraServoController) SetPulseWidth(int) error { return nil }
|
||||
func (controller *testCameraServoController) CurrentAngle() float64 { return 0 }
|
||||
func (controller *testCameraServoController) Configuration() CameraServoConfig { return controller.cfg }
|
||||
func (controller *testCameraServoController) BackendDescription() string { return "native GPIO" }
|
||||
func (controller *testCameraServoController) Close() {}
|
||||
|
||||
type testToggleController struct {
|
||||
cfg GPIOToggleConfig
|
||||
on bool
|
||||
}
|
||||
|
||||
func (controller *testToggleController) HandleAction(string) error { return nil }
|
||||
func (controller *testToggleController) On() bool { return controller.on }
|
||||
func (controller *testToggleController) Configuration() GPIOToggleConfig { return controller.cfg }
|
||||
func (controller *testToggleController) BackendDescription() string { return "native GPIO" }
|
||||
func (controller *testToggleController) Close() {}
|
||||
@@ -0,0 +1,441 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"context"
|
||||
"encoding/json"
|
||||
"io"
|
||||
"reflect"
|
||||
"sync"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestFirmataParserHandlesFragmentedSysex(t *testing.T) {
|
||||
parser := FirmataParser{}
|
||||
|
||||
first, err := parser.Feed([]byte{firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescription, 1})
|
||||
if err != nil {
|
||||
t.Fatalf("first fragment: %v", err)
|
||||
}
|
||||
if len(first) != 0 {
|
||||
t.Fatalf("first fragment unexpectedly produced %d messages", len(first))
|
||||
}
|
||||
|
||||
second, err := parser.Feed([]byte{0, 2, 0, firmataEndSysex})
|
||||
if err != nil {
|
||||
t.Fatalf("second fragment: %v", err)
|
||||
}
|
||||
want := []FirmataMessage{{
|
||||
Command: firmataPeripheralFeature,
|
||||
Data: []byte{firmataPeripheralDescription, 1, 0, 2, 0},
|
||||
Sysex: true,
|
||||
}}
|
||||
if !reflect.DeepEqual(second, want) {
|
||||
t.Fatalf("messages = %#v, want %#v", second, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataParserReturnsSeveralMessagesFromOneRead(t *testing.T) {
|
||||
parser := FirmataParser{}
|
||||
messages, err := parser.Feed([]byte{
|
||||
firmataReportVersion, 2, 5,
|
||||
firmataStartSysex, firmataCapabilityReply, 0x01, 0x01, 0x7F, firmataEndSysex,
|
||||
firmataSetDigitalPin, 18, 1,
|
||||
})
|
||||
if err != nil {
|
||||
t.Fatalf("feed: %v", err)
|
||||
}
|
||||
if len(messages) != 3 {
|
||||
t.Fatalf("got %d messages, want 3", len(messages))
|
||||
}
|
||||
if messages[0].Command != firmataReportVersion || messages[1].Command != firmataCapabilityReply || messages[2].Command != firmataSetDigitalPin {
|
||||
t.Fatalf("commands were not preserved in wire order: %#v", messages)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataParserRejectsEightBitSysexDataAndRecovers(t *testing.T) {
|
||||
parser := FirmataParser{}
|
||||
if _, err := parser.Feed([]byte{firmataStartSysex, firmataPeripheralFeature, 0x80}); err == nil {
|
||||
t.Fatal("expected invalid SysEx data to fail")
|
||||
}
|
||||
|
||||
messages, err := parser.Feed([]byte{firmataReportVersion, 2, 5})
|
||||
if err != nil {
|
||||
t.Fatalf("feed after invalid SysEx: %v", err)
|
||||
}
|
||||
if len(messages) != 1 || messages[0].Command != firmataReportVersion {
|
||||
t.Fatalf("parser did not recover: %#v", messages)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataSevenBitRoundTripIncludesUTF8(t *testing.T) {
|
||||
raw := []byte(`{"name":"Café lights","value":255}`)
|
||||
encoded := EncodeFirmata7Bit(raw)
|
||||
for index, value := range encoded {
|
||||
if value&0x80 != 0 {
|
||||
t.Fatalf("encoded byte %d is not seven-bit clean: 0x%02x", index, value)
|
||||
}
|
||||
}
|
||||
decoded, err := DecodeFirmata7Bit(encoded)
|
||||
if err != nil {
|
||||
t.Fatalf("decode: %v", err)
|
||||
}
|
||||
if !bytes.Equal(decoded, raw) {
|
||||
t.Fatalf("decoded %q, want %q", decoded, raw)
|
||||
}
|
||||
}
|
||||
|
||||
func TestDecodeFirmataSevenBitRejectsMalformedPairs(t *testing.T) {
|
||||
for name, encoded := range map[string][]byte{
|
||||
"odd length": {1},
|
||||
"high byte": {1, 2},
|
||||
"eight bit": {0x80, 0},
|
||||
} {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
if _, err := DecodeFirmata7Bit(encoded); err == nil {
|
||||
t.Fatal("expected malformed pair to fail")
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralDescriptionPreservesControlOrder(t *testing.T) {
|
||||
raw := []byte(`{
|
||||
"name":"Test peripheral",
|
||||
"controls":[
|
||||
{"id":"servo","type":"slider","name":"Servo","min":0,"max":180,"output":{"type":"servo","pin":14}},
|
||||
{"id":"lights","type":"slider","name":"Lights","min":0,"max":255,"output":{"type":"pwm","pin":18}},
|
||||
{"id":"action","type":"button","name":"Action","mode":"momentary","output":{"type":"custom"}}
|
||||
]
|
||||
}`)
|
||||
var description PeripheralDescription
|
||||
if err := json.Unmarshal(raw, &description); err != nil {
|
||||
t.Fatalf("unmarshal: %v", err)
|
||||
}
|
||||
if err := description.Validate(); err != nil {
|
||||
t.Fatalf("validate: %v", err)
|
||||
}
|
||||
want := []string{"servo", "lights", "action"}
|
||||
for index, id := range want {
|
||||
if description.Controls[index].ID != id {
|
||||
t.Fatalf("control %d = %q, want %q", index, description.Controls[index].ID, id)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralDescriptionRejectsInvalidDeclarations(t *testing.T) {
|
||||
minimum, maximum, pin := 10, 1, 200
|
||||
for name, description := range map[string]PeripheralDescription{
|
||||
"duplicate id": {
|
||||
Name: "device",
|
||||
Controls: []PeripheralControl{
|
||||
{ID: "same", Name: "First", Type: "button", Mode: "toggle", Output: PeripheralOutput{Type: "custom"}},
|
||||
{ID: "same", Name: "Second", Type: "button", Mode: "toggle", Output: PeripheralOutput{Type: "custom"}},
|
||||
},
|
||||
},
|
||||
"reversed range": {
|
||||
Name: "device",
|
||||
Controls: []PeripheralControl{{
|
||||
ID: "level", Name: "Level", Type: "slider", Minimum: &minimum, Maximum: &maximum, Output: PeripheralOutput{Type: "custom"},
|
||||
}},
|
||||
},
|
||||
"pin outside Firmata": {
|
||||
Name: "device",
|
||||
Controls: []PeripheralControl{{
|
||||
ID: "switch", Name: "Switch", Type: "button", Mode: "toggle", Output: PeripheralOutput{Type: "digital", Pin: &pin},
|
||||
}},
|
||||
},
|
||||
} {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
if err := description.Validate(); err == nil {
|
||||
t.Fatal("expected invalid description to fail")
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestParseFirmataCapabilities(t *testing.T) {
|
||||
pins, err := parseFirmataCapabilities([]byte{
|
||||
FirmataPinModeOutput, 1, FirmataPinModePWM, 8, 0x7F,
|
||||
FirmataPinModeOutput, 1, FirmataPinModeServo, 14, 0x7F,
|
||||
})
|
||||
if err != nil {
|
||||
t.Fatalf("parse capabilities: %v", err)
|
||||
}
|
||||
if len(pins) != 2 || len(pins[0]) != 2 || pins[1][1].Mode != FirmataPinModeServo {
|
||||
t.Fatalf("unexpected capabilities: %#v", pins)
|
||||
}
|
||||
|
||||
if _, err := parseFirmataCapabilities([]byte{FirmataPinModeOutput}); err == nil {
|
||||
t.Fatal("expected incomplete capability pair to fail")
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientWritesStandardCommands(t *testing.T) {
|
||||
connection := &recordingConnection{}
|
||||
client := NewFirmataClient(connection)
|
||||
|
||||
if err := client.SetPinMode(14, FirmataPinModeServo); err != nil {
|
||||
t.Fatalf("set pin mode: %v", err)
|
||||
}
|
||||
if err := client.ConfigureServo(14, 900, 2100); err != nil {
|
||||
t.Fatalf("configure servo: %v", err)
|
||||
}
|
||||
if err := client.ExtendedAnalog(14, 180); err != nil {
|
||||
t.Fatalf("extended analog: %v", err)
|
||||
}
|
||||
if err := client.SetDigitalPin(19, true); err != nil {
|
||||
t.Fatalf("digital write: %v", err)
|
||||
}
|
||||
|
||||
want := []byte{
|
||||
firmataSetPinMode, 14, FirmataPinModeServo,
|
||||
firmataStartSysex, firmataServoConfig, 14, 4, 7, 52, 16, firmataEndSysex,
|
||||
firmataStartSysex, firmataExtendedAnalog, 14, 52, 1, firmataEndSysex,
|
||||
firmataSetDigitalPin, 19, 1,
|
||||
}
|
||||
if got := connection.Bytes(); !bytes.Equal(got, want) {
|
||||
t.Fatalf("wire bytes = %v, want %v", got, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientQueriesAndDecodesDescription(t *testing.T) {
|
||||
descriptionJSON := []byte(`{"name":"Bench device","controls":[{"id":"go","type":"button","name":"Go","mode":"momentary","output":{"type":"custom"}}]}`)
|
||||
firmwareName := EncodeFirmata7Bit([]byte("RoverPeripheralFirmata"))
|
||||
description := append([]byte{firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescription}, EncodeFirmata7Bit(descriptionJSON)...)
|
||||
description = append(description, firmataEndSysex)
|
||||
|
||||
connection := newScriptedConnection(
|
||||
append(append([]byte{firmataStartSysex, firmataReportFirmware, 1, 0}, firmwareName...), firmataEndSysex),
|
||||
description,
|
||||
)
|
||||
client := NewFirmataClient(connection)
|
||||
ctx, cancel := context.WithTimeout(context.Background(), time.Second)
|
||||
defer cancel()
|
||||
client.Start(ctx)
|
||||
|
||||
firmware, err := client.QueryFirmware(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("query firmware: %v", err)
|
||||
}
|
||||
if firmware.Name != "RoverPeripheralFirmata" || firmware.Major != 1 || firmware.Minor != 0 {
|
||||
t.Fatalf("unexpected firmware: %#v", firmware)
|
||||
}
|
||||
|
||||
got, err := client.Describe(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("describe: %v", err)
|
||||
}
|
||||
if got.Name != "Bench device" || len(got.Controls) != 1 || got.Controls[0].ID != "go" {
|
||||
t.Fatalf("unexpected description: %#v", got)
|
||||
}
|
||||
|
||||
writes := connection.Bytes()
|
||||
wantWrites := []byte{
|
||||
firmataStartSysex, firmataReportFirmware, firmataEndSysex,
|
||||
firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescribe, firmataEndSysex,
|
||||
}
|
||||
if !bytes.Equal(writes, wantWrites) {
|
||||
t.Fatalf("queries = %v, want %v", writes, wantWrites)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientKeepsReadingAfterSerialTimeoutEOF(t *testing.T) {
|
||||
firmwareName := EncodeFirmata7Bit([]byte("RoverPeripheralFirmata"))
|
||||
response := append([]byte{firmataStartSysex, firmataReportFirmware, 1, 0}, firmwareName...)
|
||||
response = append(response, firmataEndSysex)
|
||||
|
||||
// tarm/serial returns io.EOF when its ReadTimeout expires without bytes.
|
||||
// Reproducing that behavior before the response prevents this regression
|
||||
// from being hidden by an in-memory reader that blocks indefinitely instead.
|
||||
connection := newScriptedConnection(response)
|
||||
connection.timeoutsBeforeRead = 1
|
||||
client := NewFirmataClient(connection)
|
||||
ctx, cancel := context.WithTimeout(context.Background(), time.Second)
|
||||
defer cancel()
|
||||
client.Start(ctx)
|
||||
|
||||
firmware, err := client.QueryFirmware(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("query firmware after timeout: %v", err)
|
||||
}
|
||||
if firmware.Name != "RoverPeripheralFirmata" {
|
||||
t.Fatalf("firmware name = %q", firmware.Name)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientEncodesCustomControl(t *testing.T) {
|
||||
for name, testCase := range map[string]struct {
|
||||
controlID string
|
||||
value any
|
||||
wantJSON string
|
||||
}{
|
||||
"button": {controlID: "specialAction", value: true, wantJSON: `{"control":"specialAction","value":true}`},
|
||||
"text": {controlID: "displayText", value: "Café ready", wantJSON: `{"control":"displayText","value":"Café ready"}`},
|
||||
} {
|
||||
t.Run(name, func(t *testing.T) {
|
||||
connection := &recordingConnection{}
|
||||
client := NewFirmataClient(connection)
|
||||
if err := client.SendPeripheralControl(testCase.controlID, testCase.value); err != nil {
|
||||
t.Fatalf("send control: %v", err)
|
||||
}
|
||||
|
||||
wire := connection.Bytes()
|
||||
if len(wire) < 5 || wire[0] != firmataStartSysex || wire[1] != firmataPeripheralFeature || wire[2] != firmataPeripheralControl || wire[len(wire)-1] != firmataEndSysex {
|
||||
t.Fatalf("invalid control frame: %v", wire)
|
||||
}
|
||||
raw, err := DecodeFirmata7Bit(wire[3 : len(wire)-1])
|
||||
if err != nil {
|
||||
t.Fatalf("decode control: %v", err)
|
||||
}
|
||||
if string(raw) != testCase.wantJSON {
|
||||
t.Fatalf("control JSON = %s, want %s", raw, testCase.wantJSON)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientQueriesCapabilities(t *testing.T) {
|
||||
response := []byte{
|
||||
firmataStartSysex, firmataCapabilityReply,
|
||||
FirmataPinModeOutput, 1, FirmataPinModePWM, 8, 0x7F,
|
||||
FirmataPinModeOutput, 1, FirmataPinModeServo, 14, 0x7F,
|
||||
firmataEndSysex,
|
||||
}
|
||||
connection := newScriptedConnection(response)
|
||||
client := NewFirmataClient(connection)
|
||||
ctx, cancel := context.WithTimeout(context.Background(), time.Second)
|
||||
defer cancel()
|
||||
client.Start(ctx)
|
||||
|
||||
pins, err := client.QueryCapabilities(ctx)
|
||||
if err != nil {
|
||||
t.Fatalf("query capabilities: %v", err)
|
||||
}
|
||||
if len(pins) != 2 || pins[0][1].Mode != FirmataPinModePWM || pins[1][1].Mode != FirmataPinModeServo {
|
||||
t.Fatalf("unexpected capabilities: %#v", pins)
|
||||
}
|
||||
if want := []byte{firmataStartSysex, firmataCapabilityQuery, firmataEndSysex}; !bytes.Equal(connection.Bytes(), want) {
|
||||
t.Fatalf("query bytes = %v, want %v", connection.Bytes(), want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestFirmataClientRejectsControlTooLargeForFirmwareParser(t *testing.T) {
|
||||
connection := &recordingConnection{}
|
||||
client := NewFirmataClient(connection)
|
||||
if err := client.SendPeripheralControl("displayText", string(bytes.Repeat([]byte{'x'}, 200))); err == nil {
|
||||
t.Fatal("expected oversized control to fail")
|
||||
}
|
||||
if len(connection.Bytes()) != 0 {
|
||||
t.Fatalf("oversized control wrote bytes: %v", connection.Bytes())
|
||||
}
|
||||
}
|
||||
|
||||
// recordingConnection is deliberately minimal: write-focused tests should not
|
||||
// need goroutines or a real serial device merely to inspect exact Firmata bytes.
|
||||
type recordingConnection struct {
|
||||
mu sync.Mutex
|
||||
writes bytes.Buffer
|
||||
closed bool
|
||||
writeErr error
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Read(_ []byte) (int, error) { return 0, io.EOF }
|
||||
|
||||
func (connection *recordingConnection) Write(data []byte) (int, error) {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
if connection.closed {
|
||||
return 0, io.ErrClosedPipe
|
||||
}
|
||||
if connection.writeErr != nil {
|
||||
return 0, connection.writeErr
|
||||
}
|
||||
return connection.writes.Write(data)
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Close() error {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
connection.closed = true
|
||||
return nil
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Bytes() []byte {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
return append([]byte(nil), connection.writes.Bytes()...)
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) Closed() bool {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
return connection.closed
|
||||
}
|
||||
|
||||
func (connection *recordingConnection) SetWriteError(err error) {
|
||||
connection.mu.Lock()
|
||||
defer connection.mu.Unlock()
|
||||
connection.writeErr = err
|
||||
}
|
||||
|
||||
// scriptedConnection releases one response after each client write. This
|
||||
// mirrors request/response serial behavior and prevents a fast reader goroutine
|
||||
// from publishing all scripted answers before the matching query is sent.
|
||||
type scriptedConnection struct {
|
||||
recordingConnection
|
||||
responses chan []byte
|
||||
reads chan []byte
|
||||
timeoutsBeforeRead int
|
||||
pendingRead []byte
|
||||
closeOnce sync.Once
|
||||
}
|
||||
|
||||
func newScriptedConnection(responses ...[]byte) *scriptedConnection {
|
||||
connection := &scriptedConnection{
|
||||
responses: make(chan []byte, len(responses)),
|
||||
reads: make(chan []byte, len(responses)),
|
||||
}
|
||||
for _, response := range responses {
|
||||
connection.responses <- append([]byte(nil), response...)
|
||||
}
|
||||
return connection
|
||||
}
|
||||
|
||||
func (connection *scriptedConnection) Read(target []byte) (int, error) {
|
||||
if connection.timeoutsBeforeRead > 0 {
|
||||
connection.timeoutsBeforeRead--
|
||||
return 0, io.EOF
|
||||
}
|
||||
if len(connection.pendingRead) == 0 {
|
||||
response, ok := <-connection.reads
|
||||
if !ok {
|
||||
return 0, io.ErrClosedPipe
|
||||
}
|
||||
connection.pendingRead = response
|
||||
}
|
||||
written := copy(target, connection.pendingRead)
|
||||
connection.pendingRead = connection.pendingRead[written:]
|
||||
return written, nil
|
||||
}
|
||||
|
||||
func (connection *scriptedConnection) Write(data []byte) (int, error) {
|
||||
written, err := connection.recordingConnection.Write(data)
|
||||
if err == nil {
|
||||
select {
|
||||
case response := <-connection.responses:
|
||||
connection.reads <- response
|
||||
default:
|
||||
}
|
||||
}
|
||||
return written, err
|
||||
}
|
||||
|
||||
func (connection *scriptedConnection) Close() error {
|
||||
connection.closeOnce.Do(func() {
|
||||
_ = connection.recordingConnection.Close()
|
||||
close(connection.reads)
|
||||
})
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
module multiroombarover/rover/roverd
|
||||
|
||||
go 1.25.4
|
||||
|
||||
require (
|
||||
github.com/invopop/jsonschema v0.13.0
|
||||
github.com/santhosh-tekuri/jsonschema/v6 v6.0.2
|
||||
github.com/stianeikeland/go-rpio/v4 v4.6.0
|
||||
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07
|
||||
github.com/warthog618/go-gpiocdev v0.9.1
|
||||
nhooyr.io/websocket v1.8.17
|
||||
)
|
||||
|
||||
require (
|
||||
github.com/bahlo/generic-list-go v0.2.0 // indirect
|
||||
github.com/buger/jsonparser v1.1.1 // indirect
|
||||
github.com/mailru/easyjson v0.7.7 // indirect
|
||||
github.com/wk8/go-ordered-map/v2 v2.1.8 // indirect
|
||||
golang.org/x/sys v0.38.0 // indirect
|
||||
golang.org/x/text v0.14.0 // indirect
|
||||
gopkg.in/yaml.v3 v3.0.1 // indirect
|
||||
)
|
||||
@@ -0,0 +1,41 @@
|
||||
github.com/bahlo/generic-list-go v0.2.0 h1:5sz/EEAK+ls5wF+NeqDpk5+iNdMDXrh3z3nPnH1Wvgk=
|
||||
github.com/bahlo/generic-list-go v0.2.0/go.mod h1:2KvAjgMlE5NNynlg/5iLrrCCZ2+5xWbdbCW3pNTGyYg=
|
||||
github.com/buger/jsonparser v1.1.1 h1:2PnMjfWD7wBILjqQbt530v576A/cAbQvEW9gGIpYMUs=
|
||||
github.com/buger/jsonparser v1.1.1/go.mod h1:6RYKKt7H4d4+iWqouImQ9R2FZql3VbhNgx27UK13J/0=
|
||||
github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c=
|
||||
github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
|
||||
github.com/dlclark/regexp2 v1.11.0 h1:G/nrcoOa7ZXlpoa/91N3X7mM3r8eIlMBBJZvsz/mxKI=
|
||||
github.com/dlclark/regexp2 v1.11.0/go.mod h1:DHkYz0B9wPfa6wondMfaivmHpzrQ3v9q8cnmRbL6yW8=
|
||||
github.com/invopop/jsonschema v0.13.0 h1:KvpoAJWEjR3uD9Kbm2HWJmqsEaHt8lBUpd0qHcIi21E=
|
||||
github.com/invopop/jsonschema v0.13.0/go.mod h1:ffZ5Km5SWWRAIN6wbDXItl95euhFz2uON45H2qjYt+0=
|
||||
github.com/josharian/intern v1.0.0/go.mod h1:5DoeVV0s6jJacbCEi61lwdGj/aVlrQvzHFFd8Hwg//Y=
|
||||
github.com/mailru/easyjson v0.7.7 h1:UGYAvKxe3sBsEDzO8ZeWOSlIQfWFlxbzLZe7hwFURr0=
|
||||
github.com/mailru/easyjson v0.7.7/go.mod h1:xzfreul335JAWq5oZzymOObrkdz5UnU4kGfJJLY9Nlc=
|
||||
github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4=
|
||||
github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0=
|
||||
github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM=
|
||||
github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4=
|
||||
github.com/santhosh-tekuri/jsonschema/v6 v6.0.2 h1:KRzFb2m7YtdldCEkzs6KqmJw4nqEVZGK7IN2kJkjTuQ=
|
||||
github.com/santhosh-tekuri/jsonschema/v6 v6.0.2/go.mod h1:JXeL+ps8p7/KNMjDQk3TCwPpBy0wYklyWTfbkIzdIFU=
|
||||
github.com/stianeikeland/go-rpio/v4 v4.6.0 h1:eAJgtw3jTtvn/CqwbC82ntcS+dtzUTgo5qlZKe677EY=
|
||||
github.com/stianeikeland/go-rpio/v4 v4.6.0/go.mod h1:A3GvHxC1Om5zaId+HqB3HKqx4K/AqeckxB7qRjxMK7o=
|
||||
github.com/stretchr/testify v1.9.0 h1:HtqpIVDClZ4nwg75+f6Lvsy/wHu+3BoSGCbBAcpTsTg=
|
||||
github.com/stretchr/testify v1.9.0/go.mod h1:r2ic/lqez/lEtzL7wO/rwa5dbSLXVDPFyf8C91i36aY=
|
||||
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07 h1:UyzmZLoiDWMRywV4DUYb9Fbt8uiOSooupjTq10vpvnU=
|
||||
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07/go.mod h1:kDXzergiv9cbyO7IOYJZWg1U88JhDg3PB6klq9Hg2pA=
|
||||
github.com/warthog618/go-gpiocdev v0.9.1 h1:pwHPaqjJfhCipIQl78V+O3l9OKHivdRDdmgXYbmhuCI=
|
||||
github.com/warthog618/go-gpiocdev v0.9.1/go.mod h1:dN3e3t/S2aSNC+hgigGE/dBW8jE1ONk9bDSEYfoPyl8=
|
||||
github.com/warthog618/go-gpiosim v0.1.1 h1:MRAEv+T+itmw+3GeIGpQJBfanUVyg0l3JCTwHtwdre4=
|
||||
github.com/warthog618/go-gpiosim v0.1.1/go.mod h1:YXsnB+I9jdCMY4YAlMSRrlts25ltjmuIsrnoUrBLdqU=
|
||||
github.com/wk8/go-ordered-map/v2 v2.1.8 h1:5h/BUHu93oj4gIdvHHHGsScSTMijfx5PeYkE/fJgbpc=
|
||||
github.com/wk8/go-ordered-map/v2 v2.1.8/go.mod h1:5nJHM5DyteebpVlHnWMV0rPz6Zp7+xBAnxjb1X5vnTw=
|
||||
golang.org/x/sys v0.38.0 h1:3yZWxaJjBmCWXqhN1qh02AkOnCQ1poK6oF+a7xWL6Gc=
|
||||
golang.org/x/sys v0.38.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks=
|
||||
golang.org/x/text v0.14.0 h1:ScX5w1eTa3QqT8oi6+ziP7dTV1S2+ALU0bI+0zXKWiQ=
|
||||
golang.org/x/text v0.14.0/go.mod h1:18ZOQIKpY8NJVqYksKHtTdi31H5itFRjB5/qKTNYzSU=
|
||||
gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405 h1:yhCVgyC4o1eVCa2tZl7eS0r+SDo693bJlVdllGtEeKM=
|
||||
gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0=
|
||||
gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA=
|
||||
gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM=
|
||||
nhooyr.io/websocket v1.8.17 h1:KEVeLJkUywCKVsnLIDlD/5gtayKp8VoCkksHCGGfT9Y=
|
||||
nhooyr.io/websocket v1.8.17/go.mod h1:rN9OFWIUwuxg4fR5tELlYC04bXYowCP9GX47ivo2l+c=
|
||||
@@ -0,0 +1,108 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"strings"
|
||||
"sync"
|
||||
|
||||
gpiocdev "github.com/warthog618/go-gpiocdev"
|
||||
)
|
||||
|
||||
type GPIOToggle struct {
|
||||
cfg GPIOToggleConfig
|
||||
name string
|
||||
logger *log.Logger
|
||||
line *gpiocdev.Line
|
||||
mu sync.Mutex
|
||||
on bool
|
||||
closed bool
|
||||
}
|
||||
|
||||
func NewGPIOToggle(name string, cfg GPIOToggleConfig, logger *log.Logger) (*GPIOToggle, error) {
|
||||
if !cfg.Enabled {
|
||||
return nil, fmt.Errorf("%s disabled", name)
|
||||
}
|
||||
chip := cfg.GPIOChip
|
||||
if chip == "" {
|
||||
chip = "gpiochip0"
|
||||
}
|
||||
line, err := gpiocdev.RequestLine(
|
||||
chip,
|
||||
cfg.GPIOPin,
|
||||
gpiocdev.AsOutput(cfg.LogicalToGPIO(cfg.InitialOn)),
|
||||
gpiocdev.WithConsumer(fmt.Sprintf("roverd-%s", name)),
|
||||
)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("gpio request: %w", err)
|
||||
}
|
||||
toggle := &GPIOToggle{
|
||||
cfg: cfg,
|
||||
name: name,
|
||||
logger: logger,
|
||||
line: line,
|
||||
on: cfg.InitialOn,
|
||||
}
|
||||
logger.Printf("%s on GPIO %d (initial=%v activeLow=%v)", name, cfg.GPIOPin, cfg.InitialOn, cfg.ActiveLow)
|
||||
return toggle, nil
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) Close() {
|
||||
g.mu.Lock()
|
||||
defer g.mu.Unlock()
|
||||
if g.closed {
|
||||
return
|
||||
}
|
||||
// Preserve the last logical state while closing the line. The daemon is not
|
||||
// trying to force a safety state here; it is only releasing the GPIO handle.
|
||||
_ = g.line.SetValue(g.cfg.LogicalToGPIO(g.on))
|
||||
g.line.Close()
|
||||
g.closed = true
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) HandleAction(action string) error {
|
||||
g.mu.Lock()
|
||||
defer g.mu.Unlock()
|
||||
if g.closed {
|
||||
return fmt.Errorf("%s controller closed", g.name)
|
||||
}
|
||||
act := strings.ToLower(strings.TrimSpace(action))
|
||||
switch act {
|
||||
case "", "toggle":
|
||||
return g.setLocked(!g.on)
|
||||
case "on":
|
||||
return g.setLocked(true)
|
||||
case "off":
|
||||
return g.setLocked(false)
|
||||
default:
|
||||
return fmt.Errorf("unknown action %q", action)
|
||||
}
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) On() bool {
|
||||
g.mu.Lock()
|
||||
defer g.mu.Unlock()
|
||||
return g.on
|
||||
}
|
||||
|
||||
// Configuration returns the native toggle behavior used in the rover hello.
|
||||
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
|
||||
return g.cfg
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) BackendDescription() string {
|
||||
return "native GPIO"
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) setLocked(on bool) error {
|
||||
// This is the only place a logical device state becomes an electrical GPIO
|
||||
// value. Hardware that turns on when pulled low sets activeLow in roverd
|
||||
// config; every caller above this layer still uses plain on/off semantics.
|
||||
if err := g.line.SetValue(g.cfg.LogicalToGPIO(on)); err != nil {
|
||||
return err
|
||||
}
|
||||
g.on = on
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
)
|
||||
|
||||
type GPIOToggle struct {
|
||||
name string
|
||||
}
|
||||
|
||||
func NewGPIOToggle(name string, cfg GPIOToggleConfig, logger *log.Logger) (*GPIOToggle, error) {
|
||||
return nil, fmt.Errorf("%s not supported in dummy build", name)
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) Close() {}
|
||||
|
||||
func (g *GPIOToggle) HandleAction(action string) error {
|
||||
return fmt.Errorf("%s not supported in dummy build", g.name)
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) On() bool {
|
||||
return false
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
|
||||
return GPIOToggleConfig{}
|
||||
}
|
||||
|
||||
func (g *GPIOToggle) BackendDescription() string {
|
||||
return "native GPIO"
|
||||
}
|
||||
@@ -0,0 +1,137 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
// Both physical servo backends consume these exact motion constants. Keeping
|
||||
// them in shared code prevents Pi PWM and ESP32 Firmata movement from drifting
|
||||
// apart as either implementation evolves.
|
||||
const (
|
||||
maxServoDegPerSec = 60.0
|
||||
servoStepInterval = 20 * time.Millisecond
|
||||
servoAngleEpsilon = 0.01
|
||||
)
|
||||
|
||||
// CameraServoController is the hardware-neutral camera-tilt contract used by
|
||||
// WSClient. Native Pi PWM and ESP32 Firmata implementations expose identical
|
||||
// logical behavior, so command handling never branches on the rover host type.
|
||||
type CameraServoController interface {
|
||||
SetAngle(angle float64) error
|
||||
Nudge(delta float64) error
|
||||
SetPulseWidth(micros int) error
|
||||
CurrentAngle() float64
|
||||
Configuration() CameraServoConfig
|
||||
BackendDescription() string
|
||||
Close()
|
||||
}
|
||||
|
||||
// ToggleController keeps headlight and laser command/state behavior independent
|
||||
// of whether the electrical write happens on native Pi GPIO or an ESP32 pin.
|
||||
type ToggleController interface {
|
||||
HandleAction(action string) error
|
||||
On() bool
|
||||
Configuration() GPIOToggleConfig
|
||||
BackendDescription() string
|
||||
Close()
|
||||
}
|
||||
|
||||
// RoverHardwareControllers is the result of the single startup-time backend
|
||||
// decision. Effective configurations describe the selected role providers.
|
||||
type RoverHardwareControllers struct {
|
||||
CameraServo CameraServoController
|
||||
Headlight ToggleController
|
||||
Laser ToggleController
|
||||
issues []string
|
||||
}
|
||||
|
||||
// StartupBroadcasts returns short operator-facing messages. Detailed pin and
|
||||
// protocol information remains in the journal; tty1 reports selected providers
|
||||
// and unavailable roles.
|
||||
func (controllers RoverHardwareControllers) StartupBroadcasts() []string {
|
||||
var messages []string
|
||||
messages = append(messages, controllers.issues...)
|
||||
messages = append(messages, fmt.Sprintf(
|
||||
"Rover hardware ready: camera servo via %s, headlight via %s, laser via %s.",
|
||||
controllerBackend(controllers.CameraServo),
|
||||
controllerBackend(controllers.Headlight),
|
||||
controllerBackend(controllers.Laser),
|
||||
))
|
||||
return messages
|
||||
}
|
||||
|
||||
func controllerBackend(controller interface{ BackendDescription() string }) string {
|
||||
if controller == nil {
|
||||
return "disabled"
|
||||
}
|
||||
return controller.BackendDescription()
|
||||
}
|
||||
|
||||
type nativeHardwareControllerFactories struct {
|
||||
newCameraServo func(CameraServoConfig, *log.Logger) (CameraServoController, error)
|
||||
newToggle func(string, GPIOToggleConfig, *log.Logger) (ToggleController, error)
|
||||
}
|
||||
|
||||
// ResolveRoverHardwareControllers selects one provider per role, independent of
|
||||
// transport. Conflicts disable only the affected role and are reported locally.
|
||||
func ResolveRoverHardwareControllers(cfg *Config, peripherals *PeripheralManager, logger *log.Logger) (RoverHardwareControllers, error) {
|
||||
factories := nativeHardwareControllerFactories{
|
||||
newCameraServo: func(config CameraServoConfig, logger *log.Logger) (CameraServoController, error) {
|
||||
return NewCameraServo(config, logger)
|
||||
},
|
||||
newToggle: func(name string, config GPIOToggleConfig, logger *log.Logger) (ToggleController, error) {
|
||||
return NewGPIOToggle(name, config, logger)
|
||||
},
|
||||
}
|
||||
return resolveRoverHardwareControllers(cfg, peripherals, logger, factories)
|
||||
}
|
||||
|
||||
func resolveRoverHardwareControllers(cfg *Config, peripherals *PeripheralManager, logger *log.Logger, factories nativeHardwareControllerFactories) (RoverHardwareControllers, error) {
|
||||
var controllers RoverHardwareControllers
|
||||
if cfg.PiGPIO.Enabled {
|
||||
peripherals.piGPIO = &piGPIOPeripheral{cfg: cfg.PiGPIO, factories: factories}
|
||||
}
|
||||
// Resolving each role separately leaves unrelated hardware usable when a
|
||||
// declaration conflicts or an individual device cannot initialize.
|
||||
var err error
|
||||
report := func(role string, err error) {
|
||||
if err != nil {
|
||||
message := fmt.Sprintf("%s unavailable: %v", role, err)
|
||||
controllers.issues = append(controllers.issues, message)
|
||||
logger.Print(message)
|
||||
}
|
||||
}
|
||||
controllers.CameraServo, err = peripherals.NewCameraServo(logger)
|
||||
if err != nil {
|
||||
controllers.CameraServo = nil
|
||||
}
|
||||
report("camera servo", err)
|
||||
controllers.Headlight, err = peripherals.NewToggle("headlight", logger)
|
||||
if err != nil {
|
||||
controllers.Headlight = nil
|
||||
}
|
||||
report("headlight", err)
|
||||
controllers.Laser, err = peripherals.NewToggle("laser", logger)
|
||||
if err != nil {
|
||||
controllers.Laser = nil
|
||||
}
|
||||
report("laser", err)
|
||||
return controllers, nil
|
||||
}
|
||||
|
||||
// Close releases selected controller resources in reverse dependency order.
|
||||
// Firmata controllers do not close the shared serial connection; that remains
|
||||
// owned by PeripheralManager and is released by its separate shutdown defer.
|
||||
func (controllers *RoverHardwareControllers) Close() {
|
||||
if controllers.Laser != nil {
|
||||
controllers.Laser.Close()
|
||||
}
|
||||
if controllers.Headlight != nil {
|
||||
controllers.Headlight.Close()
|
||||
}
|
||||
if controllers.CameraServo != nil {
|
||||
controllers.CameraServo.Close()
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,270 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/binary"
|
||||
"fmt"
|
||||
"log"
|
||||
"math"
|
||||
"os/exec"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
const (
|
||||
hornAttack = 20 * time.Millisecond
|
||||
hornRelease = 60 * time.Millisecond
|
||||
)
|
||||
|
||||
type HornSynth struct {
|
||||
cfg HornConfig
|
||||
log *log.Logger
|
||||
gain float64
|
||||
|
||||
mu sync.Mutex
|
||||
stop chan struct{}
|
||||
active bool
|
||||
proc *exec.Cmd
|
||||
}
|
||||
|
||||
func NewHornSynth(cfg HornConfig, logger *log.Logger) *HornSynth {
|
||||
return &HornSynth{
|
||||
cfg: cfg,
|
||||
log: logger,
|
||||
gain: 1.0,
|
||||
}
|
||||
}
|
||||
|
||||
func (h *HornSynth) SetGlobalGain(gain float64) {
|
||||
h.mu.Lock()
|
||||
defer h.mu.Unlock()
|
||||
h.gain = clampAudioGain(gain)
|
||||
}
|
||||
|
||||
func (h *HornSynth) HandlePayload(payload *hornPayload) error {
|
||||
if payload == nil {
|
||||
return fmt.Errorf("horn payload required")
|
||||
}
|
||||
action := strings.ToLower(strings.TrimSpace(payload.Action))
|
||||
switch action {
|
||||
case "start", "on", "honk":
|
||||
waveform := strings.ToLower(strings.TrimSpace(payload.Waveform))
|
||||
if waveform != "sine" && waveform != "saw" {
|
||||
waveform = "saw"
|
||||
}
|
||||
freqs := sanitizeHornFreqs(payload.Freqs)
|
||||
if len(freqs) == 0 {
|
||||
h.Stop()
|
||||
return nil
|
||||
}
|
||||
return h.Start(waveform, freqs)
|
||||
case "stop", "off":
|
||||
h.Stop()
|
||||
return nil
|
||||
default:
|
||||
return fmt.Errorf("unsupported horn action: %s", payload.Action)
|
||||
}
|
||||
}
|
||||
|
||||
func (h *HornSynth) Start(waveform string, freqs []float64) error {
|
||||
h.mu.Lock()
|
||||
if h.active {
|
||||
h.mu.Unlock()
|
||||
return nil
|
||||
}
|
||||
stop := make(chan struct{})
|
||||
h.stop = stop
|
||||
h.active = true
|
||||
h.mu.Unlock()
|
||||
|
||||
go h.run(waveform, freqs, stop)
|
||||
return nil
|
||||
}
|
||||
|
||||
func (h *HornSynth) Stop() {
|
||||
h.mu.Lock()
|
||||
if !h.active {
|
||||
h.mu.Unlock()
|
||||
return
|
||||
}
|
||||
stop := h.stop
|
||||
proc := h.proc
|
||||
h.stop = nil
|
||||
h.proc = nil
|
||||
h.active = false
|
||||
h.mu.Unlock()
|
||||
|
||||
if stop != nil {
|
||||
close(stop)
|
||||
}
|
||||
if proc != nil && proc.Process != nil {
|
||||
_ = proc.Process.Kill()
|
||||
}
|
||||
}
|
||||
|
||||
func (h *HornSynth) run(waveform string, freqs []float64, stop <-chan struct{}) {
|
||||
rate := h.cfg.SampleRate
|
||||
if rate <= 0 {
|
||||
rate = 48000
|
||||
}
|
||||
channels := h.cfg.Channels
|
||||
if channels <= 0 {
|
||||
channels = 1
|
||||
}
|
||||
volume := h.cfg.Volume
|
||||
if volume <= 0 {
|
||||
volume = 0.25
|
||||
}
|
||||
if volume > 1 {
|
||||
volume = 1
|
||||
}
|
||||
h.mu.Lock()
|
||||
gain := h.gain
|
||||
h.mu.Unlock()
|
||||
volume *= gain
|
||||
|
||||
device := strings.TrimSpace(h.cfg.Device)
|
||||
if device == "" {
|
||||
device = "horn"
|
||||
}
|
||||
args := []string{"-q", "-D", device, "-f", "S16_LE", "-c", fmt.Sprintf("%d", channels), "-r", fmt.Sprintf("%d", rate), "-t", "raw"}
|
||||
cmd := exec.Command("aplay", args...)
|
||||
stdin, err := cmd.StdinPipe()
|
||||
if err != nil {
|
||||
h.log.Printf("horn: aplay stdin failed: %v", err)
|
||||
return
|
||||
}
|
||||
if err := cmd.Start(); err != nil {
|
||||
h.log.Printf("horn: aplay start failed: %v", err)
|
||||
_ = stdin.Close()
|
||||
return
|
||||
}
|
||||
|
||||
h.mu.Lock()
|
||||
if h.active {
|
||||
h.proc = cmd
|
||||
}
|
||||
h.mu.Unlock()
|
||||
|
||||
writer := bufio.NewWriterSize(stdin, 32*1024)
|
||||
maxFrames := 0
|
||||
if h.cfg.MaxDuration.Duration > 0 {
|
||||
maxFrames = int(float64(rate) * h.cfg.MaxDuration.Duration.Seconds())
|
||||
}
|
||||
if err := h.synthLoop(writer, waveform, freqs, rate, channels, volume, maxFrames, stop); err != nil {
|
||||
h.log.Printf("horn: synth failed: %v", err)
|
||||
}
|
||||
_ = writer.Flush()
|
||||
_ = stdin.Close()
|
||||
if err := cmd.Wait(); err != nil {
|
||||
h.log.Printf("horn: aplay exit: %v", err)
|
||||
}
|
||||
|
||||
h.mu.Lock()
|
||||
if h.proc == cmd {
|
||||
h.proc = nil
|
||||
}
|
||||
h.mu.Unlock()
|
||||
}
|
||||
|
||||
func (h *HornSynth) synthLoop(writer *bufio.Writer, waveform string, freqs []float64, rate, channels int, volume float64, maxFrames int, stop <-chan struct{}) error {
|
||||
phase := make([]float64, len(freqs))
|
||||
increment := make([]float64, len(freqs))
|
||||
for i, f := range freqs {
|
||||
increment[i] = 2 * math.Pi * f / float64(rate)
|
||||
}
|
||||
attackFrames := int(float64(rate) * hornAttack.Seconds())
|
||||
releaseFrames := int(float64(rate) * hornRelease.Seconds())
|
||||
framesPerChunk := 512
|
||||
buf := make([]byte, framesPerChunk*channels*2)
|
||||
scale := volume / float64(len(freqs))
|
||||
if waveform == "sine" {
|
||||
scale *= h.cfg.SineGain
|
||||
} else {
|
||||
scale *= h.cfg.SawGain
|
||||
}
|
||||
|
||||
stopRequested := false
|
||||
releaseStart := -1
|
||||
sampleIndex := 0
|
||||
|
||||
for {
|
||||
if !stopRequested {
|
||||
select {
|
||||
case <-stop:
|
||||
stopRequested = true
|
||||
releaseStart = sampleIndex
|
||||
default:
|
||||
}
|
||||
}
|
||||
for i := 0; i < framesPerChunk; i++ {
|
||||
if maxFrames > 0 && sampleIndex >= maxFrames && !stopRequested {
|
||||
stopRequested = true
|
||||
releaseStart = sampleIndex
|
||||
}
|
||||
env := 1.0
|
||||
if attackFrames > 0 && sampleIndex < attackFrames {
|
||||
env = float64(sampleIndex) / float64(attackFrames)
|
||||
} else if stopRequested && releaseFrames > 0 {
|
||||
relIndex := sampleIndex - releaseStart
|
||||
if relIndex >= releaseFrames {
|
||||
return nil
|
||||
}
|
||||
env = float64(releaseFrames-relIndex) / float64(releaseFrames)
|
||||
} else if stopRequested {
|
||||
return nil
|
||||
}
|
||||
|
||||
sample := 0.0
|
||||
for j := range freqs {
|
||||
switch waveform {
|
||||
case "sine":
|
||||
sample += math.Sin(phase[j])
|
||||
default:
|
||||
sample += sawFromPhase(phase[j])
|
||||
}
|
||||
phase[j] += increment[j]
|
||||
if phase[j] > 2*math.Pi {
|
||||
phase[j] -= 2 * math.Pi
|
||||
}
|
||||
}
|
||||
sample *= scale * env
|
||||
if sample > 1.0 {
|
||||
sample = 1.0
|
||||
} else if sample < -1.0 {
|
||||
sample = -1.0
|
||||
}
|
||||
intSample := int16(sample * math.MaxInt16)
|
||||
offset := i * channels * 2
|
||||
for ch := 0; ch < channels; ch++ {
|
||||
binary.LittleEndian.PutUint16(buf[offset+ch*2:], uint16(intSample))
|
||||
}
|
||||
sampleIndex++
|
||||
}
|
||||
if _, err := writer.Write(buf); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func sanitizeHornFreqs(freqs []float64) []float64 {
|
||||
if len(freqs) == 0 {
|
||||
return nil
|
||||
}
|
||||
out := make([]float64, 0, 4)
|
||||
for _, f := range freqs {
|
||||
if len(out) >= 4 {
|
||||
break
|
||||
}
|
||||
if f <= 0 {
|
||||
continue
|
||||
}
|
||||
out = append(out, f)
|
||||
}
|
||||
return out
|
||||
}
|
||||
|
||||
func sawFromPhase(phase float64) float64 {
|
||||
return 2.0*(phase/(2*math.Pi)) - 1.0
|
||||
}
|
||||
@@ -0,0 +1,638 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"context"
|
||||
"errors"
|
||||
"fmt"
|
||||
"math"
|
||||
"os"
|
||||
"os/exec"
|
||||
"strconv"
|
||||
"strings"
|
||||
"syscall"
|
||||
"time"
|
||||
)
|
||||
|
||||
const (
|
||||
hostStatsInterval = 1 * time.Second
|
||||
rootFilesystem = "/"
|
||||
)
|
||||
|
||||
// HostStats is the wire format sent from roverd to the server. It avoids
|
||||
// Roomba-specific sensor naming so the browser can treat these values as Pi
|
||||
// host health, not robot telemetry.
|
||||
type HostStats struct {
|
||||
UptimeSec *float64 `json:"uptimeSec,omitempty"`
|
||||
CPUTempC *float64 `json:"cpuTempC,omitempty"`
|
||||
CPUUsedPct *float64 `json:"cpuUsedPct,omitempty"`
|
||||
CoreVoltageV *float64 `json:"coreVoltageV,omitempty"`
|
||||
LoadAvg1m *float64 `json:"loadAvg1m,omitempty"`
|
||||
MemoryTotalKb *uint64 `json:"memoryTotalKb,omitempty"`
|
||||
MemoryAvailableKb *uint64 `json:"memoryAvailableKb,omitempty"`
|
||||
MemoryUsedPct *float64 `json:"memoryUsedPct,omitempty"`
|
||||
DiskTotalBytes *uint64 `json:"diskTotalBytes,omitempty"`
|
||||
DiskFreeBytes *uint64 `json:"diskFreeBytes,omitempty"`
|
||||
DiskUsedPct *float64 `json:"diskUsedPct,omitempty"`
|
||||
WiFi *WiFiStats `json:"wifi,omitempty"`
|
||||
ThrottledRaw string `json:"throttledRaw,omitempty"`
|
||||
UndervoltageNow *bool `json:"undervoltageNow,omitempty"`
|
||||
UndervoltageSeen *bool `json:"undervoltageSeen,omitempty"`
|
||||
FrequencyCapNow *bool `json:"frequencyCappedNow,omitempty"`
|
||||
FrequencyCapSeen *bool `json:"frequencyCappedSeen,omitempty"`
|
||||
ThrottledNow *bool `json:"throttledNow,omitempty"`
|
||||
ThrottledSeen *bool `json:"throttledSeen,omitempty"`
|
||||
SoftTempLimitNow *bool `json:"softTempLimitNow,omitempty"`
|
||||
SoftTempLimitSeen *bool `json:"softTempLimitSeen,omitempty"`
|
||||
Errors map[string]string `json:"errors,omitempty"`
|
||||
}
|
||||
|
||||
// WiFiStats contains only network details that are useful in the UI. The
|
||||
// wireless interface name is intentionally not part of this struct because the
|
||||
// rover fleet has one active Wi-Fi interface and the user asked not to display
|
||||
// it.
|
||||
type WiFiStats struct {
|
||||
SSIDSample string `json:"ssidSample,omitempty"`
|
||||
FrequencyMhz *int `json:"frequencyMhz,omitempty"`
|
||||
SignalDbm *float64 `json:"signalDbm,omitempty"`
|
||||
Quality *float64 `json:"quality,omitempty"`
|
||||
QualityMax *float64 `json:"qualityMax,omitempty"`
|
||||
NoiseDbm *float64 `json:"noiseDbm,omitempty"`
|
||||
RXBitrateMbit *float64 `json:"rxBitrateMbit,omitempty"`
|
||||
TXBitrateMbit *float64 `json:"txBitrateMbit,omitempty"`
|
||||
RXBytes *uint64 `json:"rxBytes,omitempty"`
|
||||
TXBytes *uint64 `json:"txBytes,omitempty"`
|
||||
RXPackets *uint64 `json:"rxPackets,omitempty"`
|
||||
TXPackets *uint64 `json:"txPackets,omitempty"`
|
||||
DownloadMbps *float64 `json:"downloadMbps,omitempty"`
|
||||
UploadMbps *float64 `json:"uploadMbps,omitempty"`
|
||||
InactiveMs *int `json:"inactiveMs,omitempty"`
|
||||
|
||||
// networkSampledAt records the instant associated with the kernel byte
|
||||
// counters. Keeping it out of JSON lets the websocket loop calculate rates
|
||||
// with monotonic Go timestamps without expanding the browser contract with
|
||||
// an implementation-only value.
|
||||
networkSampledAt time.Time
|
||||
}
|
||||
|
||||
// networkRateSample is scoped to one rover websocket connection. A new
|
||||
// connection intentionally starts a new baseline so counters from an old boot
|
||||
// or network interface lifetime can never create an artificial traffic spike.
|
||||
type networkRateSample struct {
|
||||
rxBytes uint64
|
||||
txBytes uint64
|
||||
sampledAt time.Time
|
||||
}
|
||||
|
||||
// CollectHostStats gathers every source independently so one missing kernel
|
||||
// file, firmware utility, or Wi-Fi utility does not suppress the remaining Pi
|
||||
// health data.
|
||||
func CollectHostStats(ctx context.Context) HostStats {
|
||||
stats := HostStats{Errors: map[string]string{}}
|
||||
|
||||
if uptime, err := readUptimeSec(); err != nil {
|
||||
stats.addError("uptime", err)
|
||||
} else {
|
||||
stats.UptimeSec = &uptime
|
||||
}
|
||||
|
||||
if temp, err := readCPUTempC(); err != nil {
|
||||
stats.addError("cpuTemp", err)
|
||||
} else {
|
||||
stats.CPUTempC = &temp
|
||||
}
|
||||
|
||||
if cpuUsedPct, err := readCPUUsedPct(); err != nil {
|
||||
stats.addError("cpuUsed", err)
|
||||
} else {
|
||||
stats.CPUUsedPct = &cpuUsedPct
|
||||
}
|
||||
|
||||
if load, err := readLoadAvg1m(); err != nil {
|
||||
stats.addError("loadAvg", err)
|
||||
} else {
|
||||
stats.LoadAvg1m = &load
|
||||
}
|
||||
|
||||
if memTotal, memAvailable, err := readMemoryKb(); err != nil {
|
||||
stats.addError("memory", err)
|
||||
} else {
|
||||
stats.MemoryTotalKb = &memTotal
|
||||
stats.MemoryAvailableKb = &memAvailable
|
||||
if memTotal > 0 {
|
||||
usedPct := roundOneDecimal(float64(memTotal-memAvailable) / float64(memTotal) * 100)
|
||||
stats.MemoryUsedPct = &usedPct
|
||||
}
|
||||
}
|
||||
|
||||
if diskTotal, diskFree, err := readDiskBytes(rootFilesystem); err != nil {
|
||||
stats.addError("disk", err)
|
||||
} else {
|
||||
stats.DiskTotalBytes = &diskTotal
|
||||
stats.DiskFreeBytes = &diskFree
|
||||
if diskTotal > 0 {
|
||||
usedPct := roundOneDecimal(float64(diskTotal-diskFree) / float64(diskTotal) * 100)
|
||||
stats.DiskUsedPct = &usedPct
|
||||
}
|
||||
}
|
||||
|
||||
if coreV, err := readCoreVoltage(ctx); err != nil {
|
||||
stats.addError("coreVoltage", err)
|
||||
} else {
|
||||
stats.CoreVoltageV = &coreV
|
||||
}
|
||||
|
||||
if raw, flags, err := readThrottleFlags(ctx); err != nil {
|
||||
stats.addError("throttled", err)
|
||||
} else {
|
||||
stats.ThrottledRaw = raw
|
||||
stats.applyThrottleFlags(flags)
|
||||
}
|
||||
|
||||
if wifi, err := collectWiFiStats(ctx); err != nil {
|
||||
stats.addError("wifi", err)
|
||||
if wifi != nil {
|
||||
stats.WiFi = wifi
|
||||
}
|
||||
} else {
|
||||
stats.WiFi = wifi
|
||||
}
|
||||
|
||||
if len(stats.Errors) == 0 {
|
||||
stats.Errors = nil
|
||||
}
|
||||
return stats
|
||||
}
|
||||
|
||||
func (s *HostStats) addError(source string, err error) {
|
||||
if err == nil {
|
||||
return
|
||||
}
|
||||
if s.Errors == nil {
|
||||
s.Errors = map[string]string{}
|
||||
}
|
||||
s.Errors[source] = err.Error()
|
||||
}
|
||||
|
||||
func (s *HostStats) applyThrottleFlags(flags uint64) {
|
||||
undervoltageNow := flags&(1<<0) != 0
|
||||
frequencyCapNow := flags&(1<<1) != 0
|
||||
throttledNow := flags&(1<<2) != 0
|
||||
softTempLimitNow := flags&(1<<3) != 0
|
||||
undervoltageSeen := flags&(1<<16) != 0
|
||||
frequencyCapSeen := flags&(1<<17) != 0
|
||||
throttledSeen := flags&(1<<18) != 0
|
||||
softTempLimitSeen := flags&(1<<19) != 0
|
||||
|
||||
s.UndervoltageNow = &undervoltageNow
|
||||
s.FrequencyCapNow = &frequencyCapNow
|
||||
s.ThrottledNow = &throttledNow
|
||||
s.SoftTempLimitNow = &softTempLimitNow
|
||||
s.UndervoltageSeen = &undervoltageSeen
|
||||
s.FrequencyCapSeen = &frequencyCapSeen
|
||||
s.ThrottledSeen = &throttledSeen
|
||||
s.SoftTempLimitSeen = &softTempLimitSeen
|
||||
}
|
||||
|
||||
func readUptimeSec() (float64, error) {
|
||||
raw, err := os.ReadFile("/proc/uptime")
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
fields := strings.Fields(string(raw))
|
||||
if len(fields) == 0 {
|
||||
return 0, fmt.Errorf("missing uptime value")
|
||||
}
|
||||
return strconv.ParseFloat(fields[0], 64)
|
||||
}
|
||||
|
||||
func readCPUTempC() (float64, error) {
|
||||
raw, err := os.ReadFile("/sys/class/thermal/thermal_zone0/temp")
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
milliC, err := strconv.ParseFloat(strings.TrimSpace(string(raw)), 64)
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
return roundOneDecimal(milliC / 1000), nil
|
||||
}
|
||||
|
||||
func readCPUUsedPct() (float64, error) {
|
||||
first, err := readCPUTimeSample()
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
|
||||
// CPU utilization is a rate, not a directly stored value. Sampling twice
|
||||
// over a short window gives the UI a true percentage without carrying global
|
||||
// collector state between websocket messages.
|
||||
time.Sleep(150 * time.Millisecond)
|
||||
|
||||
second, err := readCPUTimeSample()
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
totalDelta := second.total - first.total
|
||||
idleDelta := second.idle - first.idle
|
||||
if totalDelta == 0 {
|
||||
return 0, fmt.Errorf("zero CPU sample delta")
|
||||
}
|
||||
usedPct := (1 - (float64(idleDelta) / float64(totalDelta))) * 100
|
||||
return roundOneDecimal(usedPct), nil
|
||||
}
|
||||
|
||||
type cpuTimeSample struct {
|
||||
total uint64
|
||||
idle uint64
|
||||
}
|
||||
|
||||
func readCPUTimeSample() (cpuTimeSample, error) {
|
||||
raw, err := os.ReadFile("/proc/stat")
|
||||
if err != nil {
|
||||
return cpuTimeSample{}, err
|
||||
}
|
||||
lines := strings.Split(string(raw), "\n")
|
||||
if len(lines) == 0 {
|
||||
return cpuTimeSample{}, fmt.Errorf("missing /proc/stat cpu line")
|
||||
}
|
||||
fields := strings.Fields(lines[0])
|
||||
if len(fields) < 5 || fields[0] != "cpu" {
|
||||
return cpuTimeSample{}, fmt.Errorf("invalid /proc/stat cpu line")
|
||||
}
|
||||
|
||||
var values []uint64
|
||||
for _, field := range fields[1:] {
|
||||
value, err := strconv.ParseUint(field, 10, 64)
|
||||
if err != nil {
|
||||
return cpuTimeSample{}, err
|
||||
}
|
||||
values = append(values, value)
|
||||
}
|
||||
|
||||
var total uint64
|
||||
for _, value := range values {
|
||||
total += value
|
||||
}
|
||||
|
||||
// Linux reports idle time as idle+iowait. Treating iowait as idle matches
|
||||
// common CPU usage tools and avoids making disk waits look like CPU work.
|
||||
idle := values[3]
|
||||
if len(values) > 4 {
|
||||
idle += values[4]
|
||||
}
|
||||
return cpuTimeSample{total: total, idle: idle}, nil
|
||||
}
|
||||
|
||||
func readLoadAvg1m() (float64, error) {
|
||||
raw, err := os.ReadFile("/proc/loadavg")
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
fields := strings.Fields(string(raw))
|
||||
if len(fields) == 0 {
|
||||
return 0, fmt.Errorf("missing load average value")
|
||||
}
|
||||
load, err := strconv.ParseFloat(fields[0], 64)
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
return roundTwoDecimals(load), nil
|
||||
}
|
||||
|
||||
func readMemoryKb() (uint64, uint64, error) {
|
||||
file, err := os.Open("/proc/meminfo")
|
||||
if err != nil {
|
||||
return 0, 0, err
|
||||
}
|
||||
defer file.Close()
|
||||
|
||||
var total uint64
|
||||
var available uint64
|
||||
scanner := bufio.NewScanner(file)
|
||||
for scanner.Scan() {
|
||||
key, value, ok := parseMeminfoLine(scanner.Text())
|
||||
if !ok {
|
||||
continue
|
||||
}
|
||||
switch key {
|
||||
case "MemTotal":
|
||||
total = value
|
||||
case "MemAvailable":
|
||||
available = value
|
||||
}
|
||||
}
|
||||
if err := scanner.Err(); err != nil {
|
||||
return 0, 0, err
|
||||
}
|
||||
if total == 0 || available == 0 {
|
||||
return 0, 0, fmt.Errorf("missing MemTotal or MemAvailable")
|
||||
}
|
||||
return total, available, nil
|
||||
}
|
||||
|
||||
func parseMeminfoLine(line string) (string, uint64, bool) {
|
||||
parts := strings.Fields(line)
|
||||
if len(parts) < 2 {
|
||||
return "", 0, false
|
||||
}
|
||||
value, err := strconv.ParseUint(parts[1], 10, 64)
|
||||
if err != nil {
|
||||
return "", 0, false
|
||||
}
|
||||
return strings.TrimSuffix(parts[0], ":"), value, true
|
||||
}
|
||||
|
||||
func readDiskBytes(path string) (uint64, uint64, error) {
|
||||
var fs syscall.Statfs_t
|
||||
if err := syscall.Statfs(path, &fs); err != nil {
|
||||
return 0, 0, err
|
||||
}
|
||||
total := fs.Blocks * uint64(fs.Bsize)
|
||||
free := fs.Bavail * uint64(fs.Bsize)
|
||||
return total, free, nil
|
||||
}
|
||||
|
||||
func readCoreVoltage(ctx context.Context) (float64, error) {
|
||||
out, err := runCommand(ctx, "vcgencmd", "measure_volts", "core")
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
raw := strings.TrimSpace(out)
|
||||
raw = strings.TrimPrefix(raw, "volt=")
|
||||
raw = strings.TrimSuffix(raw, "V")
|
||||
value, err := strconv.ParseFloat(raw, 64)
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
return roundThreeDecimals(value), nil
|
||||
}
|
||||
|
||||
func readThrottleFlags(ctx context.Context) (string, uint64, error) {
|
||||
out, err := runCommand(ctx, "vcgencmd", "get_throttled")
|
||||
if err != nil {
|
||||
return "", 0, err
|
||||
}
|
||||
raw := strings.TrimSpace(out)
|
||||
valueText := strings.TrimPrefix(raw, "throttled=")
|
||||
flags, err := strconv.ParseUint(valueText, 0, 64)
|
||||
if err != nil {
|
||||
return raw, 0, err
|
||||
}
|
||||
return valueText, flags, nil
|
||||
}
|
||||
|
||||
func collectWiFiStats(ctx context.Context) (*WiFiStats, error) {
|
||||
iface, stats, err := readWirelessStats()
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
// The interface is used only for local collection. It is not copied into
|
||||
// WiFiStats because the UI does not need to expose Linux device names.
|
||||
iwErr := enrichWiFiWithIW(ctx, iface, stats)
|
||||
|
||||
// Read the kernel counters after iw because iw also provides cumulative
|
||||
// station counters. The kernel interface values deliberately win: they are
|
||||
// the host-traffic source used for both the cumulative display and Mbps math.
|
||||
// Link capacity still comes independently from iw's bitrate fields.
|
||||
counterErr := enrichWiFiWithNetworkCounters(iface, stats)
|
||||
return stats, errors.Join(counterErr, iwErr)
|
||||
}
|
||||
|
||||
func enrichWiFiWithNetworkCounters(iface string, stats *WiFiStats) error {
|
||||
basePath := "/sys/class/net/" + iface + "/statistics/"
|
||||
rxBytes, err := readUintFile(basePath + "rx_bytes")
|
||||
if err != nil {
|
||||
return fmt.Errorf("read %s receive bytes: %w", iface, err)
|
||||
}
|
||||
txBytes, err := readUintFile(basePath + "tx_bytes")
|
||||
if err != nil {
|
||||
return fmt.Errorf("read %s transmit bytes: %w", iface, err)
|
||||
}
|
||||
|
||||
stats.RXBytes = &rxBytes
|
||||
stats.TXBytes = &txBytes
|
||||
// Capture the timestamp immediately beside the counter reads so unrelated
|
||||
// host-stat collection latency cannot distort the elapsed-time divisor.
|
||||
stats.networkSampledAt = time.Now()
|
||||
return nil
|
||||
}
|
||||
|
||||
func readUintFile(path string) (uint64, error) {
|
||||
raw, err := os.ReadFile(path)
|
||||
if err != nil {
|
||||
return 0, err
|
||||
}
|
||||
return strconv.ParseUint(strings.TrimSpace(string(raw)), 10, 64)
|
||||
}
|
||||
|
||||
func applyNetworkThroughput(stats *WiFiStats, previous *networkRateSample) *networkRateSample {
|
||||
if stats == nil || stats.RXBytes == nil || stats.TXBytes == nil || stats.networkSampledAt.IsZero() {
|
||||
// Do not discard the last valid baseline during a temporary read failure.
|
||||
// The next successful calculation then covers the full elapsed interval and
|
||||
// remains an accurate average for all traffic transferred during the gap.
|
||||
return previous
|
||||
}
|
||||
|
||||
current := &networkRateSample{
|
||||
rxBytes: *stats.RXBytes,
|
||||
txBytes: *stats.TXBytes,
|
||||
sampledAt: stats.networkSampledAt,
|
||||
}
|
||||
if previous == nil {
|
||||
return current
|
||||
}
|
||||
|
||||
elapsed := current.sampledAt.Sub(previous.sampledAt).Seconds()
|
||||
// Linux counters can return to zero after an interface reset. Re-baselining
|
||||
// on any decrease prevents unsigned underflow from becoming a huge false
|
||||
// throughput spike in the host-stat card.
|
||||
if elapsed <= 0 || current.rxBytes < previous.rxBytes || current.txBytes < previous.txBytes {
|
||||
return current
|
||||
}
|
||||
|
||||
downloadMbps := bytesToMbps(current.rxBytes-previous.rxBytes, elapsed)
|
||||
uploadMbps := bytesToMbps(current.txBytes-previous.txBytes, elapsed)
|
||||
stats.DownloadMbps = &downloadMbps
|
||||
stats.UploadMbps = &uploadMbps
|
||||
return current
|
||||
}
|
||||
|
||||
func bytesToMbps(byteDelta uint64, elapsedSeconds float64) float64 {
|
||||
// Mbps uses decimal megabits, matching network equipment and link-rate
|
||||
// conventions: eight bits per byte and 1,000,000 bits per megabit.
|
||||
return roundOneDecimal((float64(byteDelta) * 8) / elapsedSeconds / 1_000_000)
|
||||
}
|
||||
|
||||
func readWirelessStats() (string, *WiFiStats, error) {
|
||||
file, err := os.Open("/proc/net/wireless")
|
||||
if err != nil {
|
||||
return "", nil, err
|
||||
}
|
||||
defer file.Close()
|
||||
|
||||
scanner := bufio.NewScanner(file)
|
||||
lineNumber := 0
|
||||
for scanner.Scan() {
|
||||
lineNumber++
|
||||
if lineNumber <= 2 {
|
||||
continue
|
||||
}
|
||||
iface, stats, ok := parseWirelessLine(scanner.Text())
|
||||
if ok {
|
||||
return iface, stats, nil
|
||||
}
|
||||
}
|
||||
if err := scanner.Err(); err != nil {
|
||||
return "", nil, err
|
||||
}
|
||||
return "", nil, fmt.Errorf("no active wireless stats")
|
||||
}
|
||||
|
||||
func parseWirelessLine(line string) (string, *WiFiStats, bool) {
|
||||
parts := strings.Fields(strings.TrimSpace(line))
|
||||
if len(parts) < 5 {
|
||||
return "", nil, false
|
||||
}
|
||||
iface := strings.TrimSuffix(parts[0], ":")
|
||||
if iface == "" {
|
||||
return "", nil, false
|
||||
}
|
||||
|
||||
stats := &WiFiStats{}
|
||||
if quality, err := parseWirelessFloat(parts[2]); err == nil {
|
||||
stats.Quality = &quality
|
||||
qualityMax := 70.0
|
||||
stats.QualityMax = &qualityMax
|
||||
}
|
||||
if signal, err := parseWirelessFloat(parts[3]); err == nil && signal > -200 {
|
||||
stats.SignalDbm = &signal
|
||||
}
|
||||
if noise, err := parseWirelessFloat(parts[4]); err == nil && noise > -200 {
|
||||
stats.NoiseDbm = &noise
|
||||
}
|
||||
return iface, stats, true
|
||||
}
|
||||
|
||||
func parseWirelessFloat(raw string) (float64, error) {
|
||||
return strconv.ParseFloat(strings.TrimSuffix(raw, "."), 64)
|
||||
}
|
||||
|
||||
func enrichWiFiWithIW(ctx context.Context, iface string, stats *WiFiStats) error {
|
||||
out, err := runCommand(ctx, "iw", "dev", iface, "link")
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
for _, line := range strings.Split(out, "\n") {
|
||||
parseIWLine(strings.TrimSpace(line), stats)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func parseIWLine(line string, stats *WiFiStats) {
|
||||
switch {
|
||||
case strings.HasPrefix(line, "SSID:"):
|
||||
ssid := strings.TrimSpace(strings.TrimPrefix(line, "SSID:"))
|
||||
stats.SSIDSample = sampleSSID(ssid)
|
||||
case strings.HasPrefix(line, "freq:"):
|
||||
if value, ok := parseFirstInt(strings.TrimSpace(strings.TrimPrefix(line, "freq:"))); ok {
|
||||
stats.FrequencyMhz = &value
|
||||
}
|
||||
case strings.HasPrefix(line, "signal:"):
|
||||
if value, ok := parseFirstFloat(strings.TrimSpace(strings.TrimPrefix(line, "signal:"))); ok {
|
||||
stats.SignalDbm = &value
|
||||
}
|
||||
case strings.HasPrefix(line, "rx bitrate:"):
|
||||
if value, ok := parseFirstFloat(strings.TrimSpace(strings.TrimPrefix(line, "rx bitrate:"))); ok {
|
||||
stats.RXBitrateMbit = &value
|
||||
}
|
||||
case strings.HasPrefix(line, "tx bitrate:"):
|
||||
if value, ok := parseFirstFloat(strings.TrimSpace(strings.TrimPrefix(line, "tx bitrate:"))); ok {
|
||||
stats.TXBitrateMbit = &value
|
||||
}
|
||||
case strings.HasPrefix(line, "inactive time:"):
|
||||
if value, ok := parseFirstInt(strings.TrimSpace(strings.TrimPrefix(line, "inactive time:"))); ok {
|
||||
stats.InactiveMs = &value
|
||||
}
|
||||
case strings.HasPrefix(line, "RX:"):
|
||||
parseIWPackedCounter(strings.TrimSpace(strings.TrimPrefix(line, "RX:")), &stats.RXBytes, &stats.RXPackets)
|
||||
case strings.HasPrefix(line, "TX:"):
|
||||
parseIWPackedCounter(strings.TrimSpace(strings.TrimPrefix(line, "TX:")), &stats.TXBytes, &stats.TXPackets)
|
||||
}
|
||||
}
|
||||
|
||||
func parseIWPackedCounter(raw string, bytesTarget **uint64, packetsTarget **uint64) {
|
||||
fields := strings.Fields(raw)
|
||||
if len(fields) == 0 {
|
||||
return
|
||||
}
|
||||
if bytes, err := strconv.ParseUint(fields[0], 10, 64); err == nil {
|
||||
*bytesTarget = &bytes
|
||||
}
|
||||
for idx, field := range fields {
|
||||
if strings.HasPrefix(field, "(") && idx+1 < len(fields) {
|
||||
packetText := strings.TrimPrefix(field, "(")
|
||||
if packets, err := strconv.ParseUint(packetText, 10, 64); err == nil {
|
||||
*packetsTarget = &packets
|
||||
}
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func sampleSSID(ssid string) string {
|
||||
runes := []rune(ssid)
|
||||
var builder strings.Builder
|
||||
for idx, r := range runes {
|
||||
if idx%2 == 0 {
|
||||
builder.WriteRune(r)
|
||||
}
|
||||
}
|
||||
return builder.String()
|
||||
}
|
||||
|
||||
func parseFirstFloat(raw string) (float64, bool) {
|
||||
fields := strings.Fields(raw)
|
||||
if len(fields) == 0 {
|
||||
return 0, false
|
||||
}
|
||||
value, err := strconv.ParseFloat(fields[0], 64)
|
||||
return value, err == nil
|
||||
}
|
||||
|
||||
func parseFirstInt(raw string) (int, bool) {
|
||||
fields := strings.Fields(raw)
|
||||
if len(fields) == 0 {
|
||||
return 0, false
|
||||
}
|
||||
value, err := strconv.Atoi(fields[0])
|
||||
return value, err == nil
|
||||
}
|
||||
|
||||
func runCommand(ctx context.Context, name string, args ...string) (string, error) {
|
||||
cmdCtx, cancel := context.WithTimeout(ctx, 1500*time.Millisecond)
|
||||
defer cancel()
|
||||
|
||||
cmd := exec.CommandContext(cmdCtx, name, args...)
|
||||
out, err := cmd.Output()
|
||||
if cmdCtx.Err() != nil {
|
||||
return "", cmdCtx.Err()
|
||||
}
|
||||
if err != nil {
|
||||
return "", err
|
||||
}
|
||||
return string(out), nil
|
||||
}
|
||||
|
||||
func roundOneDecimal(value float64) float64 {
|
||||
return math.Round(value*10) / 10
|
||||
}
|
||||
|
||||
func roundTwoDecimals(value float64) float64 {
|
||||
return math.Round(value*100) / 100
|
||||
}
|
||||
|
||||
func roundThreeDecimals(value float64) float64 {
|
||||
return math.Round(value*1000) / 1000
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestApplyNetworkThroughputCalculatesMbpsFromActualElapsedTime(t *testing.T) {
|
||||
startedAt := time.Unix(100, 0)
|
||||
previous := &networkRateSample{rxBytes: 1_000, txBytes: 2_000, sampledAt: startedAt}
|
||||
rxBytes := uint64(2_001_000)
|
||||
txBytes := uint64(1_002_000)
|
||||
stats := &WiFiStats{
|
||||
RXBytes: &rxBytes,
|
||||
TXBytes: &txBytes,
|
||||
networkSampledAt: startedAt.Add(2 * time.Second),
|
||||
}
|
||||
|
||||
next := applyNetworkThroughput(stats, previous)
|
||||
|
||||
if stats.DownloadMbps == nil || *stats.DownloadMbps != 8.0 {
|
||||
t.Fatalf("expected 8.0 Mbps download, got %v", stats.DownloadMbps)
|
||||
}
|
||||
if stats.UploadMbps == nil || *stats.UploadMbps != 4.0 {
|
||||
t.Fatalf("expected 4.0 Mbps upload, got %v", stats.UploadMbps)
|
||||
}
|
||||
if next == nil || next.rxBytes != rxBytes || next.txBytes != txBytes {
|
||||
t.Fatalf("expected current counters to become the next baseline, got %#v", next)
|
||||
}
|
||||
}
|
||||
|
||||
func TestApplyNetworkThroughputFirstSampleOnlyEstablishesBaseline(t *testing.T) {
|
||||
rxBytes := uint64(100)
|
||||
txBytes := uint64(200)
|
||||
stats := &WiFiStats{RXBytes: &rxBytes, TXBytes: &txBytes, networkSampledAt: time.Unix(100, 0)}
|
||||
|
||||
next := applyNetworkThroughput(stats, nil)
|
||||
|
||||
if stats.DownloadMbps != nil || stats.UploadMbps != nil {
|
||||
t.Fatalf("expected no rates for the first sample, got download=%v upload=%v", stats.DownloadMbps, stats.UploadMbps)
|
||||
}
|
||||
if next == nil {
|
||||
t.Fatal("expected the first valid sample to establish a baseline")
|
||||
}
|
||||
}
|
||||
|
||||
func TestApplyNetworkThroughputCounterResetEstablishesNewBaseline(t *testing.T) {
|
||||
startedAt := time.Unix(100, 0)
|
||||
previous := &networkRateSample{rxBytes: 10_000, txBytes: 20_000, sampledAt: startedAt}
|
||||
rxBytes := uint64(10)
|
||||
txBytes := uint64(20)
|
||||
stats := &WiFiStats{RXBytes: &rxBytes, TXBytes: &txBytes, networkSampledAt: startedAt.Add(time.Second)}
|
||||
|
||||
next := applyNetworkThroughput(stats, previous)
|
||||
|
||||
if stats.DownloadMbps != nil || stats.UploadMbps != nil {
|
||||
t.Fatalf("expected no rates after a counter reset, got download=%v upload=%v", stats.DownloadMbps, stats.UploadMbps)
|
||||
}
|
||||
if next == nil || next.rxBytes != rxBytes || next.txBytes != txBytes {
|
||||
t.Fatalf("expected reset counters to become the new baseline, got %#v", next)
|
||||
}
|
||||
}
|
||||
|
||||
func TestApplyNetworkThroughputInvalidElapsedTimeEstablishesNewBaseline(t *testing.T) {
|
||||
sampledAt := time.Unix(100, 0)
|
||||
previous := &networkRateSample{rxBytes: 100, txBytes: 200, sampledAt: sampledAt}
|
||||
rxBytes := uint64(200)
|
||||
txBytes := uint64(300)
|
||||
stats := &WiFiStats{RXBytes: &rxBytes, TXBytes: &txBytes, networkSampledAt: sampledAt}
|
||||
|
||||
next := applyNetworkThroughput(stats, previous)
|
||||
|
||||
if stats.DownloadMbps != nil || stats.UploadMbps != nil {
|
||||
t.Fatalf("expected no rates with zero elapsed time, got download=%v upload=%v", stats.DownloadMbps, stats.UploadMbps)
|
||||
}
|
||||
if next == nil || next.sampledAt != sampledAt {
|
||||
t.Fatalf("expected invalid timing sample to become the new baseline, got %#v", next)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,273 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"log"
|
||||
"net/url"
|
||||
"os/exec"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"nhooyr.io/websocket"
|
||||
)
|
||||
|
||||
type ManagementClient struct {
|
||||
cfg *Config
|
||||
configPath string
|
||||
events chan RoverEvent
|
||||
log *log.Logger
|
||||
restart func()
|
||||
mu sync.Mutex
|
||||
runtime *WSClient
|
||||
media *MediaSupervisor
|
||||
stopping bool
|
||||
}
|
||||
|
||||
type managementCommand struct {
|
||||
Type string `json:"type"`
|
||||
ID string `json:"id"`
|
||||
Config json.RawMessage `json:"config"`
|
||||
Reboot *rebootPayload `json:"reboot"`
|
||||
Media *mediaCommand `json:"media"`
|
||||
}
|
||||
|
||||
func NewManagementClient(cfg *Config, path string, events chan RoverEvent, logger *log.Logger, restart func()) *ManagementClient {
|
||||
return &ManagementClient{cfg: cfg, configPath: path, events: events, log: logger, restart: restart}
|
||||
}
|
||||
|
||||
// Startup can finish while a management action is being accepted. Do not attach
|
||||
// a new command source once shutdown has begun.
|
||||
func (m *ManagementClient) SetRuntime(client *WSClient, media *MediaSupervisor) bool {
|
||||
m.mu.Lock()
|
||||
defer m.mu.Unlock()
|
||||
if m.stopping {
|
||||
client.PrepareShutdown()
|
||||
return false
|
||||
}
|
||||
m.runtime, m.media = client, media
|
||||
return true
|
||||
}
|
||||
|
||||
func (m *ManagementClient) Run(ctx context.Context) {
|
||||
for ctx.Err() == nil {
|
||||
if err := m.connect(ctx); err != nil && ctx.Err() == nil {
|
||||
m.log.Printf("management connection: %v", err)
|
||||
}
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-time.After(2 * time.Second):
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (m *ManagementClient) connect(parent context.Context) error {
|
||||
ctx, cancel := context.WithCancel(parent)
|
||||
defer cancel()
|
||||
endpoint, _ := url.Parse(m.cfg.ServerURL)
|
||||
endpoint.Path = "/rover/management"
|
||||
dialCtx, dialCancel := context.WithTimeout(ctx, dialTimeout)
|
||||
conn, _, err := websocket.Dial(dialCtx, endpoint.String(), nil)
|
||||
dialCancel()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer conn.CloseNow()
|
||||
conn.SetReadLimit(1024 * 1024)
|
||||
if err := writeJSON(ctx, conn, map[string]any{
|
||||
"type": "hello", "name": m.cfg.Name, "config": m.cfg,
|
||||
"schema": ConfigurationSchema(), "profiles": map[string]any{"video": []any{}, "audioCapture": []any{}, "audioPlayback": []any{}},
|
||||
}); err != nil {
|
||||
return err
|
||||
}
|
||||
errors := make(chan error, 3)
|
||||
go func() { errors <- keepalive(ctx, conn) }()
|
||||
go func() { errors <- m.read(ctx, conn) }()
|
||||
go func() {
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case event := <-m.events:
|
||||
if err := writeJSON(ctx, conn, event); err != nil {
|
||||
errors <- err
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
go m.forwardHostStats(ctx, conn)
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return ctx.Err()
|
||||
case err := <-errors:
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
func (m *ManagementClient) read(ctx context.Context, conn *websocket.Conn) error {
|
||||
for {
|
||||
_, data, err := conn.Read(ctx)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
var command managementCommand
|
||||
if err := json.Unmarshal(data, &command); err != nil {
|
||||
return err
|
||||
}
|
||||
if command.ID == "" {
|
||||
continue
|
||||
}
|
||||
after, commandErr := m.execute(ctx, command)
|
||||
ack := ackMessage{Type: "ack", ID: command.ID, Status: "ok"}
|
||||
if commandErr != nil {
|
||||
ack.Status = "error"
|
||||
ack.Error = commandErr.Error()
|
||||
}
|
||||
err = writeJSON(ctx, conn, ack)
|
||||
// Once configuration is committed, restart even if the acknowledgement
|
||||
// cannot reach the server. Reconnection reports the actual saved object.
|
||||
if after != nil {
|
||||
after()
|
||||
}
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (m *ManagementClient) execute(ctx context.Context, command managementCommand) (func(), error) {
|
||||
m.mu.Lock()
|
||||
defer m.mu.Unlock()
|
||||
if m.stopping {
|
||||
return nil, fmt.Errorf("management action already pending")
|
||||
}
|
||||
if command.Type == "media" {
|
||||
if m.media == nil || command.Media == nil {
|
||||
return nil, fmt.Errorf("media supervisor unavailable")
|
||||
}
|
||||
return nil, m.media.HandleAction(ctx, command.Media.Action)
|
||||
}
|
||||
var config *Config
|
||||
var err error
|
||||
switch command.Type {
|
||||
case "config.set":
|
||||
config, err = DecodeConfig(command.Config)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
case "reboot", "update":
|
||||
default:
|
||||
return nil, fmt.Errorf("unsupported management command: %s", command.Type)
|
||||
}
|
||||
if m.runtime != nil {
|
||||
if err := m.runtime.PrepareShutdown(); err != nil {
|
||||
m.runtime.ResumeControl()
|
||||
return nil, err
|
||||
}
|
||||
}
|
||||
resume := func() {
|
||||
if m.runtime != nil {
|
||||
m.runtime.ResumeControl()
|
||||
}
|
||||
}
|
||||
if command.Type == "config.set" {
|
||||
if err := SaveConfig(m.configPath, config); err != nil {
|
||||
resume()
|
||||
return nil, err
|
||||
}
|
||||
m.stopping = true
|
||||
return m.restart, nil
|
||||
}
|
||||
// The updater remains a fixed-purpose helper; browser input cannot select
|
||||
// executable paths or shell arguments. Its implementation is a later stage.
|
||||
path, args := "systemctl", []string{"reboot"}
|
||||
if command.Type == "update" {
|
||||
path, args = "/usr/local/sbin/roverd-self-update", nil
|
||||
}
|
||||
if _, err := exec.LookPath(path); err != nil {
|
||||
resume()
|
||||
return nil, err
|
||||
}
|
||||
m.stopping = true
|
||||
delay := 300 * time.Millisecond
|
||||
if command.Reboot != nil && command.Reboot.DelayMs > 0 {
|
||||
delay = time.Duration(clampInt(command.Reboot.DelayMs, 50, 5000)) * time.Millisecond
|
||||
}
|
||||
return func() {
|
||||
go func() {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
case <-time.After(delay):
|
||||
}
|
||||
m.emit("system."+command.Type+"Starting", nil)
|
||||
cmd := exec.Command(path, args...)
|
||||
if err := cmd.Run(); err != nil {
|
||||
m.log.Printf("management %s failed: %v", command.Type, err)
|
||||
m.emit("system."+command.Type+"Failed", map[string]any{"error": err.Error()})
|
||||
m.mu.Lock()
|
||||
m.stopping = false
|
||||
resume()
|
||||
m.mu.Unlock()
|
||||
}
|
||||
}()
|
||||
}, nil
|
||||
}
|
||||
func (c *ManagementClient) forwardHostStats(ctx context.Context, conn *websocket.Conn) {
|
||||
var previousNetworkSample *networkRateSample
|
||||
|
||||
send := func() bool {
|
||||
// Host stats are collected on demand so each outbound message describes
|
||||
// the current Pi state. Collection failures are encoded into the stats
|
||||
// payload, which keeps this telemetry path from closing the rover socket.
|
||||
stats := CollectHostStats(ctx)
|
||||
// Throughput is derived here because this loop owns the ordered, periodic
|
||||
// samples for one connection. CollectHostStats stays independent, while a
|
||||
// reconnect automatically receives a clean counter baseline.
|
||||
previousNetworkSample = applyNetworkThroughput(stats.WiFi, previousNetworkSample)
|
||||
msg := hostStatsMessage{
|
||||
Type: "hostStats",
|
||||
Timestamp: time.Now().UnixMilli(),
|
||||
Stats: stats,
|
||||
}
|
||||
if err := writeJSON(ctx, conn, msg); err != nil {
|
||||
c.log.Printf("host stats send failed: %v", err)
|
||||
return false
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
// Send once immediately so a newly connected rover can populate the UI
|
||||
// without waiting for the first ticker interval.
|
||||
if !send() {
|
||||
return
|
||||
}
|
||||
|
||||
ticker := time.NewTicker(hostStatsInterval)
|
||||
defer ticker.Stop()
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-ticker.C:
|
||||
if !send() {
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (m *ManagementClient) SetMedia(media *MediaSupervisor) {
|
||||
m.mu.Lock()
|
||||
defer m.mu.Unlock()
|
||||
m.media = media
|
||||
}
|
||||
|
||||
func (m *ManagementClient) emit(event string, data map[string]any) {
|
||||
select {
|
||||
case m.events <- RoverEvent{Type: "event", Event: event, Ts: time.Now().UnixMilli(), Data: data}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"fmt"
|
||||
"os"
|
||||
"path/filepath"
|
||||
)
|
||||
|
||||
const publisherEnvPath = "/var/lib/roverd/media.env"
|
||||
|
||||
func UpdatePublisherEnv(media MediaConfig) error {
|
||||
if media.Video.Enabled && media.Video.PublishURL == "" {
|
||||
return fmt.Errorf("media video publishUrl missing")
|
||||
}
|
||||
if media.AudioCapture.Enabled && media.AudioCapture.PublishURL == "" {
|
||||
return fmt.Errorf("media audioCapture publishUrl missing")
|
||||
}
|
||||
if media.AudioPlayback.Enabled && media.AudioPlayback.ForwardURL == "" {
|
||||
return fmt.Errorf("media audioPlayback forwardUrl missing")
|
||||
}
|
||||
if media.Video.Width <= 0 || media.Video.Height <= 0 || media.Video.FPS <= 0 || media.Video.Bitrate <= 0 {
|
||||
return fmt.Errorf("invalid media video dimensions/bitrate")
|
||||
}
|
||||
if err := os.MkdirAll(filepath.Dir(publisherEnvPath), 0o755); err != nil {
|
||||
return err
|
||||
}
|
||||
var buf bytes.Buffer
|
||||
/*
|
||||
The env file is intentionally verbose: every publisher receives concrete
|
||||
values instead of silently falling back to script-local defaults. That
|
||||
makes roverd.json the owner of media behavior while keeping the shell
|
||||
scripts as small pipeline launchers.
|
||||
*/
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_ENABLE=%d\n", boolToInt(media.Video.Enabled))
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_PUBLISHER=%s\n", media.Video.Publisher)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_PUBLISH_URL=%s\n", media.Video.PublishURL)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_DEVICE=%s\n", media.Video.Device)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_INPUT_FORMAT=%s\n", media.Video.InputFormat)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_WIDTH=%d\n", media.Video.Width)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_HEIGHT=%d\n", media.Video.Height)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_FPS=%d\n", media.Video.FPS)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_BITRATE=%d\n", media.Video.Bitrate)
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_INVERT=%d\n", boolToInt(media.Video.Inverted))
|
||||
fmt.Fprintf(&buf, "ROVERD_VIDEO_SENSOR_MODE=%s\n", media.Video.SensorMode)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_CAPTURE_ENABLE=%d\n", boolToInt(media.AudioCapture.Enabled))
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_CAPTURE_PUBLISH_URL=%s\n", media.AudioCapture.PublishURL)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_CAPTURE_DEVICE=%s\n", media.AudioCapture.Device)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_CAPTURE_SAMPLE_RATE=%d\n", media.AudioCapture.SampleRate)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_CAPTURE_CHANNELS=%d\n", media.AudioCapture.Channels)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_CAPTURE_BITRATE=%d\n", media.AudioCapture.Bitrate)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_PLAYBACK_ENABLE=%d\n", boolToInt(media.AudioPlayback.Enabled))
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_PLAYBACK_FORWARD_URL=%s\n", media.AudioPlayback.ForwardURL)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_PLAYBACK_DEVICE=%s\n", media.AudioPlayback.Device)
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_PLAYBACK_NORMALIZE=%d\n", boolToInt(media.AudioPlayback.Normalize))
|
||||
fmt.Fprintf(&buf, "ROVERD_AUDIO_PLAYBACK_NORMALIZE_FILTER=%s\n", media.AudioPlayback.NormalizeFilter)
|
||||
if err := os.WriteFile(publisherEnvPath, buf.Bytes(), 0o640); err != nil {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func boolToInt(v bool) int {
|
||||
if v {
|
||||
return 1
|
||||
}
|
||||
return 0
|
||||
}
|
||||
@@ -0,0 +1,163 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"net/http"
|
||||
"os/exec"
|
||||
"time"
|
||||
)
|
||||
|
||||
type MediaSupervisor struct {
|
||||
cfg MediaConfig
|
||||
logger *log.Logger
|
||||
client *http.Client
|
||||
checkInterval time.Duration
|
||||
}
|
||||
|
||||
func NewMediaSupervisor(cfg MediaConfig, logger *log.Logger) *MediaSupervisor {
|
||||
if err := UpdatePublisherEnv(cfg); err != nil {
|
||||
logger.Printf("media supervisor: update env failed: %v", err)
|
||||
}
|
||||
if !cfg.Manage {
|
||||
return nil
|
||||
}
|
||||
interval := cfg.HealthInterval.Duration
|
||||
if interval <= 0 {
|
||||
interval = 30 * time.Second
|
||||
}
|
||||
var client *http.Client
|
||||
if cfg.HealthURL != "" {
|
||||
client = &http.Client{Timeout: 5 * time.Second}
|
||||
}
|
||||
return &MediaSupervisor{
|
||||
cfg: cfg,
|
||||
logger: logger,
|
||||
client: client,
|
||||
checkInterval: interval,
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) Start(ctx context.Context) {
|
||||
if m == nil {
|
||||
return
|
||||
}
|
||||
if err := UpdatePublisherEnv(m.cfg); err != nil {
|
||||
m.logger.Printf("media supervisor: update env failed: %v", err)
|
||||
}
|
||||
if m.cfg.HealthURL == "" || m.client == nil {
|
||||
return
|
||||
}
|
||||
go func() {
|
||||
ticker := time.NewTicker(m.checkInterval)
|
||||
defer ticker.Stop()
|
||||
|
||||
if err := m.checkAndRepair(); err != nil {
|
||||
m.logger.Printf("media supervisor: %v", err)
|
||||
}
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-ticker.C:
|
||||
if err := m.checkAndRepair(); err != nil {
|
||||
m.logger.Printf("media supervisor: %v", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) HandleAction(ctx context.Context, action string) error {
|
||||
if m == nil {
|
||||
return errors.New("media supervisor disabled")
|
||||
}
|
||||
if err := UpdatePublisherEnv(m.cfg); err != nil {
|
||||
return err
|
||||
}
|
||||
switch action {
|
||||
case "start", "stop", "restart", "reload", "status":
|
||||
return m.runSystemctl(ctx, action)
|
||||
default:
|
||||
return fmt.Errorf("unknown media action: %s", action)
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) checkAndRepair() error {
|
||||
ctx, cancel := context.WithTimeout(context.Background(), 7*time.Second)
|
||||
defer cancel()
|
||||
|
||||
if m.checkHealth(ctx) {
|
||||
return nil
|
||||
}
|
||||
m.logger.Printf("media supervisor: health check failed, restarting configured media services")
|
||||
if err := m.runSystemctl(ctx, "restart"); err != nil {
|
||||
return fmt.Errorf("restart media services: %w", err)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) checkHealth(ctx context.Context) bool {
|
||||
if m.client == nil || m.cfg.HealthURL == "" {
|
||||
return true
|
||||
}
|
||||
req, err := http.NewRequestWithContext(ctx, http.MethodGet, m.cfg.HealthURL, nil)
|
||||
if err != nil {
|
||||
m.logger.Printf("media supervisor: health request: %v", err)
|
||||
return false
|
||||
}
|
||||
resp, err := m.client.Do(req)
|
||||
if err != nil {
|
||||
m.logger.Printf("media supervisor: health request failed: %v", err)
|
||||
return false
|
||||
}
|
||||
defer resp.Body.Close()
|
||||
_, _ = io.Copy(io.Discard, resp.Body)
|
||||
if resp.StatusCode >= 200 && resp.StatusCode < 300 {
|
||||
return true
|
||||
}
|
||||
m.logger.Printf("media supervisor: unexpected health status %d", resp.StatusCode)
|
||||
return false
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) runSystemctl(ctx context.Context, action string) error {
|
||||
services := m.managedServices()
|
||||
if len(services) == 0 {
|
||||
return errors.New("no media services configured")
|
||||
}
|
||||
runCtx, cancel := context.WithTimeout(ctx, 15*time.Second)
|
||||
defer cancel()
|
||||
|
||||
for _, service := range services {
|
||||
cmd := exec.CommandContext(runCtx, "systemctl", action, service)
|
||||
output, err := cmd.CombinedOutput()
|
||||
if err != nil {
|
||||
return fmt.Errorf("systemctl %s %s: %w (%s)", action, service, err, string(output))
|
||||
}
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) managedServices() []string {
|
||||
/*
|
||||
Only enabled streams are service-managed. This matters for audio capture:
|
||||
the service may be installed everywhere, but a rover with capture disabled
|
||||
should not have remote media commands starting a publisher that the config
|
||||
says should stay off.
|
||||
*/
|
||||
services := []string{}
|
||||
if m.cfg.Video.Enabled && m.cfg.Video.Service != "" {
|
||||
services = append(services, m.cfg.Video.Service)
|
||||
}
|
||||
if m.cfg.AudioCapture.Enabled && m.cfg.AudioCapture.Service != "" {
|
||||
services = append(services, m.cfg.AudioCapture.Service)
|
||||
}
|
||||
if m.cfg.AudioPlayback.Enabled && m.cfg.AudioPlayback.Service != "" {
|
||||
services = append(services, m.cfg.AudioPlayback.Service)
|
||||
}
|
||||
return services
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
package roverd
|
||||
|
||||
// These tests pin the network-agnostic RTSP contract. A rover provides its server URL and name
|
||||
// once; all three media paths must then resolve to distinct, safely escaped MediaMTX paths.
|
||||
|
||||
import (
|
||||
"strings"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestMediaURLsDeriveFromServerURLAndRoverName(t *testing.T) {
|
||||
cfg := MediaConfig{
|
||||
Video: VideoMediaConfig{Enabled: true},
|
||||
AudioCapture: AudioCaptureConfig{Enabled: true},
|
||||
AudioPlayback: AudioPlaybackConfig{Enabled: true},
|
||||
}
|
||||
if err := validateMediaConfig(&cfg, "ws://control-server.local:8080/rover", "rover one"); err != nil {
|
||||
t.Fatalf("validate media config: %v", err)
|
||||
}
|
||||
|
||||
wants := map[string]string{
|
||||
"video": "rtsp://control-server.local:8554/rover%20one",
|
||||
"mic": "rtsp://control-server.local:8554/rover%20one-audio",
|
||||
"speaker": "rtsp://control-server.local:8554/rover%20one-fwd",
|
||||
}
|
||||
got := map[string]string{
|
||||
"video": cfg.Video.PublishURL,
|
||||
"mic": cfg.AudioCapture.PublishURL,
|
||||
"speaker": cfg.AudioPlayback.ForwardURL,
|
||||
}
|
||||
for name, want := range wants {
|
||||
if got[name] != want {
|
||||
t.Errorf("%s URL: got %q, want %q", name, got[name], want)
|
||||
}
|
||||
}
|
||||
if cfg.RTSPPort != 8554 {
|
||||
t.Fatalf("RTSP port: got %d, want 8554", cfg.RTSPPort)
|
||||
}
|
||||
}
|
||||
|
||||
func TestExplicitMediaPortAppliesToEveryRTSPPath(t *testing.T) {
|
||||
cfg := MediaConfig{
|
||||
RTSPPort: 10554,
|
||||
Video: VideoMediaConfig{Enabled: true},
|
||||
AudioCapture: AudioCaptureConfig{Enabled: true},
|
||||
AudioPlayback: AudioPlaybackConfig{Enabled: true},
|
||||
}
|
||||
if err := validateMediaConfig(&cfg, "ws://media.example/rover", "r1"); err != nil {
|
||||
t.Fatalf("validate media config: %v", err)
|
||||
}
|
||||
for name, value := range map[string]string{
|
||||
"video": cfg.Video.PublishURL, "mic": cfg.AudioCapture.PublishURL, "speaker": cfg.AudioPlayback.ForwardURL,
|
||||
} {
|
||||
if !strings.Contains(value, ":10554/") {
|
||||
t.Errorf("%s URL did not use configured port: %q", name, value)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestLegacyExplicitSRTURLsCannotKeepAnUpdatedRoverOnTheOldTransport(t *testing.T) {
|
||||
/*
|
||||
Deployed rover configs can still contain these former fields. Validation must replace
|
||||
them unconditionally so updating roverd is sufficient to move the whole media path.
|
||||
*/
|
||||
cfg := MediaConfig{
|
||||
Video: VideoMediaConfig{Enabled: true, PublishURL: "srt://old/video"},
|
||||
AudioCapture: AudioCaptureConfig{Enabled: true, PublishURL: "srt://old/audio"},
|
||||
AudioPlayback: AudioPlaybackConfig{Enabled: true, ForwardURL: "srt://old/forward"},
|
||||
}
|
||||
if err := validateMediaConfig(&cfg, "ws://new-server.local:8080/rover", "r1"); err != nil {
|
||||
t.Fatalf("validate media config: %v", err)
|
||||
}
|
||||
for name, value := range map[string]string{
|
||||
"video": cfg.Video.PublishURL, "mic": cfg.AudioCapture.PublishURL, "speaker": cfg.AudioPlayback.ForwardURL,
|
||||
} {
|
||||
if !strings.HasPrefix(value, "rtsp://new-server.local:8554/") {
|
||||
t.Errorf("%s retained an old transport URL: %q", name, value)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"encoding/json"
|
||||
"strings"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestHelloPeripheralMetadataContainsOnlyRenderableFields(t *testing.T) {
|
||||
minimum, maximum := 0, 180
|
||||
message := helloMessage{
|
||||
Type: "hello",
|
||||
Name: "test-rover",
|
||||
Peripherals: []RoverPeripheralMetadata{{
|
||||
ID: "firmata-0",
|
||||
Name: "Camera arm",
|
||||
Controls: []RoverPeripheralControl{{
|
||||
ID: "position", Type: "slider", Name: "Position", Minimum: &minimum, Maximum: &maximum,
|
||||
}},
|
||||
}},
|
||||
}
|
||||
|
||||
encoded, err := json.Marshal(message)
|
||||
if err != nil {
|
||||
t.Fatalf("marshal hello: %v", err)
|
||||
}
|
||||
text := string(encoded)
|
||||
if !strings.Contains(text, `"peripherals":[{"id":"firmata-0","name":"Camera arm","controls":[{"id":"position","type":"slider","name":"Position","min":0,"max":180}]`) {
|
||||
t.Fatalf("hello is missing ordered peripheral metadata: %s", text)
|
||||
}
|
||||
var envelope map[string]json.RawMessage
|
||||
if err := json.Unmarshal(encoded, &envelope); err != nil {
|
||||
t.Fatalf("unmarshal hello envelope: %v", err)
|
||||
}
|
||||
peripheralJSON := string(envelope["peripherals"])
|
||||
if strings.Contains(peripheralJSON, `"pin"`) || strings.Contains(peripheralJSON, `"output"`) {
|
||||
t.Fatalf("hello exposed private Firmata routing: %s", peripheralJSON)
|
||||
}
|
||||
}
|
||||
|
||||
func TestInboundPeripheralCommandPreservesRawJSONValue(t *testing.T) {
|
||||
var message inboundMessage
|
||||
err := json.Unmarshal([]byte(`{
|
||||
"type":"peripheral",
|
||||
"id":"command-1",
|
||||
"peripheral":{"id":"firmata-0","control":"displayText","value":"hello rover"}
|
||||
}`), &message)
|
||||
if err != nil {
|
||||
t.Fatalf("unmarshal command: %v", err)
|
||||
}
|
||||
if message.Peripheral == nil || message.Peripheral.ID != "firmata-0" || message.Peripheral.Control != "displayText" {
|
||||
t.Fatalf("unexpected peripheral command: %#v", message.Peripheral)
|
||||
}
|
||||
if string(message.Peripheral.Value) != `"hello rover"` {
|
||||
t.Fatalf("raw value = %s", message.Peripheral.Value)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"io"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
// DiscoverPeripheralManager remains inert in a dummy build. The dummy daemon is
|
||||
// specifically used without rover hardware and must not probe or reset serial
|
||||
// devices that happen to be attached to a developer's machine.
|
||||
func DiscoverPeripheralManager(ctx context.Context, excludedDevice string, logger *log.Logger) (*PeripheralManager, error) {
|
||||
dependencies := peripheralDiscoveryDependencies{
|
||||
listCandidates: func(string) ([]string, error) { return nil, nil },
|
||||
open: func(string) (io.ReadWriteCloser, error) { return nil, nil },
|
||||
sleep: func(time.Duration) {},
|
||||
startupWait: 0,
|
||||
handshakeWait: 0,
|
||||
}
|
||||
return discoverPeripheralManager(ctx, excludedDevice, logger, dependencies)
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"io"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
const (
|
||||
peripheralBaud = 115200
|
||||
peripheralReadTimeout = 100 * time.Millisecond
|
||||
)
|
||||
|
||||
// DiscoverPeripheralManager performs the one and only peripheral scan for this
|
||||
// roverd process. The Roomba Open Interface serial device is explicitly
|
||||
// excluded because it belongs to SerialAdapter and must never be probed as an
|
||||
// ESP32 peripheral.
|
||||
func DiscoverPeripheralManager(ctx context.Context, excludedDevice string, logger *log.Logger) (*PeripheralManager, error) {
|
||||
dependencies := peripheralDiscoveryDependencies{
|
||||
listCandidates: listPeripheralCandidates,
|
||||
open: func(devicePath string) (io.ReadWriteCloser, error) {
|
||||
return serial.OpenPort(&serial.Config{
|
||||
Name: devicePath,
|
||||
Baud: peripheralBaud,
|
||||
ReadTimeout: peripheralReadTimeout,
|
||||
})
|
||||
},
|
||||
sleep: time.Sleep,
|
||||
startupWait: peripheralStartupWait,
|
||||
handshakeWait: peripheralHandshakeTimeout,
|
||||
}
|
||||
return discoverPeripheralManager(ctx, excludedDevice, logger, dependencies)
|
||||
}
|
||||
@@ -0,0 +1,597 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"path/filepath"
|
||||
"sort"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
"unicode/utf8"
|
||||
)
|
||||
|
||||
const (
|
||||
peripheralStartupWait = 2 * time.Second
|
||||
peripheralHandshakeTimeout = 5 * time.Second
|
||||
peripheralFirmwareName = "RoverPeripheralFirmata"
|
||||
)
|
||||
|
||||
// RoverPeripheralMetadata is the part of a peripheral description that leaves
|
||||
// roverd. Pin numbers and output mappings intentionally remain private to the
|
||||
// rover process; the server and browser identify only the declared control.
|
||||
type RoverPeripheralMetadata struct {
|
||||
ID string `json:"id"`
|
||||
Name string `json:"name"`
|
||||
Controls []RoverPeripheralControl `json:"controls"`
|
||||
}
|
||||
|
||||
// RoverPeripheralControl contains only fields needed to render and operate one
|
||||
// of the four generic UI controls. Pointer fields preserve legitimate zero
|
||||
// bounds while still omitting properties that do not apply to a control type.
|
||||
type RoverPeripheralControl struct {
|
||||
ID string `json:"id"`
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Mode string `json:"mode,omitempty"`
|
||||
Minimum *int `json:"min,omitempty"`
|
||||
Maximum *int `json:"max,omitempty"`
|
||||
MaximumLength *int `json:"maxLength,omitempty"`
|
||||
}
|
||||
|
||||
type managedPeripheral struct {
|
||||
metadata RoverPeripheralMetadata
|
||||
description PeripheralDescription
|
||||
controls map[string]PeripheralControl
|
||||
client *FirmataClient
|
||||
connection io.ReadWriteCloser
|
||||
devicePath string
|
||||
capabilities [][]FirmataPinCapability
|
||||
}
|
||||
|
||||
// PeripheralManager owns the immutable boot-time inventory and every serial
|
||||
// connection behind it. The inventory never changes after discovery, even if a
|
||||
// USB device later disappears; a process restart is the only rescan mechanism.
|
||||
type PeripheralManager struct {
|
||||
piGPIO *piGPIOPeripheral
|
||||
mu sync.RWMutex
|
||||
peripherals []*managedPeripheral
|
||||
byID map[string]*managedPeripheral
|
||||
cancel context.CancelFunc
|
||||
closeOnce sync.Once
|
||||
logger *log.Logger
|
||||
failures chan PeripheralFailure
|
||||
}
|
||||
|
||||
// PeripheralFailure is emitted once when a successfully discovered device's
|
||||
// serial reader terminates unexpectedly. Device identity is retained even
|
||||
// though reconnection still requires restarting roverd.
|
||||
type PeripheralFailure struct {
|
||||
ID string
|
||||
Name string
|
||||
Err error
|
||||
}
|
||||
|
||||
type peripheralDiscoveryDependencies struct {
|
||||
listCandidates func(excludedDevice string) ([]string, error)
|
||||
open func(devicePath string) (io.ReadWriteCloser, error)
|
||||
sleep func(time.Duration)
|
||||
startupWait time.Duration
|
||||
handshakeWait time.Duration
|
||||
}
|
||||
|
||||
func discoverPeripheralManager(ctx context.Context, excludedDevice string, logger *log.Logger, dependencies peripheralDiscoveryDependencies) (*PeripheralManager, error) {
|
||||
managerContext, cancel := context.WithCancel(ctx)
|
||||
manager := &PeripheralManager{
|
||||
byID: make(map[string]*managedPeripheral),
|
||||
cancel: cancel,
|
||||
logger: logger,
|
||||
failures: make(chan PeripheralFailure, 16),
|
||||
}
|
||||
|
||||
candidates, err := dependencies.listCandidates(excludedDevice)
|
||||
if err != nil {
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("list peripheral serial devices: %w", err)
|
||||
}
|
||||
|
||||
for _, devicePath := range candidates {
|
||||
connection, err := dependencies.open(devicePath)
|
||||
if err != nil {
|
||||
logger.Printf("skipping peripheral candidate %s: open failed: %v", devicePath, err)
|
||||
continue
|
||||
}
|
||||
|
||||
// UART bridge and native-USB development boards may reset when opened.
|
||||
// Waiting and then draining boot fragments gives the handshake a fresh
|
||||
// parser boundary instead of occasionally starting inside an old SysEx.
|
||||
dependencies.sleep(dependencies.startupWait)
|
||||
if err := drainPeripheralSerial(connection); err != nil {
|
||||
connection.Close()
|
||||
logger.Printf("skipping peripheral candidate %s: drain failed: %v", devicePath, err)
|
||||
continue
|
||||
}
|
||||
|
||||
client := NewFirmataClient(connection)
|
||||
client.Start(managerContext)
|
||||
firmware, err := queryPeripheralFirmware(managerContext, client, dependencies.handshakeWait)
|
||||
if err != nil {
|
||||
connection.Close()
|
||||
logger.Printf("skipping peripheral candidate %s: Firmata query failed: %v", devicePath, err)
|
||||
continue
|
||||
}
|
||||
if firmware.Name != peripheralFirmwareName {
|
||||
connection.Close()
|
||||
logger.Printf("skipping Firmata device %s: firmware %q does not expose rover peripherals", devicePath, firmware.Name)
|
||||
continue
|
||||
}
|
||||
|
||||
capabilities, err := queryPeripheralCapabilities(managerContext, client, dependencies.handshakeWait)
|
||||
if err != nil {
|
||||
connection.Close()
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("query capabilities from rover peripheral %s: %w", devicePath, err)
|
||||
}
|
||||
description, err := queryPeripheralDescription(managerContext, client, dependencies.handshakeWait)
|
||||
if err != nil {
|
||||
connection.Close()
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("describe rover peripheral %s: %w", devicePath, err)
|
||||
}
|
||||
|
||||
peripheral := newManagedPeripheral(len(manager.peripherals), devicePath, connection, client, description, capabilities)
|
||||
if err := peripheral.initializeStandardOutputs(); err != nil {
|
||||
connection.Close()
|
||||
manager.Close()
|
||||
return nil, fmt.Errorf("initialize rover peripheral %s: %w", devicePath, err)
|
||||
}
|
||||
manager.peripherals = append(manager.peripherals, peripheral)
|
||||
manager.byID[peripheral.metadata.ID] = peripheral
|
||||
client.SetTerminalErrorHandler(func(terminalErr error) {
|
||||
failure := PeripheralFailure{ID: peripheral.metadata.ID, Name: peripheral.metadata.Name, Err: terminalErr}
|
||||
select {
|
||||
case manager.failures <- failure:
|
||||
default:
|
||||
// The channel is intentionally bounded because broadcasts are
|
||||
// diagnostic. Never block a Firmata reader during a fleet-wide
|
||||
// shutdown or an unlikely burst of simultaneous USB failures.
|
||||
logger.Printf("peripheral failure notification queue full for %s: %v", peripheral.metadata.ID, terminalErr)
|
||||
}
|
||||
})
|
||||
logger.Printf("discovered rover peripheral %s on %s with %d generic controls", description.Name, devicePath, len(description.Controls))
|
||||
}
|
||||
|
||||
return manager, nil
|
||||
}
|
||||
|
||||
// StartupBroadcasts describes the fixed inventory without exposing device
|
||||
// paths or wiring details on the rover's local console.
|
||||
func (manager *PeripheralManager) StartupBroadcasts() []string {
|
||||
inventory := manager.Inventory()
|
||||
if len(inventory) == 0 {
|
||||
return []string{"No rover peripherals enabled or detected during startup."}
|
||||
}
|
||||
messages := make([]string, 0, len(inventory))
|
||||
for _, peripheral := range inventory {
|
||||
messages = append(messages, fmt.Sprintf(
|
||||
"Rover peripheral %q connected as %s with %d additional controls.",
|
||||
peripheral.Name,
|
||||
peripheral.ID,
|
||||
len(peripheral.Controls),
|
||||
))
|
||||
}
|
||||
return messages
|
||||
}
|
||||
|
||||
// Failures exposes unexpected runtime disconnects to the daemon entry point,
|
||||
// which owns the ConsoleNotifier and therefore owns user-facing wording.
|
||||
func (manager *PeripheralManager) Failures() <-chan PeripheralFailure {
|
||||
if manager == nil {
|
||||
return nil
|
||||
}
|
||||
return manager.failures
|
||||
}
|
||||
|
||||
func listPeripheralCandidates(excludedDevice string) ([]string, error) {
|
||||
patterns := []string{
|
||||
"/dev/serial/by-id/*",
|
||||
"/dev/ttyUSB*",
|
||||
"/dev/ttyACM*",
|
||||
}
|
||||
var matchesInPreferenceOrder []string
|
||||
|
||||
for _, pattern := range patterns {
|
||||
matches, err := filepath.Glob(pattern)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
sort.Strings(matches)
|
||||
matchesInPreferenceOrder = append(matchesInPreferenceOrder, matches...)
|
||||
}
|
||||
return uniquePeripheralCandidates(matchesInPreferenceOrder, excludedDevice), nil
|
||||
}
|
||||
|
||||
func uniquePeripheralCandidates(matches []string, excludedDevice string) []string {
|
||||
excludedCanonical := canonicalDevicePath(excludedDevice)
|
||||
seen := make(map[string]struct{})
|
||||
var candidates []string
|
||||
for _, match := range matches {
|
||||
canonical := canonicalDevicePath(match)
|
||||
if canonical == excludedCanonical {
|
||||
continue
|
||||
}
|
||||
if _, exists := seen[canonical]; exists {
|
||||
continue
|
||||
}
|
||||
seen[canonical] = struct{}{}
|
||||
// /dev/serial/by-id matches are passed first, so retaining the first
|
||||
// spelling favors stable names while still removing each tty alias.
|
||||
candidates = append(candidates, match)
|
||||
}
|
||||
return candidates
|
||||
}
|
||||
|
||||
func canonicalDevicePath(devicePath string) string {
|
||||
if devicePath == "" {
|
||||
return ""
|
||||
}
|
||||
resolved, err := filepath.EvalSymlinks(devicePath)
|
||||
if err == nil {
|
||||
return resolved
|
||||
}
|
||||
abs, err := filepath.Abs(devicePath)
|
||||
if err == nil {
|
||||
return filepath.Clean(abs)
|
||||
}
|
||||
return filepath.Clean(devicePath)
|
||||
}
|
||||
|
||||
func drainPeripheralSerial(connection io.Reader) error {
|
||||
buffer := make([]byte, 256)
|
||||
for {
|
||||
_, err := connection.Read(buffer)
|
||||
if errors.Is(err, io.EOF) {
|
||||
// tarm/serial uses EOF to mean its short read timeout elapsed. That
|
||||
// quiet interval is precisely the boundary needed before handshaking.
|
||||
return nil
|
||||
}
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func queryPeripheralFirmware(ctx context.Context, client *FirmataClient, timeout time.Duration) (FirmataFirmware, error) {
|
||||
queryContext, cancel := context.WithTimeout(ctx, timeout)
|
||||
defer cancel()
|
||||
return client.QueryFirmware(queryContext)
|
||||
}
|
||||
|
||||
func queryPeripheralCapabilities(ctx context.Context, client *FirmataClient, timeout time.Duration) ([][]FirmataPinCapability, error) {
|
||||
queryContext, cancel := context.WithTimeout(ctx, timeout)
|
||||
defer cancel()
|
||||
return client.QueryCapabilities(queryContext)
|
||||
}
|
||||
|
||||
func queryPeripheralDescription(ctx context.Context, client *FirmataClient, timeout time.Duration) (PeripheralDescription, error) {
|
||||
queryContext, cancel := context.WithTimeout(ctx, timeout)
|
||||
defer cancel()
|
||||
return client.Describe(queryContext)
|
||||
}
|
||||
|
||||
func newManagedPeripheral(index int, devicePath string, connection io.ReadWriteCloser, client *FirmataClient, description PeripheralDescription, capabilities [][]FirmataPinCapability) *managedPeripheral {
|
||||
controls := make(map[string]PeripheralControl, len(description.Controls))
|
||||
metadataControls := make([]RoverPeripheralControl, 0, len(description.Controls))
|
||||
for _, control := range description.Controls {
|
||||
controls[control.ID] = control
|
||||
metadataControls = append(metadataControls, RoverPeripheralControl{
|
||||
ID: control.ID,
|
||||
Type: control.Type,
|
||||
Name: control.Name,
|
||||
Mode: control.Mode,
|
||||
Minimum: cloneIntPointer(control.Minimum),
|
||||
Maximum: cloneIntPointer(control.Maximum),
|
||||
MaximumLength: cloneIntPointer(control.MaximumLength),
|
||||
})
|
||||
}
|
||||
|
||||
return &managedPeripheral{
|
||||
metadata: RoverPeripheralMetadata{
|
||||
ID: fmt.Sprintf("firmata-%d", index),
|
||||
Name: description.Name,
|
||||
Controls: metadataControls,
|
||||
},
|
||||
description: description,
|
||||
controls: controls,
|
||||
client: client,
|
||||
connection: connection,
|
||||
devicePath: devicePath,
|
||||
capabilities: capabilities,
|
||||
}
|
||||
}
|
||||
|
||||
func cloneIntPointer(value *int) *int {
|
||||
if value == nil {
|
||||
return nil
|
||||
}
|
||||
cloned := *value
|
||||
return &cloned
|
||||
}
|
||||
|
||||
func (peripheral *managedPeripheral) initializeStandardOutputs() error {
|
||||
if camera := peripheral.description.RoverControls.CameraServo; camera != nil {
|
||||
if err := peripheral.requirePinMode("cameraServo", camera.Pin, FirmataPinModeServo); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
if headlight := peripheral.description.RoverControls.Headlight; headlight != nil {
|
||||
if err := peripheral.requirePinMode("headlight", headlight.Pin, FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
if laser := peripheral.description.RoverControls.Laser; laser != nil {
|
||||
if err := peripheral.requirePinMode("laser", laser.Pin, FirmataPinModeOutput); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
for _, control := range peripheral.description.Controls {
|
||||
if control.Output.Type == "custom" {
|
||||
continue
|
||||
}
|
||||
pin := byte(*control.Output.Pin)
|
||||
requiredMode := FirmataPinModeOutput
|
||||
if control.Output.Type == "pwm" {
|
||||
requiredMode = FirmataPinModePWM
|
||||
} else if control.Output.Type == "servo" {
|
||||
requiredMode = FirmataPinModeServo
|
||||
}
|
||||
if err := peripheral.requirePinMode("control "+control.ID, int(pin), requiredMode); err != nil {
|
||||
return err
|
||||
}
|
||||
switch control.Output.Type {
|
||||
case "digital":
|
||||
if err := peripheral.client.SetPinMode(pin, FirmataPinModeOutput); err != nil {
|
||||
return fmt.Errorf("configure control %q as digital: %w", control.ID, err)
|
||||
}
|
||||
// A generic button begins logically off. Active-low hardware needs a
|
||||
// high electrical level to represent that same initial state.
|
||||
if err := peripheral.client.SetDigitalPin(pin, control.Output.ActiveLow); err != nil {
|
||||
return fmt.Errorf("initialize digital control %q: %w", control.ID, err)
|
||||
}
|
||||
case "pwm":
|
||||
if err := peripheral.client.SetPinMode(pin, FirmataPinModePWM); err != nil {
|
||||
return fmt.Errorf("configure control %q as PWM: %w", control.ID, err)
|
||||
}
|
||||
case "servo":
|
||||
if err := peripheral.client.SetPinMode(pin, FirmataPinModeServo); err != nil {
|
||||
return fmt.Errorf("configure control %q as servo: %w", control.ID, err)
|
||||
}
|
||||
}
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (peripheral *managedPeripheral) requirePinMode(owner string, pin int, requiredMode byte) error {
|
||||
if pin < 0 || pin >= len(peripheral.capabilities) {
|
||||
return fmt.Errorf("%s advertises pin %d, but Firmata reported only %d pins", owner, pin, len(peripheral.capabilities))
|
||||
}
|
||||
for _, capability := range peripheral.capabilities[pin] {
|
||||
if capability.Mode == requiredMode {
|
||||
return nil
|
||||
}
|
||||
}
|
||||
return fmt.Errorf("%s advertises pin %d without required Firmata mode 0x%02x", owner, pin, requiredMode)
|
||||
}
|
||||
|
||||
// Role providers share identity and controller construction, without requiring
|
||||
// internal hardware to pretend it has a Firmata client or serial connection.
|
||||
type roverRoleProvider interface {
|
||||
roleID() string
|
||||
hasRole(string) bool
|
||||
cameraServo(*log.Logger) (CameraServoController, error)
|
||||
toggle(string, *log.Logger) (ToggleController, error)
|
||||
}
|
||||
|
||||
func (p *managedPeripheral) roleID() string { return p.metadata.ID }
|
||||
func (p *managedPeripheral) hasRole(role string) bool {
|
||||
switch role {
|
||||
case "cameraServo":
|
||||
return p.description.RoverControls.CameraServo != nil
|
||||
case "headlight":
|
||||
return p.description.RoverControls.Headlight != nil
|
||||
case "laser":
|
||||
return p.description.RoverControls.Laser != nil
|
||||
}
|
||||
return false
|
||||
}
|
||||
func (p *managedPeripheral) cameraServo(logger *log.Logger) (CameraServoController, error) {
|
||||
return newFirmataCameraServo(p, *p.description.RoverControls.CameraServo, logger)
|
||||
}
|
||||
func (p *managedPeripheral) toggle(role string, logger *log.Logger) (ToggleController, error) {
|
||||
declaration := p.description.RoverControls.Headlight
|
||||
if role == "laser" {
|
||||
declaration = p.description.RoverControls.Laser
|
||||
}
|
||||
return newFirmataToggle(role, p, *declaration, logger)
|
||||
}
|
||||
|
||||
func (manager *PeripheralManager) roleProvider(role string) (roverRoleProvider, error) {
|
||||
if manager == nil {
|
||||
return nil, nil
|
||||
}
|
||||
manager.mu.RLock()
|
||||
defer manager.mu.RUnlock()
|
||||
var selected roverRoleProvider
|
||||
var ids []string
|
||||
consider := func(provider roverRoleProvider) {
|
||||
if provider.hasRole(role) {
|
||||
selected = provider
|
||||
ids = append(ids, provider.roleID())
|
||||
}
|
||||
}
|
||||
for _, peripheral := range manager.peripherals {
|
||||
consider(peripheral)
|
||||
}
|
||||
if manager.piGPIO != nil {
|
||||
consider(manager.piGPIO)
|
||||
}
|
||||
if len(ids) > 1 {
|
||||
return nil, fmt.Errorf("rover peripheral role %s has multiple providers: %s", role, strings.Join(ids, ", "))
|
||||
}
|
||||
return selected, nil
|
||||
}
|
||||
|
||||
func (manager *PeripheralManager) NewCameraServo(logger *log.Logger) (CameraServoController, error) {
|
||||
provider, err := manager.roleProvider("cameraServo")
|
||||
if err != nil || provider == nil {
|
||||
return nil, err
|
||||
}
|
||||
return provider.cameraServo(logger)
|
||||
}
|
||||
|
||||
func (manager *PeripheralManager) NewToggle(role string, logger *log.Logger) (ToggleController, error) {
|
||||
provider, err := manager.roleProvider(role)
|
||||
if err != nil || provider == nil {
|
||||
return nil, err
|
||||
}
|
||||
return provider.toggle(role, logger)
|
||||
}
|
||||
|
||||
// StartInternal starts automatic functions after role registration. BRC belongs
|
||||
// to the internal peripheral lifecycle rather than the daemon entry point.
|
||||
func (manager *PeripheralManager) StartInternal(ctx context.Context) error {
|
||||
if manager == nil || manager.piGPIO == nil {
|
||||
return nil
|
||||
}
|
||||
return manager.piGPIO.start(ctx, manager.logger)
|
||||
}
|
||||
|
||||
// Inventory returns a defensive copy in startup order. Server reconnects reuse
|
||||
// this same list and therefore never cause a USB rescan or ID reassignment.
|
||||
func (manager *PeripheralManager) Inventory() []RoverPeripheralMetadata {
|
||||
if manager == nil {
|
||||
return nil
|
||||
}
|
||||
manager.mu.RLock()
|
||||
defer manager.mu.RUnlock()
|
||||
|
||||
inventory := make([]RoverPeripheralMetadata, 0, len(manager.peripherals))
|
||||
for _, peripheral := range manager.peripherals {
|
||||
metadata := peripheral.metadata
|
||||
metadata.Controls = make([]RoverPeripheralControl, 0, len(peripheral.metadata.Controls))
|
||||
for _, control := range peripheral.metadata.Controls {
|
||||
control.Minimum = cloneIntPointer(control.Minimum)
|
||||
control.Maximum = cloneIntPointer(control.Maximum)
|
||||
control.MaximumLength = cloneIntPointer(control.MaximumLength)
|
||||
metadata.Controls = append(metadata.Controls, control)
|
||||
}
|
||||
inventory = append(inventory, metadata)
|
||||
}
|
||||
if manager.piGPIO != nil {
|
||||
inventory = append(inventory, RoverPeripheralMetadata{ID: "pi-gpio", Name: "Pi GPIO", Controls: []RoverPeripheralControl{}})
|
||||
}
|
||||
return inventory
|
||||
}
|
||||
|
||||
// SetControl validates the browser-shaped value against the ESP32 declaration,
|
||||
// then uses the private output mapping selected during startup. Neither the
|
||||
// server nor browser can choose a pin or switch a custom control into raw GPIO.
|
||||
func (manager *PeripheralManager) SetControl(peripheralID, controlID string, rawValue json.RawMessage) error {
|
||||
if manager == nil {
|
||||
return errors.New("rover peripherals disabled")
|
||||
}
|
||||
manager.mu.RLock()
|
||||
peripheral := manager.byID[peripheralID]
|
||||
manager.mu.RUnlock()
|
||||
if peripheral == nil {
|
||||
return fmt.Errorf("unknown peripheral %q", peripheralID)
|
||||
}
|
||||
control, exists := peripheral.controls[controlID]
|
||||
if !exists {
|
||||
return fmt.Errorf("unknown control %q on peripheral %q", controlID, peripheralID)
|
||||
}
|
||||
|
||||
value, err := decodePeripheralControlValue(control, rawValue)
|
||||
if err != nil {
|
||||
return fmt.Errorf("control %q: %w", controlID, err)
|
||||
}
|
||||
|
||||
switch control.Output.Type {
|
||||
case "digital":
|
||||
enabled := value.(bool)
|
||||
if control.Output.ActiveLow {
|
||||
enabled = !enabled
|
||||
}
|
||||
return peripheral.client.SetDigitalPin(byte(*control.Output.Pin), enabled)
|
||||
case "pwm", "servo":
|
||||
return peripheral.client.ExtendedAnalog(byte(*control.Output.Pin), value.(int))
|
||||
case "custom":
|
||||
return peripheral.client.SendPeripheralControl(control.ID, value)
|
||||
default:
|
||||
return fmt.Errorf("control has unsupported output %q", control.Output.Type)
|
||||
}
|
||||
}
|
||||
|
||||
func decodePeripheralControlValue(control PeripheralControl, rawValue json.RawMessage) (any, error) {
|
||||
if len(rawValue) == 0 {
|
||||
return nil, errors.New("value is required")
|
||||
}
|
||||
|
||||
switch control.Type {
|
||||
case "slider", "number":
|
||||
var value int
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return nil, errors.New("value must be a whole number")
|
||||
}
|
||||
if value < *control.Minimum || value > *control.Maximum {
|
||||
return nil, fmt.Errorf("value must be between %d and %d", *control.Minimum, *control.Maximum)
|
||||
}
|
||||
return value, nil
|
||||
case "button":
|
||||
var value bool
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return nil, errors.New("value must be true or false")
|
||||
}
|
||||
return value, nil
|
||||
case "text":
|
||||
var value string
|
||||
if err := json.Unmarshal(rawValue, &value); err != nil {
|
||||
return nil, errors.New("value must be text")
|
||||
}
|
||||
if utf8.RuneCountInString(value) > *control.MaximumLength {
|
||||
return nil, fmt.Errorf("value must contain at most %d characters", *control.MaximumLength)
|
||||
}
|
||||
return value, nil
|
||||
default:
|
||||
return nil, fmt.Errorf("unsupported control type %q", control.Type)
|
||||
}
|
||||
}
|
||||
|
||||
// Close releases every discovered USB connection exactly once. It does not
|
||||
// alter inventory or attempt to reconnect devices because shutdown/restart is
|
||||
// the lifecycle boundary chosen for this feature.
|
||||
func (manager *PeripheralManager) Close() {
|
||||
if manager == nil {
|
||||
return
|
||||
}
|
||||
manager.closeOnce.Do(func() {
|
||||
if manager.cancel != nil {
|
||||
manager.cancel()
|
||||
}
|
||||
if manager.piGPIO != nil {
|
||||
manager.piGPIO.close()
|
||||
}
|
||||
manager.mu.Lock()
|
||||
defer manager.mu.Unlock()
|
||||
for _, peripheral := range manager.peripherals {
|
||||
if err := peripheral.connection.Close(); err != nil {
|
||||
manager.logger.Printf("close rover peripheral %s on %s: %v", peripheral.metadata.ID, peripheral.devicePath, err)
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
@@ -0,0 +1,425 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"context"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"io"
|
||||
"log"
|
||||
"os"
|
||||
"path/filepath"
|
||||
"strings"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestPeripheralManagerDiscoversInventoryAndDispatchesControls(t *testing.T) {
|
||||
description := testPeripheralDescription("Bench accessory", false)
|
||||
connection := scriptedPeripheralConnection(t, description)
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB9"},
|
||||
map[string]*scriptedConnection{"/dev/ttyUSB9": connection},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/ttyUSB0", discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
inventory := manager.Inventory()
|
||||
if len(inventory) != 1 {
|
||||
t.Fatalf("inventory length = %d, want 1", len(inventory))
|
||||
}
|
||||
if inventory[0].ID != "firmata-0" || inventory[0].Name != "Bench accessory" {
|
||||
t.Fatalf("unexpected peripheral metadata: %#v", inventory[0])
|
||||
}
|
||||
wantBroadcast := `Rover peripheral "Bench accessory" connected as firmata-0 with 3 additional controls.`
|
||||
if broadcasts := manager.StartupBroadcasts(); len(broadcasts) != 1 || broadcasts[0] != wantBroadcast {
|
||||
t.Fatalf("startup broadcasts = %#v, want %q", broadcasts, wantBroadcast)
|
||||
}
|
||||
wantOrder := []string{"servoPosition", "lightBrightness", "specialAction"}
|
||||
for index, controlID := range wantOrder {
|
||||
if inventory[0].Controls[index].ID != controlID {
|
||||
t.Fatalf("control %d = %q, want %q", index, inventory[0].Controls[index].ID, controlID)
|
||||
}
|
||||
}
|
||||
*inventory[0].Controls[0].Minimum = 99
|
||||
if fresh := manager.Inventory(); *fresh[0].Controls[0].Minimum != 0 {
|
||||
t.Fatal("caller mutation changed the manager's fixed inventory")
|
||||
}
|
||||
|
||||
// Standard modes are configured once during discovery. Runtime slider
|
||||
// commands should consequently contain only EXTENDED_ANALOG, not repeated
|
||||
// mode changes that would detach and reattach a servo while it is moving.
|
||||
baseline := len(connection.Bytes())
|
||||
if err := manager.SetControl("firmata-0", "servoPosition", json.RawMessage(`90`)); err != nil {
|
||||
t.Fatalf("set servo: %v", err)
|
||||
}
|
||||
servoWrite := connection.Bytes()[baseline:]
|
||||
wantServo := []byte{firmataStartSysex, firmataExtendedAnalog, 13, 90, firmataEndSysex}
|
||||
if !bytes.Equal(servoWrite, wantServo) {
|
||||
t.Fatalf("servo bytes = %v, want %v", servoWrite, wantServo)
|
||||
}
|
||||
|
||||
baseline = len(connection.Bytes())
|
||||
if err := manager.SetControl("firmata-0", "lightBrightness", json.RawMessage(`128`)); err != nil {
|
||||
t.Fatalf("set PWM: %v", err)
|
||||
}
|
||||
pwmWrite := connection.Bytes()[baseline:]
|
||||
wantPWM := []byte{firmataStartSysex, firmataExtendedAnalog, 17, 0, 1, firmataEndSysex}
|
||||
if !bytes.Equal(pwmWrite, wantPWM) {
|
||||
t.Fatalf("PWM bytes = %v, want %v", pwmWrite, wantPWM)
|
||||
}
|
||||
|
||||
baseline = len(connection.Bytes())
|
||||
if err := manager.SetControl("firmata-0", "specialAction", json.RawMessage(`true`)); err != nil {
|
||||
t.Fatalf("set custom button: %v", err)
|
||||
}
|
||||
customWrite := connection.Bytes()[baseline:]
|
||||
if len(customWrite) < 5 || customWrite[1] != firmataPeripheralFeature || customWrite[2] != firmataPeripheralControl {
|
||||
t.Fatalf("custom control did not use rover-peripheral SysEx: %v", customWrite)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerBroadcastsNoDevices(t *testing.T) {
|
||||
manager := &PeripheralManager{byID: make(map[string]*managedPeripheral)}
|
||||
want := "No rover peripherals enabled or detected during startup."
|
||||
if messages := manager.StartupBroadcasts(); len(messages) != 1 || messages[0] != want {
|
||||
t.Fatalf("startup broadcasts = %#v, want %q", messages, want)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerReportsUnexpectedDisconnectOnce(t *testing.T) {
|
||||
connection := scriptedPeripheralConnection(t, testPeripheralDescription("Bench accessory", false))
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/accessory"}, map[string]*scriptedConnection{"/dev/accessory": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
// Closing the fake read stream models an unplugged USB serial adapter. The
|
||||
// manager should publish one identified failure and never attempt reconnect.
|
||||
_ = connection.Close()
|
||||
select {
|
||||
case failure := <-manager.Failures():
|
||||
if failure.ID != "firmata-0" || failure.Name != "Bench accessory" || !errors.Is(failure.Err, io.ErrClosedPipe) {
|
||||
t.Fatalf("unexpected failure: %#v", failure)
|
||||
}
|
||||
case <-time.After(time.Second):
|
||||
t.Fatal("timed out waiting for peripheral disconnect")
|
||||
}
|
||||
select {
|
||||
case duplicate := <-manager.Failures():
|
||||
t.Fatalf("unexpected duplicate disconnect: %#v", duplicate)
|
||||
case <-time.After(20 * time.Millisecond):
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerRejectsInvalidValuesBeforeWriting(t *testing.T) {
|
||||
connection := scriptedPeripheralConnection(t, testPeripheralDescription("Bench accessory", false))
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/accessory"}, map[string]*scriptedConnection{"/dev/accessory": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
baseline := len(connection.Bytes())
|
||||
invalid := []struct {
|
||||
control string
|
||||
value string
|
||||
}{
|
||||
{control: "servoPosition", value: `181`},
|
||||
{control: "lightBrightness", value: `12.5`},
|
||||
{control: "specialAction", value: `"yes"`},
|
||||
}
|
||||
for _, testCase := range invalid {
|
||||
if err := manager.SetControl("firmata-0", testCase.control, json.RawMessage(testCase.value)); err == nil {
|
||||
t.Fatalf("expected %s=%s to fail", testCase.control, testCase.value)
|
||||
}
|
||||
}
|
||||
if got := len(connection.Bytes()); got != baseline {
|
||||
t.Fatalf("invalid values wrote %d bytes", got-baseline)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerSkipsOtherFirmataFirmware(t *testing.T) {
|
||||
other := newScriptedConnection(testFirmwareFrame("StandardFirmata"))
|
||||
other.timeoutsBeforeRead = 1
|
||||
rover := scriptedPeripheralConnection(t, testPeripheralDescription("Rover accessory", false))
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyACM0", "/dev/ttyUSB0"},
|
||||
map[string]*scriptedConnection{
|
||||
"/dev/ttyACM0": other,
|
||||
"/dev/ttyUSB0": rover,
|
||||
},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
if inventory := manager.Inventory(); len(inventory) != 1 || inventory[0].ID != "firmata-0" || inventory[0].Name != "Rover accessory" {
|
||||
t.Fatalf("unexpected inventory: %#v", inventory)
|
||||
}
|
||||
if !other.Closed() {
|
||||
t.Fatal("non-rover Firmata port was not closed")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerFailsMalformedRoverDescription(t *testing.T) {
|
||||
connection := newScriptedConnection(
|
||||
testFirmwareFrame(peripheralFirmwareName),
|
||||
testCapabilityFrame(),
|
||||
testDescriptionFrame([]byte(`not-json`)),
|
||||
)
|
||||
connection.timeoutsBeforeRead = 1
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB0"},
|
||||
map[string]*scriptedConnection{"/dev/ttyUSB0": connection},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err == nil || !strings.Contains(err.Error(), "describe rover peripheral") {
|
||||
t.Fatalf("expected malformed description error, got manager=%v err=%v", manager, err)
|
||||
}
|
||||
if !connection.Closed() {
|
||||
t.Fatal("malformed rover peripheral connection was not closed")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerRejectsAdvertisedUnsupportedPinMode(t *testing.T) {
|
||||
description := testPeripheralDescription("Bad capability", false)
|
||||
rawDescription, err := json.Marshal(description)
|
||||
if err != nil {
|
||||
t.Fatalf("marshal description: %v", err)
|
||||
}
|
||||
connection := newScriptedConnection(
|
||||
testFirmwareFrame(peripheralFirmwareName),
|
||||
[]byte{
|
||||
firmataStartSysex, firmataCapabilityReply,
|
||||
FirmataPinModeOutput, 1, 0x7F,
|
||||
firmataEndSysex,
|
||||
},
|
||||
testDescriptionFrame(rawDescription),
|
||||
)
|
||||
connection.timeoutsBeforeRead = 1
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB0"},
|
||||
map[string]*scriptedConnection{"/dev/ttyUSB0": connection},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err == nil || !strings.Contains(err.Error(), "Firmata reported only 1 pins") {
|
||||
t.Fatalf("expected unsupported capability error, got manager=%v err=%v", manager, err)
|
||||
}
|
||||
if !connection.Closed() {
|
||||
t.Fatal("unsupported peripheral connection was not closed")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerRejectsDuplicateBuiltInProvidersWhenRoleIsSelected(t *testing.T) {
|
||||
first := scriptedPeripheralConnection(t, testPeripheralDescription("First", true))
|
||||
second := scriptedPeripheralConnection(t, testPeripheralDescription("Second", true))
|
||||
dependencies := testPeripheralDiscoveryDependencies(
|
||||
[]string{"/dev/ttyUSB0", "/dev/ttyUSB1"},
|
||||
map[string]*scriptedConnection{
|
||||
"/dev/ttyUSB0": first,
|
||||
"/dev/ttyUSB1": second,
|
||||
},
|
||||
)
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), "/dev/roomba", discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discovery should retain providers until native precedence is known: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
if _, err := manager.NewToggle("headlight", discardLogger()); err == nil || !strings.Contains(err.Error(), "role headlight has multiple providers") {
|
||||
t.Fatalf("expected duplicate provider selection error, got %v", err)
|
||||
}
|
||||
if first.Closed() || second.Closed() {
|
||||
t.Fatal("selection validation unexpectedly closed manager-owned ports")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerReturnsHardwareWriteFailure(t *testing.T) {
|
||||
connection := scriptedPeripheralConnection(t, testPeripheralDescription("Bench accessory", false))
|
||||
manager, err := discoverPeripheralManager(
|
||||
context.Background(),
|
||||
"/dev/roomba",
|
||||
discardLogger(),
|
||||
testPeripheralDiscoveryDependencies([]string{"/dev/accessory"}, map[string]*scriptedConnection{"/dev/accessory": connection}),
|
||||
)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
defer manager.Close()
|
||||
|
||||
connection.SetWriteError(errors.New("USB device removed"))
|
||||
err = manager.SetControl("firmata-0", "lightBrightness", json.RawMessage(`128`))
|
||||
if err == nil || !strings.Contains(err.Error(), "USB device removed") {
|
||||
t.Fatalf("expected hardware error, got %v", err)
|
||||
}
|
||||
select {
|
||||
case failure := <-manager.Failures():
|
||||
if failure.ID != "firmata-0" || !strings.Contains(failure.Err.Error(), "USB device removed") {
|
||||
t.Fatalf("unexpected write failure notification: %#v", failure)
|
||||
}
|
||||
case <-time.After(time.Second):
|
||||
t.Fatal("timed out waiting for write failure notification")
|
||||
}
|
||||
}
|
||||
|
||||
func TestPeripheralManagerPassesRoombaDeviceToCandidateExclusion(t *testing.T) {
|
||||
const roombaDevice = "/dev/serial/by-id/roomba-base"
|
||||
listed := false
|
||||
dependencies := peripheralDiscoveryDependencies{
|
||||
listCandidates: func(excluded string) ([]string, error) {
|
||||
listed = true
|
||||
if excluded != roombaDevice {
|
||||
t.Fatalf("excluded device = %q, want %q", excluded, roombaDevice)
|
||||
}
|
||||
return nil, nil
|
||||
},
|
||||
open: func(string) (io.ReadWriteCloser, error) { return nil, errors.New("unexpected open") },
|
||||
sleep: func(time.Duration) {},
|
||||
startupWait: 0,
|
||||
handshakeWait: time.Second,
|
||||
}
|
||||
|
||||
manager, err := discoverPeripheralManager(context.Background(), roombaDevice, discardLogger(), dependencies)
|
||||
if err != nil {
|
||||
t.Fatalf("discover: %v", err)
|
||||
}
|
||||
manager.Close()
|
||||
if !listed {
|
||||
t.Fatal("candidate listing was not called")
|
||||
}
|
||||
}
|
||||
|
||||
func TestUniquePeripheralCandidatesPrefersStableAliasAndExcludesRoomba(t *testing.T) {
|
||||
temporaryDirectory := t.TempDir()
|
||||
peripheralTarget := filepath.Join(temporaryDirectory, "ttyUSB0")
|
||||
roombaTarget := filepath.Join(temporaryDirectory, "ttyUSB1")
|
||||
if err := os.WriteFile(peripheralTarget, nil, 0o600); err != nil {
|
||||
t.Fatalf("create peripheral target: %v", err)
|
||||
}
|
||||
if err := os.WriteFile(roombaTarget, nil, 0o600); err != nil {
|
||||
t.Fatalf("create Roomba target: %v", err)
|
||||
}
|
||||
stableAlias := filepath.Join(temporaryDirectory, "usb-rover-peripheral")
|
||||
if err := os.Symlink(peripheralTarget, stableAlias); err != nil {
|
||||
t.Fatalf("create stable alias: %v", err)
|
||||
}
|
||||
|
||||
candidates := uniquePeripheralCandidates(
|
||||
[]string{stableAlias, peripheralTarget, roombaTarget},
|
||||
roombaTarget,
|
||||
)
|
||||
if len(candidates) != 1 || candidates[0] != stableAlias {
|
||||
t.Fatalf("candidates = %v, want stable peripheral alias only", candidates)
|
||||
}
|
||||
}
|
||||
|
||||
func testPeripheralDiscoveryDependencies(paths []string, connections map[string]*scriptedConnection) peripheralDiscoveryDependencies {
|
||||
return peripheralDiscoveryDependencies{
|
||||
listCandidates: func(string) ([]string, error) {
|
||||
return append([]string(nil), paths...), nil
|
||||
},
|
||||
open: func(devicePath string) (io.ReadWriteCloser, error) {
|
||||
connection := connections[devicePath]
|
||||
if connection == nil {
|
||||
return nil, errors.New("test connection not found")
|
||||
}
|
||||
return connection, nil
|
||||
},
|
||||
sleep: func(time.Duration) {},
|
||||
startupWait: 0,
|
||||
handshakeWait: time.Second,
|
||||
}
|
||||
}
|
||||
|
||||
func scriptedPeripheralConnection(t *testing.T, description PeripheralDescription) *scriptedConnection {
|
||||
t.Helper()
|
||||
rawDescription, err := json.Marshal(description)
|
||||
if err != nil {
|
||||
t.Fatalf("marshal description: %v", err)
|
||||
}
|
||||
connection := newScriptedConnection(
|
||||
testFirmwareFrame(peripheralFirmwareName),
|
||||
testCapabilityFrame(),
|
||||
testDescriptionFrame(rawDescription),
|
||||
)
|
||||
// The first read represents the quiet timeout used to drain boot output
|
||||
// before the client's parser starts consuming explicit query responses.
|
||||
connection.timeoutsBeforeRead = 1
|
||||
return connection
|
||||
}
|
||||
|
||||
func testPeripheralDescription(name string, provideHeadlight bool) PeripheralDescription {
|
||||
minimumServo, maximumServo := 0, 180
|
||||
minimumPWM, maximumPWM := 0, 255
|
||||
servoPin, pwmPin := 13, 17
|
||||
description := PeripheralDescription{
|
||||
Name: name,
|
||||
Controls: []PeripheralControl{
|
||||
{
|
||||
ID: "servoPosition", Type: "slider", Name: "Servo position",
|
||||
Minimum: &minimumServo, Maximum: &maximumServo,
|
||||
Output: PeripheralOutput{Type: "servo", Pin: &servoPin},
|
||||
},
|
||||
{
|
||||
ID: "lightBrightness", Type: "slider", Name: "Light brightness",
|
||||
Minimum: &minimumPWM, Maximum: &maximumPWM,
|
||||
Output: PeripheralOutput{Type: "pwm", Pin: &pwmPin},
|
||||
},
|
||||
{
|
||||
ID: "specialAction", Type: "button", Name: "Run special action", Mode: "momentary",
|
||||
Output: PeripheralOutput{Type: "custom"},
|
||||
},
|
||||
},
|
||||
}
|
||||
if provideHeadlight {
|
||||
description.RoverControls.Headlight = &PeripheralDigitalRole{Pin: 18}
|
||||
}
|
||||
return description
|
||||
}
|
||||
|
||||
func testFirmwareFrame(name string) []byte {
|
||||
frame := []byte{firmataStartSysex, firmataReportFirmware, 1, 0}
|
||||
frame = append(frame, EncodeFirmata7Bit([]byte(name))...)
|
||||
return append(frame, firmataEndSysex)
|
||||
}
|
||||
|
||||
func testCapabilityFrame() []byte {
|
||||
frame := []byte{firmataStartSysex, firmataCapabilityReply}
|
||||
for pin := 0; pin < 40; pin++ {
|
||||
// The test ESP32 reports the same three output modes as the reference
|
||||
// firmware. Repeating real pin entries also exercises capability parsing
|
||||
// independently of any particular example control pin.
|
||||
frame = append(frame, FirmataPinModeOutput, 1, FirmataPinModePWM, 8, FirmataPinModeServo, 14, 0x7F)
|
||||
}
|
||||
return append(frame, firmataEndSysex)
|
||||
}
|
||||
|
||||
func testDescriptionFrame(rawDescription []byte) []byte {
|
||||
frame := []byte{firmataStartSysex, firmataPeripheralFeature, firmataPeripheralDescription}
|
||||
frame = append(frame, EncodeFirmata7Bit(rawDescription)...)
|
||||
return append(frame, firmataEndSysex)
|
||||
}
|
||||
|
||||
func discardLogger() *log.Logger {
|
||||
return log.New(io.Discard, "", 0)
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"log"
|
||||
)
|
||||
|
||||
// piGPIOPeripheral is an in-process provider. It exists only when the parent
|
||||
// switch is enabled; merely loading saved GPIO settings never opens hardware.
|
||||
type piGPIOPeripheral struct {
|
||||
cfg PiGPIOConfig
|
||||
factories nativeHardwareControllerFactories
|
||||
pulser *BRCPulser
|
||||
cancel context.CancelFunc
|
||||
}
|
||||
|
||||
func (p *piGPIOPeripheral) roleID() string { return "pi-gpio" }
|
||||
|
||||
func (p *piGPIOPeripheral) hasRole(role string) bool {
|
||||
switch role {
|
||||
case "cameraServo":
|
||||
return p.cfg.CameraServo.Enabled
|
||||
case "headlight":
|
||||
return p.cfg.Headlight.Enabled
|
||||
case "laser":
|
||||
return p.cfg.Laser.Enabled
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
func (p *piGPIOPeripheral) cameraServo(logger *log.Logger) (CameraServoController, error) {
|
||||
return p.factories.newCameraServo(p.cfg.CameraServo, logger)
|
||||
}
|
||||
|
||||
func (p *piGPIOPeripheral) toggle(role string, logger *log.Logger) (ToggleController, error) {
|
||||
cfg := p.cfg.Headlight
|
||||
if role == "laser" {
|
||||
cfg = p.cfg.Laser
|
||||
}
|
||||
return p.factories.newToggle(role, cfg, logger)
|
||||
}
|
||||
|
||||
func (p *piGPIOPeripheral) start(ctx context.Context, logger *log.Logger) error {
|
||||
if !p.cfg.BRC.Enabled {
|
||||
return nil
|
||||
}
|
||||
pulser, err := NewBRCPulser(p.cfg.BRC, logger)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
p.pulser = pulser
|
||||
ctx, p.cancel = context.WithCancel(ctx)
|
||||
p.pulser.Start(ctx)
|
||||
return nil
|
||||
}
|
||||
|
||||
func (p *piGPIOPeripheral) close() {
|
||||
if p.cancel != nil {
|
||||
p.cancel()
|
||||
}
|
||||
if p.pulser != nil {
|
||||
p.pulser.Close()
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
{
|
||||
"piGpio": {
|
||||
"enabled": false,
|
||||
"brc": {
|
||||
"enabled": false,
|
||||
"gpioPin": 4,
|
||||
"gpioChip": "gpiochip0",
|
||||
"pulseEvery": "1m0s",
|
||||
"pulseWidth": "1s"
|
||||
},
|
||||
"cameraServo": {
|
||||
"enabled": false,
|
||||
"pin": 12,
|
||||
"freqHz": 50,
|
||||
"cycleLen": 20000,
|
||||
"minPulseUs": 900,
|
||||
"maxPulseUs": 2100,
|
||||
"minAngle": -15,
|
||||
"maxAngle": 30,
|
||||
"homeAngle": 0,
|
||||
"nudgeDegrees": 2,
|
||||
"allowRawPulse": false,
|
||||
"invert": false
|
||||
},
|
||||
"headlight": {
|
||||
"enabled": false,
|
||||
"gpioPin": 22,
|
||||
"gpioChip": "gpiochip0",
|
||||
"initialOn": false,
|
||||
"activeLow": true
|
||||
},
|
||||
"laser": {
|
||||
"enabled": false,
|
||||
"gpioPin": 27,
|
||||
"gpioChip": "gpiochip0",
|
||||
"initialOn": false,
|
||||
"activeLow": false
|
||||
}
|
||||
},
|
||||
"name": "laptop-rover",
|
||||
"description": "Debian laptop rover using USB serial, webcam, mic, and speakers.",
|
||||
"color": "#4DB6AC",
|
||||
"serverUrl": "ws://control-server.local:8080/rover",
|
||||
"serial": {
|
||||
"device": "/dev/ttyUSB0",
|
||||
"baud": 115200
|
||||
},
|
||||
"battery": {
|
||||
"full": 2068,
|
||||
"warn": 1700,
|
||||
"urgent": 1650
|
||||
},
|
||||
"maxWheelSpeed": 350,
|
||||
"media": {
|
||||
"rtspPort": 8554,
|
||||
"manage": true,
|
||||
"healthUrl": "",
|
||||
"healthInterval": "30s",
|
||||
"video": {
|
||||
"enabled": true,
|
||||
"service": "debian-laptop-video-publisher.service",
|
||||
"publisher": "debian-laptop-v4l2",
|
||||
"device": "/dev/video0",
|
||||
"inputFormat": "mjpeg",
|
||||
"width": 640,
|
||||
"height": 480,
|
||||
"fps": 30,
|
||||
"bitrate": 2000000,
|
||||
"inverted": false,
|
||||
"sensorMode": "1296:972"
|
||||
},
|
||||
"audioCapture": {
|
||||
"enabled": true,
|
||||
"service": "audio-only-publisher.service",
|
||||
"device": "default",
|
||||
"sampleRate": 48000,
|
||||
"channels": 2,
|
||||
"bitrate": 510000
|
||||
},
|
||||
"audioPlayback": {
|
||||
"enabled": true,
|
||||
"service": "audio-forward-listener.service",
|
||||
"device": "forward",
|
||||
"normalize": true,
|
||||
"normalizeFilter": "dynaudnorm=f=75:g=15:m=10:p=0.9,alimiter=limit=0.85:level=disabled"
|
||||
}
|
||||
},
|
||||
"audio": {
|
||||
"ttsEnabled": true,
|
||||
"defaultEngine": "flite",
|
||||
"defaultVoice": "rms",
|
||||
"defaultPitch": 50
|
||||
},
|
||||
"horn": {
|
||||
"enabled": true,
|
||||
"volume": 0.25,
|
||||
"sampleRate": 48000,
|
||||
"channels": 1,
|
||||
"device": "",
|
||||
"sineGain": 1,
|
||||
"sawGain": 0.7,
|
||||
"maxDuration": "10s"
|
||||
},
|
||||
"autoSideBrush": {
|
||||
"enabled": true,
|
||||
"speed": 20
|
||||
},
|
||||
"private": {
|
||||
"enabled": false,
|
||||
"safety": {
|
||||
"speedLimitEnabled": false,
|
||||
"speedLimitMaxWheelSpeed": 250,
|
||||
"hardOvercurrentEnabled": false,
|
||||
"overcurrentStopMs": 300,
|
||||
"hardBumpEnabled": false,
|
||||
"bumpBackoffSpeed": 250,
|
||||
"bumpBackoffMs": 350,
|
||||
"cliffEnabled": false,
|
||||
"cliffBackoffSpeed": 250,
|
||||
"cliffBackoffMs": 500,
|
||||
"virtualWallEnabled": true,
|
||||
"virtualWallBackoffSpeed": 250,
|
||||
"virtualWallBackoffMs": 500,
|
||||
"triggerCooldownMs": 800
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
{
|
||||
"piGpio": {
|
||||
"enabled": false,
|
||||
"brc": {
|
||||
"enabled": true,
|
||||
"gpioPin": 4,
|
||||
"gpioChip": "gpiochip0",
|
||||
"pulseEvery": "1m",
|
||||
"pulseWidth": "1s"
|
||||
},
|
||||
"cameraServo": {
|
||||
"enabled": false,
|
||||
"pin": 19,
|
||||
"freqHz": 50,
|
||||
"cycleLen": 20000,
|
||||
"minPulseUs": 900,
|
||||
"maxPulseUs": 2100,
|
||||
"minAngle": -15,
|
||||
"maxAngle": 30,
|
||||
"homeAngle": 0,
|
||||
"nudgeDegrees": 2,
|
||||
"allowRawPulse": false,
|
||||
"invert": false
|
||||
},
|
||||
"headlight": {
|
||||
"enabled": true,
|
||||
"gpioPin": 22,
|
||||
"gpioChip": "gpiochip0",
|
||||
"initialOn": false,
|
||||
"activeLow": true
|
||||
},
|
||||
"laser": {
|
||||
"enabled": false,
|
||||
"gpioPin": 27,
|
||||
"gpioChip": "gpiochip0",
|
||||
"initialOn": false,
|
||||
"activeLow": false
|
||||
}
|
||||
},
|
||||
"name": "roomba-alpha",
|
||||
"description": "Loves corners, hates cords.",
|
||||
"color": "",
|
||||
"serverUrl": "ws://control-server.local:8080/rover",
|
||||
"serial": {
|
||||
"device": "/dev/ttyAMA0",
|
||||
"baud": 115200
|
||||
},
|
||||
"battery": {
|
||||
"full": 2068,
|
||||
"warn": 1700,
|
||||
"urgent": 1650
|
||||
},
|
||||
"maxWheelSpeed": 350,
|
||||
"media": {
|
||||
"rtspPort": 8554,
|
||||
"manage": false,
|
||||
"healthUrl": "http://127.0.0.1:9997/v3/paths/list",
|
||||
"healthInterval": "30s",
|
||||
"video": {
|
||||
"enabled": true,
|
||||
"service": "video-publisher.service",
|
||||
"publisher": "pi-libcamera",
|
||||
"device": "",
|
||||
"inputFormat": "",
|
||||
"width": 640,
|
||||
"height": 480,
|
||||
"fps": 30,
|
||||
"bitrate": 2000000,
|
||||
"inverted": true,
|
||||
"sensorMode": "1296:972"
|
||||
},
|
||||
"audioCapture": {
|
||||
"enabled": false,
|
||||
"service": "audio-only-publisher.service",
|
||||
"device": "hw:0,0",
|
||||
"sampleRate": 48000,
|
||||
"channels": 2,
|
||||
"bitrate": 510000
|
||||
},
|
||||
"audioPlayback": {
|
||||
"enabled": true,
|
||||
"service": "audio-forward-listener.service",
|
||||
"device": "forward",
|
||||
"normalize": true,
|
||||
"normalizeFilter": "dynaudnorm=f=75:g=15:m=10:p=0.9,alimiter=limit=0.85:level=disabled"
|
||||
}
|
||||
},
|
||||
"audio": {
|
||||
"ttsEnabled": false,
|
||||
"defaultEngine": "flite",
|
||||
"defaultVoice": "rms",
|
||||
"defaultPitch": 50
|
||||
},
|
||||
"horn": {
|
||||
"enabled": false,
|
||||
"volume": 0.25,
|
||||
"sampleRate": 48000,
|
||||
"channels": 1,
|
||||
"device": "",
|
||||
"sineGain": 1,
|
||||
"sawGain": 0.7,
|
||||
"maxDuration": "10s"
|
||||
},
|
||||
"autoSideBrush": {
|
||||
"enabled": true,
|
||||
"speed": 20
|
||||
},
|
||||
"private": {
|
||||
"enabled": false,
|
||||
"safety": {
|
||||
"speedLimitEnabled": false,
|
||||
"speedLimitMaxWheelSpeed": 250,
|
||||
"hardOvercurrentEnabled": false,
|
||||
"overcurrentStopMs": 300,
|
||||
"hardBumpEnabled": false,
|
||||
"bumpBackoffSpeed": 250,
|
||||
"bumpBackoffMs": 350,
|
||||
"cliffEnabled": false,
|
||||
"cliffBackoffSpeed": 250,
|
||||
"cliffBackoffMs": 500,
|
||||
"virtualWallEnabled": true,
|
||||
"virtualWallBackoffSpeed": 250,
|
||||
"virtualWallBackoffMs": 500,
|
||||
"triggerCooldownMs": 800
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
{
|
||||
"piGpio": {
|
||||
"enabled": false,
|
||||
"brc": {
|
||||
"enabled": true,
|
||||
"gpioPin": 4,
|
||||
"gpioChip": "gpiochip0",
|
||||
"pulseEvery": "1m",
|
||||
"pulseWidth": "1s"
|
||||
},
|
||||
"cameraServo": {
|
||||
"enabled": false,
|
||||
"pin": 12,
|
||||
"freqHz": 50,
|
||||
"cycleLen": 20000,
|
||||
"minPulseUs": 900,
|
||||
"maxPulseUs": 2100,
|
||||
"minAngle": -15,
|
||||
"maxAngle": 30,
|
||||
"homeAngle": 0,
|
||||
"nudgeDegrees": 2,
|
||||
"allowRawPulse": false,
|
||||
"invert": false
|
||||
},
|
||||
"headlight": {
|
||||
"enabled": true,
|
||||
"gpioPin": 22,
|
||||
"gpioChip": "gpiochip0",
|
||||
"initialOn": false,
|
||||
"activeLow": true
|
||||
},
|
||||
"laser": {
|
||||
"enabled": false,
|
||||
"gpioPin": 27,
|
||||
"gpioChip": "gpiochip0",
|
||||
"initialOn": false,
|
||||
"activeLow": false
|
||||
}
|
||||
},
|
||||
"name": "roomba-alpha",
|
||||
"description": "Loves corners, hates cords.",
|
||||
"color": "#4DB6AC",
|
||||
"serverUrl": "ws://control-server.local:8080/rover",
|
||||
"serial": {
|
||||
"device": "/dev/ttyAMA0",
|
||||
"baud": 115200
|
||||
},
|
||||
"battery": {
|
||||
"full": 2068,
|
||||
"warn": 1700,
|
||||
"urgent": 1650
|
||||
},
|
||||
"maxWheelSpeed": 350,
|
||||
"media": {
|
||||
"rtspPort": 8554,
|
||||
"manage": true,
|
||||
"healthUrl": "",
|
||||
"healthInterval": "30s",
|
||||
"video": {
|
||||
"enabled": true,
|
||||
"service": "video-publisher.service",
|
||||
"publisher": "pi-libcamera",
|
||||
"device": "",
|
||||
"inputFormat": "",
|
||||
"width": 640,
|
||||
"height": 480,
|
||||
"fps": 30,
|
||||
"bitrate": 2000000,
|
||||
"inverted": true,
|
||||
"sensorMode": "1296:972"
|
||||
},
|
||||
"audioCapture": {
|
||||
"enabled": false,
|
||||
"service": "audio-only-publisher.service",
|
||||
"device": "hw:0,0",
|
||||
"sampleRate": 48000,
|
||||
"channels": 2,
|
||||
"bitrate": 510000
|
||||
},
|
||||
"audioPlayback": {
|
||||
"enabled": true,
|
||||
"service": "audio-forward-listener.service",
|
||||
"device": "forward",
|
||||
"normalize": true,
|
||||
"normalizeFilter": "dynaudnorm=f=75:g=15:m=10:p=0.9,alimiter=limit=0.85:level=disabled"
|
||||
}
|
||||
},
|
||||
"audio": {
|
||||
"ttsEnabled": false,
|
||||
"defaultEngine": "flite",
|
||||
"defaultVoice": "rms",
|
||||
"defaultPitch": 50
|
||||
},
|
||||
"horn": {
|
||||
"enabled": false,
|
||||
"volume": 0.25,
|
||||
"sampleRate": 48000,
|
||||
"channels": 1,
|
||||
"device": "",
|
||||
"sineGain": 1,
|
||||
"sawGain": 0.7,
|
||||
"maxDuration": "1.2s"
|
||||
},
|
||||
"autoSideBrush": {
|
||||
"enabled": true,
|
||||
"speed": 20
|
||||
},
|
||||
"private": {
|
||||
"enabled": false,
|
||||
"safety": {
|
||||
"speedLimitEnabled": false,
|
||||
"speedLimitMaxWheelSpeed": 250,
|
||||
"hardOvercurrentEnabled": false,
|
||||
"overcurrentStopMs": 300,
|
||||
"hardBumpEnabled": false,
|
||||
"bumpBackoffSpeed": 250,
|
||||
"bumpBackoffMs": 350,
|
||||
"cliffEnabled": false,
|
||||
"cliffBackoffSpeed": 250,
|
||||
"cliffBackoffMs": 500,
|
||||
"virtualWallEnabled": true,
|
||||
"virtualWallBackoffSpeed": 250,
|
||||
"virtualWallBackoffMs": 500,
|
||||
"triggerCooldownMs": 800
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package roverd
|
||||
|
||||
var (
|
||||
defaultStreamPackets = []byte{100, 21, 34}
|
||||
packetSizes = map[byte]int{
|
||||
100: 80,
|
||||
21: 1,
|
||||
34: 1,
|
||||
}
|
||||
expectedPayloadLength = func() int {
|
||||
sum := 0
|
||||
for _, id := range defaultStreamPackets {
|
||||
sum += 1 + packetSizes[id]
|
||||
}
|
||||
return sum
|
||||
}()
|
||||
)
|
||||
|
||||
type SensorSample struct {
|
||||
Timestamp int64
|
||||
ChargingState byte
|
||||
ChargeSources byte
|
||||
}
|
||||
@@ -0,0 +1,136 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"context"
|
||||
"encoding/hex"
|
||||
"io"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
const (
|
||||
sensorHeader = 19
|
||||
sensorReadTimeout = 150 * time.Millisecond
|
||||
sensorThrottleMinimum = 50 * time.Millisecond
|
||||
)
|
||||
|
||||
type SensorStreamer struct {
|
||||
r io.Reader
|
||||
rawOut chan<- []byte
|
||||
parsed chan<- SensorSample
|
||||
logger *log.Logger
|
||||
}
|
||||
|
||||
func NewSensorStreamer(r io.Reader, rawOut chan<- []byte, parsed chan<- SensorSample, logger *log.Logger) *SensorStreamer {
|
||||
return &SensorStreamer{r: r, rawOut: rawOut, parsed: parsed, logger: logger}
|
||||
}
|
||||
|
||||
func (s *SensorStreamer) Run(ctx context.Context) {
|
||||
reader := bufio.NewReader(s.r)
|
||||
var nextSend time.Time
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
}
|
||||
|
||||
header, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
if ctx.Err() != nil {
|
||||
return
|
||||
}
|
||||
continue
|
||||
}
|
||||
if header != sensorHeader {
|
||||
continue
|
||||
}
|
||||
nBytes, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
frame := make([]byte, int(nBytes)+3)
|
||||
frame[0] = sensorHeader
|
||||
frame[1] = nBytes
|
||||
if _, err := io.ReadFull(reader, frame[2:]); err != nil {
|
||||
continue
|
||||
}
|
||||
|
||||
if !validateChecksum(frame) {
|
||||
s.logger.Printf("sensor checksum failed: %s", hex.EncodeToString(frame))
|
||||
continue
|
||||
}
|
||||
|
||||
now := time.Now()
|
||||
if !nextSend.IsZero() && now.Before(nextSend) {
|
||||
continue
|
||||
}
|
||||
nextSend = now.Add(sensorThrottleMinimum)
|
||||
|
||||
select {
|
||||
case s.rawOut <- frame:
|
||||
default:
|
||||
}
|
||||
|
||||
if s.parsed != nil {
|
||||
if sample, ok := decodeSensorSample(frame); ok {
|
||||
select {
|
||||
case s.parsed <- sample:
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func validateChecksum(buf []byte) bool {
|
||||
var sum int
|
||||
for _, b := range buf {
|
||||
sum += int(b)
|
||||
}
|
||||
return byte(sum&0xFF) == 0
|
||||
}
|
||||
|
||||
func decodeSensorSample(frame []byte) (SensorSample, bool) {
|
||||
if len(frame) < 3 {
|
||||
return SensorSample{}, false
|
||||
}
|
||||
nBytes := int(frame[1])
|
||||
if nBytes+3 != len(frame) {
|
||||
return SensorSample{}, false
|
||||
}
|
||||
payload := frame[2 : 2+nBytes]
|
||||
if len(payload) != expectedPayloadLength {
|
||||
return SensorSample{}, false
|
||||
}
|
||||
|
||||
idx := 0
|
||||
var sample SensorSample
|
||||
var seen byte
|
||||
for idx < len(payload) {
|
||||
id := payload[idx]
|
||||
idx++
|
||||
size, ok := packetSizes[id]
|
||||
if !ok {
|
||||
return SensorSample{}, false
|
||||
}
|
||||
if idx+size > len(payload) {
|
||||
return SensorSample{}, false
|
||||
}
|
||||
segment := payload[idx : idx+size]
|
||||
switch id {
|
||||
case 21:
|
||||
sample.ChargingState = segment[0]
|
||||
seen |= 1
|
||||
case 34:
|
||||
sample.ChargeSources = segment[0]
|
||||
seen |= 2
|
||||
}
|
||||
idx += size
|
||||
}
|
||||
sample.Timestamp = time.Now().UnixMilli()
|
||||
return sample, seen&3 == 3
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"log"
|
||||
"math/rand"
|
||||
"time"
|
||||
)
|
||||
|
||||
const sensorHeader = 19
|
||||
|
||||
type SensorStreamer struct {
|
||||
rawOut chan<- []byte
|
||||
parsed chan<- SensorSample
|
||||
logger *log.Logger
|
||||
}
|
||||
|
||||
func NewSensorStreamer(_ interface{}, rawOut chan<- []byte, parsed chan<- SensorSample, logger *log.Logger) *SensorStreamer {
|
||||
return &SensorStreamer{rawOut: rawOut, parsed: parsed, logger: logger}
|
||||
}
|
||||
|
||||
func (s *SensorStreamer) Run(ctx context.Context) {
|
||||
ticker := time.NewTicker(200 * time.Millisecond)
|
||||
defer ticker.Stop()
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-ticker.C:
|
||||
frame := buildDummyFrame()
|
||||
select {
|
||||
case s.rawOut <- frame:
|
||||
default:
|
||||
}
|
||||
sample := SensorSample{
|
||||
Timestamp: time.Now().UnixMilli(),
|
||||
ChargingState: 3, // trickle charging
|
||||
ChargeSources: 0b10, // home base present
|
||||
}
|
||||
select {
|
||||
case s.parsed <- sample:
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func buildDummyFrame() []byte {
|
||||
payload := make([]byte, 0, expectedPayloadLength)
|
||||
payload = append(payload, 100)
|
||||
group := make([]byte, packetSizes[100])
|
||||
group[0] = byte(rand.Intn(16)) // bumps
|
||||
payload = append(payload, group...)
|
||||
payload = append(payload, 21, 3)
|
||||
payload = append(payload, 34, 0b10)
|
||||
|
||||
buf := make([]byte, 0, len(payload)+3)
|
||||
buf = append(buf, sensorHeader, byte(len(payload)))
|
||||
buf = append(buf, payload...)
|
||||
checksum := calcChecksum(buf)
|
||||
buf = append(buf, checksum)
|
||||
return buf
|
||||
}
|
||||
|
||||
func calcChecksum(buf []byte) byte {
|
||||
sum := 0
|
||||
for _, b := range buf {
|
||||
sum += int(b)
|
||||
}
|
||||
return byte((-sum) & 0xFF)
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
//go:build !dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"encoding/base64"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"sync"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
type SerialAdapter struct {
|
||||
port io.ReadWriteCloser
|
||||
encoder *base64.Encoding
|
||||
mu sync.Mutex
|
||||
log *log.Logger
|
||||
}
|
||||
|
||||
func OpenSerial(cfg SerialConfig) (*serial.Port, error) {
|
||||
return serial.OpenPort(&serial.Config{
|
||||
Name: cfg.Device,
|
||||
Baud: cfg.Baud,
|
||||
ReadTimeout: sensorReadTimeout,
|
||||
})
|
||||
}
|
||||
|
||||
func NewSerialAdapter(port io.ReadWriteCloser, logger *log.Logger) *SerialAdapter {
|
||||
return &SerialAdapter{
|
||||
port: port,
|
||||
encoder: base64.StdEncoding,
|
||||
log: logger,
|
||||
}
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) write(buf []byte) error {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
|
||||
n, err := s.port.Write(buf)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
if n != len(buf) {
|
||||
return fmt.Errorf("short write %d/%d", n, len(buf))
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) DriveDirect(left, right int) error {
|
||||
payload := []byte{
|
||||
145,
|
||||
byte((right >> 8) & 0xFF),
|
||||
byte(right & 0xFF),
|
||||
byte((left >> 8) & 0xFF),
|
||||
byte(left & 0xFF),
|
||||
}
|
||||
return s.write(payload)
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) MotorPWM(main, side, vacuum int) error {
|
||||
payload := []byte{
|
||||
144,
|
||||
byte(main & 0xFF),
|
||||
byte(side & 0xFF),
|
||||
byte(vacuum & 0xFF),
|
||||
}
|
||||
return s.write(payload)
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) StartSensorStream(packets []byte) error {
|
||||
if len(packets) == 0 {
|
||||
return errors.New("sensor stream requires packets")
|
||||
}
|
||||
payload := []byte{148, byte(len(packets))}
|
||||
payload = append(payload, packets...)
|
||||
return s.write(payload)
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) PauseSensorStream(pause bool) error {
|
||||
state := byte(1)
|
||||
if pause {
|
||||
state = 0
|
||||
}
|
||||
return s.write([]byte{150, state})
|
||||
}
|
||||
|
||||
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})
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) PlaySong(slot int, notes []songNote) error {
|
||||
if len(notes) == 0 {
|
||||
return fmt.Errorf("song requires at least one note")
|
||||
}
|
||||
if len(notes) > 16 {
|
||||
return fmt.Errorf("song supports up to 16 notes, got %d", len(notes))
|
||||
}
|
||||
if slot < 0 || slot > 4 {
|
||||
return fmt.Errorf("song slot must be 0-4")
|
||||
}
|
||||
|
||||
payload := []byte{140, byte(slot), byte(len(notes))}
|
||||
for _, n := range notes {
|
||||
note := clampInt(n.Note, 31, 127)
|
||||
duration := clampInt(n.Duration, 1, 255)
|
||||
payload = append(payload, byte(note), byte(duration))
|
||||
}
|
||||
if err := s.write(payload); err != nil {
|
||||
return err
|
||||
}
|
||||
return s.write([]byte{141, byte(slot)})
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
//go:build dummy
|
||||
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"io"
|
||||
"log"
|
||||
)
|
||||
|
||||
type dummyPort struct{}
|
||||
|
||||
func (dummyPort) Read(p []byte) (int, error) { return 0, io.EOF }
|
||||
func (dummyPort) Write(p []byte) (int, error) { return len(p), nil }
|
||||
func (dummyPort) Close() error { return nil }
|
||||
|
||||
func OpenSerial(cfg SerialConfig) (io.ReadWriteCloser, error) {
|
||||
return dummyPort{}, nil
|
||||
}
|
||||
|
||||
type SerialAdapter struct {
|
||||
log *log.Logger
|
||||
}
|
||||
|
||||
func NewSerialAdapter(_ io.ReadWriteCloser, logger *log.Logger) *SerialAdapter {
|
||||
return &SerialAdapter{log: logger}
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) DriveDirect(left, right int) error {
|
||||
s.log.Printf("[dummy] drive L=%d R=%d", left, right)
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) MotorPWM(main, side, vacuum int) error {
|
||||
s.log.Printf("[dummy] motor main=%d side=%d vacuum=%d", main, side, vacuum)
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) StartSensorStream(packets []byte) error {
|
||||
if len(packets) == 0 {
|
||||
return errors.New("sensor stream requires packets")
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) PauseSensorStream(pause bool) error {
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) SendRaw(raw []byte) error {
|
||||
s.log.Printf("[dummy] raw %v", raw)
|
||||
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
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) PlaySong(slot int, notes []songNote) error {
|
||||
s.log.Printf("[dummy] play song slot=%d notes=%v", slot, notes)
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"fmt"
|
||||
"os/exec"
|
||||
"strings"
|
||||
"time"
|
||||
)
|
||||
|
||||
func (c *WSClient) handleTTSPayload(ctx context.Context, payload *ttsPayload) error {
|
||||
if payload == nil {
|
||||
return fmt.Errorf("tts payload required")
|
||||
}
|
||||
if !c.cfg.Audio.TTSEnabled {
|
||||
return fmt.Errorf("tts disabled on rover")
|
||||
}
|
||||
if payload.Speak == false {
|
||||
return nil
|
||||
}
|
||||
text := strings.TrimSpace(payload.Text)
|
||||
if text == "" {
|
||||
return fmt.Errorf("tts text required")
|
||||
}
|
||||
if len([]rune(text)) > 512 {
|
||||
text = string([]rune(text)[:512])
|
||||
}
|
||||
|
||||
engine := strings.ToLower(strings.TrimSpace(payload.Engine))
|
||||
if engine == "" {
|
||||
engine = strings.ToLower(strings.TrimSpace(c.cfg.Audio.DefaultEngine))
|
||||
}
|
||||
if engine == "" {
|
||||
engine = "flite"
|
||||
}
|
||||
|
||||
voice := strings.TrimSpace(payload.Voice)
|
||||
if voice == "" {
|
||||
voice = strings.TrimSpace(c.cfg.Audio.DefaultVoice)
|
||||
}
|
||||
espeakPitch := int(payload.Pitch)
|
||||
if espeakPitch <= 0 {
|
||||
espeakPitch = c.cfg.Audio.DefaultPitch
|
||||
}
|
||||
espeakPitch = clampInt(espeakPitch, 0, 99)
|
||||
|
||||
runCtx, cancel := context.WithTimeout(ctx, 40*time.Second)
|
||||
defer cancel()
|
||||
|
||||
var cmd *exec.Cmd
|
||||
switch engine {
|
||||
case "espeak", "e":
|
||||
args := []string{}
|
||||
if espeakPitch > 0 {
|
||||
args = append(args, "-p", fmt.Sprintf("%d", espeakPitch))
|
||||
}
|
||||
args = append(args, text)
|
||||
cmd = exec.CommandContext(runCtx, "espeak", args...)
|
||||
case "flite", "f":
|
||||
args := []string{}
|
||||
if voice != "" {
|
||||
args = append(args, "-voice", voice)
|
||||
}
|
||||
args = append(args, "-t", text)
|
||||
cmd = exec.CommandContext(runCtx, "flite", args...)
|
||||
case "chromegtts", "googletts", "gtts", "google":
|
||||
if c.chromeTTS == nil {
|
||||
return fmt.Errorf("chromegtts unavailable")
|
||||
}
|
||||
return c.chromeTTS.Speak(runCtx, text, voice, payload.Pitch, payload.Speed)
|
||||
default:
|
||||
return fmt.Errorf("unsupported tts engine: %s", engine)
|
||||
}
|
||||
|
||||
out, err := cmd.CombinedOutput()
|
||||
if err != nil {
|
||||
return fmt.Errorf("tts exec failed: %w (%s)", err, string(out))
|
||||
}
|
||||
return nil
|
||||
}
|
||||
@@ -0,0 +1,790 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/base64"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"log"
|
||||
"os/exec"
|
||||
"sync"
|
||||
"sync/atomic"
|
||||
"time"
|
||||
|
||||
"nhooyr.io/websocket"
|
||||
)
|
||||
|
||||
type WSClient struct {
|
||||
autoCharge *AutoChargeController
|
||||
commandMu sync.Mutex
|
||||
stopping atomic.Bool
|
||||
controlEvents chan RoverEvent
|
||||
cfg *Config
|
||||
adapter *SerialAdapter
|
||||
sensorFrames <-chan []byte
|
||||
events chan RoverEvent
|
||||
media *MediaSupervisor
|
||||
servo CameraServoController
|
||||
horn *HornSynth
|
||||
headlight ToggleController
|
||||
laser ToggleController
|
||||
peripherals *PeripheralManager
|
||||
log *log.Logger
|
||||
console *ConsoleNotifier
|
||||
recoverMu sync.Mutex
|
||||
recovering bool
|
||||
watchdogMu sync.Mutex
|
||||
watchdogOpen bool
|
||||
watchdogOK bool
|
||||
ttsQueue chan *ttsPayload
|
||||
chromeTTS *chromeTTSDaemon
|
||||
lastAux motorPWMPayload
|
||||
autoSideOn bool
|
||||
connMu sync.Mutex
|
||||
connected bool
|
||||
disconnectT *time.Timer
|
||||
rebootT *time.Timer
|
||||
seekIssued bool
|
||||
rebootIssued bool
|
||||
audioLevels AudioLevels
|
||||
audioMu sync.RWMutex
|
||||
}
|
||||
|
||||
func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events chan RoverEvent, media *MediaSupervisor, servo CameraServoController, headlight ToggleController, laser ToggleController, peripherals *PeripheralManager, logger *log.Logger, console *ConsoleNotifier) *WSClient {
|
||||
var ttsQueue chan *ttsPayload
|
||||
if cfg.Audio.TTSEnabled {
|
||||
ttsQueue = make(chan *ttsPayload, 2)
|
||||
}
|
||||
var chromeTTS *chromeTTSDaemon
|
||||
if cfg.Audio.TTSEnabled {
|
||||
chromeTTS = NewChromeTTSDaemon(logger)
|
||||
}
|
||||
var horn *HornSynth
|
||||
if cfg.Horn.Enabled {
|
||||
horn = NewHornSynth(cfg.Horn, logger)
|
||||
}
|
||||
client := &WSClient{
|
||||
controlEvents: make(chan RoverEvent, 16),
|
||||
cfg: cfg,
|
||||
adapter: adapter,
|
||||
sensorFrames: frames,
|
||||
events: events,
|
||||
media: media,
|
||||
servo: servo,
|
||||
horn: horn,
|
||||
headlight: headlight,
|
||||
laser: laser,
|
||||
peripherals: peripherals,
|
||||
log: logger,
|
||||
console: console,
|
||||
ttsQueue: ttsQueue,
|
||||
chromeTTS: chromeTTS,
|
||||
audioLevels: AudioLevels{
|
||||
HornGain: 1.0,
|
||||
TTSGain: 1.0,
|
||||
ForwardGain: 1.0,
|
||||
},
|
||||
}
|
||||
client.applyAudioLevelsToMixer(client.audioLevels)
|
||||
return client
|
||||
}
|
||||
|
||||
func (c *WSClient) Run(ctx context.Context) error {
|
||||
if c.stopping.Load() {
|
||||
return fmt.Errorf("roverd is stopping")
|
||||
}
|
||||
ctx, stop := context.WithCancel(ctx)
|
||||
defer stop()
|
||||
dialCtx, cancel := context.WithTimeout(ctx, dialTimeout)
|
||||
conn, _, err := websocket.Dial(dialCtx, c.cfg.ServerURL, nil)
|
||||
cancel()
|
||||
if err != nil {
|
||||
c.markDisconnected()
|
||||
return err
|
||||
}
|
||||
c.markConnected()
|
||||
defer conn.Close(websocket.StatusInternalError, "closed")
|
||||
defer c.markDisconnected()
|
||||
if c.chromeTTS != nil {
|
||||
defer c.chromeTTS.Shutdown()
|
||||
}
|
||||
|
||||
if err := c.sendHello(ctx, conn); err != nil {
|
||||
return err
|
||||
}
|
||||
if err := c.ensureSensorStream(); err != nil {
|
||||
c.log.Printf("sensor stream init failed: %v", err)
|
||||
}
|
||||
|
||||
errCh := make(chan error, 2)
|
||||
c.startTTSWorker(ctx)
|
||||
c.warmChromeTTS(ctx)
|
||||
go func() {
|
||||
errCh <- c.readLoop(ctx, conn)
|
||||
}()
|
||||
go func() {
|
||||
if err := keepalive(ctx, conn); err != nil {
|
||||
errCh <- err
|
||||
}
|
||||
}()
|
||||
go c.forwardSensors(ctx, conn)
|
||||
go c.forwardEvents(ctx, conn)
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
conn.Close(websocket.StatusNormalClosure, "context done")
|
||||
return ctx.Err()
|
||||
case err := <-errCh:
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
|
||||
// Built-in metadata comes from the selected controller, not necessarily
|
||||
// saved configuration. An ESP32 can enable a role whose native GPIO entry is disabled.
|
||||
cameraServoConfig := CameraServoConfig{}
|
||||
if c.servo != nil {
|
||||
cameraServoConfig = c.servo.Configuration()
|
||||
}
|
||||
headlightConfig := GPIOToggleConfig{}
|
||||
if c.headlight != nil {
|
||||
headlightConfig = c.headlight.Configuration()
|
||||
}
|
||||
laserConfig := GPIOToggleConfig{}
|
||||
if c.laser != nil {
|
||||
laserConfig = c.laser.Configuration()
|
||||
}
|
||||
msg := helloMessage{
|
||||
Type: "hello",
|
||||
Name: c.cfg.Name,
|
||||
Description: c.cfg.Description,
|
||||
Color: c.cfg.Color,
|
||||
Battery: c.cfg.Battery,
|
||||
MaxWheelSpeed: c.cfg.MaxWheelMMs,
|
||||
Media: c.cfg.MediaMetadata(),
|
||||
CameraServo: cameraServoConfig,
|
||||
Audio: c.cfg.Audio,
|
||||
Horn: c.cfg.Horn,
|
||||
Headlight: headlightConfig,
|
||||
Laser: laserConfig,
|
||||
Peripherals: c.peripherals.Inventory(),
|
||||
Private: c.cfg.Private,
|
||||
}
|
||||
c.log.Printf("sending hello (camera servo enabled=%v pin=%d)", msg.CameraServo.Enabled, msg.CameraServo.Pin)
|
||||
return writeJSON(ctx, conn, msg)
|
||||
}
|
||||
|
||||
func (c *WSClient) readLoop(ctx context.Context, conn *websocket.Conn) error {
|
||||
for {
|
||||
_, data, err := conn.Read(ctx)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
var msg inboundMessage
|
||||
if err := json.Unmarshal(data, &msg); err != nil {
|
||||
c.log.Printf("invalid command: %v", err)
|
||||
continue
|
||||
}
|
||||
if msg.ID == "" {
|
||||
continue
|
||||
}
|
||||
status := "ok"
|
||||
cmdErr := c.dispatch(ctx, &msg)
|
||||
if cmdErr != nil {
|
||||
status = "error"
|
||||
}
|
||||
ack := ackMessage{
|
||||
Type: "ack",
|
||||
ID: msg.ID,
|
||||
Status: status,
|
||||
}
|
||||
if cmdErr != nil {
|
||||
ack.Error = cmdErr.Error()
|
||||
}
|
||||
if err := writeJSON(ctx, conn, ack); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
|
||||
c.commandMu.Lock()
|
||||
defer c.commandMu.Unlock()
|
||||
if c.stopping.Load() {
|
||||
return fmt.Errorf("roverd is stopping")
|
||||
}
|
||||
switch {
|
||||
case msg.DriveDirect != nil:
|
||||
left := clamp(msg.DriveDirect.Left, -c.cfg.MaxWheelMMs, c.cfg.MaxWheelMMs)
|
||||
right := clamp(msg.DriveDirect.Right, -c.cfg.MaxWheelMMs, c.cfg.MaxWheelMMs)
|
||||
if err := c.adapter.DriveDirect(left, right); err != nil {
|
||||
return err
|
||||
}
|
||||
c.applyAutoSideBrush(left, right)
|
||||
return nil
|
||||
case msg.MotorPWM != nil:
|
||||
main := clamp(msg.MotorPWM.Main, -127, 127)
|
||||
side := clamp(msg.MotorPWM.Side, -127, 127)
|
||||
vac := clamp(msg.MotorPWM.Vacuum, 0, 127)
|
||||
c.lastAux = motorPWMPayload{Main: main, Side: side, Vacuum: vac}
|
||||
c.autoSideOn = false
|
||||
return c.adapter.MotorPWM(main, side, vac)
|
||||
case msg.SensorStream != nil:
|
||||
if msg.SensorStream.Enable {
|
||||
return c.adapter.StartSensorStream(defaultStreamPackets)
|
||||
}
|
||||
return nil
|
||||
case msg.Raw != "" && len(msg.Raw) > 0:
|
||||
buf, err := base64.StdEncoding.DecodeString(msg.Raw)
|
||||
if err != nil {
|
||||
return fmt.Errorf("raw decode: %w", err)
|
||||
}
|
||||
if err := c.adapter.SendRaw(buf); err != nil {
|
||||
return err
|
||||
}
|
||||
if len(buf) > 0 && isModeOpcode(buf[0]) {
|
||||
return c.ensureSensorStream()
|
||||
}
|
||||
return nil
|
||||
case msg.Servo != nil:
|
||||
if c.servo == nil {
|
||||
return fmt.Errorf("camera servo disabled")
|
||||
}
|
||||
return c.handleServoCommand(msg.Servo)
|
||||
case msg.TTS != nil:
|
||||
return c.enqueueTTS(msg.TTS)
|
||||
case msg.Horn != nil:
|
||||
if c.horn == nil {
|
||||
return fmt.Errorf("horn disabled")
|
||||
}
|
||||
return c.horn.HandlePayload(msg.Horn)
|
||||
case msg.AudioLevels != nil:
|
||||
return c.handleAudioLevels(msg.AudioLevels)
|
||||
case msg.Headlight != nil:
|
||||
return c.handleToggleCommand("headlight", c.headlight, msg.Headlight)
|
||||
case msg.Laser != nil:
|
||||
return c.handleToggleCommand("laser", c.laser, msg.Laser)
|
||||
case msg.Peripheral != nil:
|
||||
return c.peripherals.SetControl(msg.Peripheral.ID, msg.Peripheral.Control, msg.Peripheral.Value)
|
||||
case msg.Song != nil:
|
||||
slot := 0
|
||||
if msg.Song.Slot != nil {
|
||||
slot = clampInt(*msg.Song.Slot, 0, 4)
|
||||
}
|
||||
return c.adapter.PlaySong(slot, msg.Song.Notes)
|
||||
default:
|
||||
return fmt.Errorf("unsupported command type: %s", msg.Type)
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) handleToggleCommand(name string, toggle ToggleController, payload *togglePayload) error {
|
||||
if toggle == nil {
|
||||
return fmt.Errorf("%s disabled", name)
|
||||
}
|
||||
if err := toggle.HandleAction(payload.Action); err != nil {
|
||||
return err
|
||||
}
|
||||
// Event names and payload keys use logical device names. GPIO polarity has
|
||||
// already been handled inside GPIOToggle, so the server only sees whether
|
||||
// the headlight or laser should be considered on.
|
||||
c.emitEvent(fmt.Sprintf("%s.state", name), map[string]any{
|
||||
fmt.Sprintf("%sOn", name): toggle.On(),
|
||||
})
|
||||
return nil
|
||||
}
|
||||
|
||||
func (c *WSClient) stopMotionForSystemCommand(reason string) error {
|
||||
// System-level commands can restart the process or the whole Pi. Stopping
|
||||
// both wheel and auxiliary motors first leaves the Roomba in a predictable
|
||||
// state before roverd hands control to systemd or the update helper.
|
||||
if err := c.adapter.DriveDirect(0, 0); err != nil {
|
||||
return fmt.Errorf("stop drive before %s: %w", reason, err)
|
||||
}
|
||||
if err := c.adapter.MotorPWM(0, 0, 0); err != nil {
|
||||
return fmt.Errorf("stop aux motors before %s: %w", reason, err)
|
||||
}
|
||||
if err := c.adapter.StartOI(); err != nil {
|
||||
return fmt.Errorf("enter passive mode before %s: %w", reason, err)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (c *WSClient) applyAutoSideBrush(left, right int) {
|
||||
if c.cfg == nil || !c.cfg.AutoSideBrush.Enabled {
|
||||
if c.autoSideOn {
|
||||
c.autoSideOn = false
|
||||
if err := c.adapter.MotorPWM(c.lastAux.Main, c.lastAux.Side, c.lastAux.Vacuum); err != nil {
|
||||
c.log.Printf("auto side brush stop failed: %v", err)
|
||||
}
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
moving := left != 0 || right != 0
|
||||
if !moving {
|
||||
if c.autoSideOn {
|
||||
c.autoSideOn = false
|
||||
if err := c.adapter.MotorPWM(c.lastAux.Main, c.lastAux.Side, c.lastAux.Vacuum); err != nil {
|
||||
c.log.Printf("auto side brush stop failed: %v", err)
|
||||
}
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
if c.lastAux.Side != 0 {
|
||||
c.autoSideOn = false
|
||||
return
|
||||
}
|
||||
|
||||
autoSpeed := clampInt(c.cfg.AutoSideBrush.Speed, -127, 127)
|
||||
if autoSpeed == 0 {
|
||||
c.autoSideOn = false
|
||||
return
|
||||
}
|
||||
if c.autoSideOn {
|
||||
return
|
||||
}
|
||||
|
||||
if err := c.adapter.MotorPWM(c.lastAux.Main, autoSpeed, c.lastAux.Vacuum); err != nil {
|
||||
c.log.Printf("auto side brush start failed: %v", err)
|
||||
return
|
||||
}
|
||||
c.autoSideOn = true
|
||||
}
|
||||
|
||||
func (c *WSClient) enqueueTTS(payload *ttsPayload) error {
|
||||
if c.ttsQueue == nil {
|
||||
return fmt.Errorf("tts disabled")
|
||||
}
|
||||
select {
|
||||
case c.ttsQueue <- payload:
|
||||
return nil
|
||||
default:
|
||||
return fmt.Errorf("tts busy")
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) startTTSWorker(ctx context.Context) {
|
||||
if c.ttsQueue == nil {
|
||||
return
|
||||
}
|
||||
go func() {
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case payload := <-c.ttsQueue:
|
||||
if payload == nil {
|
||||
continue
|
||||
}
|
||||
if err := c.handleTTSPayload(ctx, payload); err != nil {
|
||||
c.log.Printf("tts failed: %v", err)
|
||||
c.emitEvent("tts.error", map[string]any{"error": err.Error()})
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (c *WSClient) warmChromeTTS(ctx context.Context) {
|
||||
if c.chromeTTS == nil {
|
||||
return
|
||||
}
|
||||
go func() {
|
||||
if err := c.chromeTTS.Start(ctx); err != nil {
|
||||
c.log.Printf("chromegtts warmup failed: %v", err)
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (c *WSClient) handleServoCommand(payload *servoPayload) error {
|
||||
switch {
|
||||
case payload.Angle != nil:
|
||||
return c.servo.SetAngle(*payload.Angle)
|
||||
case payload.Nudge != nil:
|
||||
return c.servo.Nudge(*payload.Nudge)
|
||||
case payload.PulseUs != nil:
|
||||
return c.servo.SetPulseWidth(*payload.PulseUs)
|
||||
default:
|
||||
return fmt.Errorf("servo command requires angle, nudge, or pulseUs")
|
||||
}
|
||||
}
|
||||
|
||||
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:
|
||||
// A real sensor frame is the authoritative end of a watchdog
|
||||
// episode. Successfully sending the OI restart commands alone does
|
||||
// not prove that the Roomba resumed producing sensor data.
|
||||
c.closeSensorWatchdogEpisode()
|
||||
lastFrame = time.Now()
|
||||
resetTimer()
|
||||
msg := sensorMessage{
|
||||
Type: "sensor",
|
||||
Timestamp: time.Now().UnixMilli(),
|
||||
Data: base64.StdEncoding.EncodeToString(frame),
|
||||
}
|
||||
if err := writeJSON(ctx, conn, msg); err != nil {
|
||||
c.log.Printf("sensor send failed: %v", err)
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) forwardEvents(ctx context.Context, conn *websocket.Conn) {
|
||||
if c.events == nil {
|
||||
return
|
||||
}
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case evt := <-c.controlEvents:
|
||||
if evt.Type == "" {
|
||||
evt.Type = "event"
|
||||
}
|
||||
if err := writeJSON(ctx, conn, evt); err != nil {
|
||||
c.log.Printf("event send failed: %v", err)
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) emitEvent(event string, data map[string]any) {
|
||||
events := c.events
|
||||
if event == "headlight.state" || event == "laser.state" {
|
||||
events = c.controlEvents
|
||||
}
|
||||
if events == nil {
|
||||
return
|
||||
}
|
||||
select {
|
||||
case events <- RoverEvent{
|
||||
Type: "event",
|
||||
Event: event,
|
||||
Ts: time.Now().UnixMilli(),
|
||||
Data: data,
|
||||
}:
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
func writeJSON(ctx context.Context, conn *websocket.Conn, v any) error {
|
||||
data, err := json.Marshal(v)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
return conn.Write(ctx, websocket.MessageText, data)
|
||||
}
|
||||
|
||||
func clamp(value, min, max int) int {
|
||||
if value < min {
|
||||
return min
|
||||
}
|
||||
if value > max {
|
||||
return max
|
||||
}
|
||||
return value
|
||||
}
|
||||
|
||||
func (c *WSClient) ensureSensorStream() error {
|
||||
if err := c.adapter.StartSensorStream(defaultStreamPackets); err != nil {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
const disconnectSeekDelay = time.Minute
|
||||
const disconnectRebootDelay = 6 * time.Minute
|
||||
const dialTimeout = 10 * time.Second
|
||||
const pingInterval = 15 * time.Second
|
||||
const pingTimeout = 5 * time.Second
|
||||
|
||||
func keepalive(ctx context.Context, conn *websocket.Conn) error {
|
||||
ticker := time.NewTicker(pingInterval)
|
||||
defer ticker.Stop()
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return ctx.Err()
|
||||
case <-ticker.C:
|
||||
pingCtx, cancel := context.WithTimeout(ctx, pingTimeout)
|
||||
err := conn.Ping(pingCtx)
|
||||
cancel()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) markConnected() {
|
||||
c.connMu.Lock()
|
||||
wasConnected := c.connected
|
||||
c.connected = true
|
||||
c.seekIssued = false
|
||||
c.rebootIssued = false
|
||||
if c.disconnectT != nil {
|
||||
c.disconnectT.Stop()
|
||||
c.disconnectT = nil
|
||||
}
|
||||
if c.rebootT != nil {
|
||||
c.rebootT.Stop()
|
||||
c.rebootT = nil
|
||||
}
|
||||
c.connMu.Unlock()
|
||||
|
||||
// Only print on a state transition. Run is retried indefinitely, and a
|
||||
// message on every successful internal operation would quickly bury the
|
||||
// useful lifecycle history at the login prompt.
|
||||
if !wasConnected {
|
||||
c.console.Notify("Control server connected.")
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) markDisconnected() {
|
||||
if c.stopping.Load() {
|
||||
return
|
||||
}
|
||||
c.connMu.Lock()
|
||||
wasConnected := c.connected
|
||||
c.connected = false
|
||||
if c.disconnectT == nil {
|
||||
c.disconnectT = time.AfterFunc(disconnectSeekDelay, c.handleDisconnectTimeout)
|
||||
}
|
||||
if c.rebootT == nil {
|
||||
c.rebootT = time.AfterFunc(disconnectRebootDelay, c.handleRebootTimeout)
|
||||
}
|
||||
c.connMu.Unlock()
|
||||
|
||||
// Initial dial failures are already represented by the startup message and
|
||||
// journal retry logs. The prominent disconnect alert is reserved for losing
|
||||
// a connection that was actually established.
|
||||
if wasConnected {
|
||||
c.console.Notify("Control server connection lost. Automatic dock seek in 1 minute; rover reboot in 6 minutes if the connection is not restored.")
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) handleDisconnectTimeout() {
|
||||
c.commandMu.Lock()
|
||||
defer c.commandMu.Unlock()
|
||||
if c.stopping.Load() {
|
||||
return
|
||||
}
|
||||
c.connMu.Lock()
|
||||
if c.connected || c.seekIssued {
|
||||
c.connMu.Unlock()
|
||||
return
|
||||
}
|
||||
c.seekIssued = true
|
||||
c.connMu.Unlock()
|
||||
|
||||
c.console.Notify("Control server has been disconnected for 1 minute. Seeking the dock now.")
|
||||
if err := c.adapter.SeekDock(); err != nil {
|
||||
c.log.Printf("seek dock on disconnect failed: %v", err)
|
||||
return
|
||||
}
|
||||
c.log.Printf("seek dock issued after websocket disconnect")
|
||||
}
|
||||
|
||||
func (c *WSClient) handleRebootTimeout() {
|
||||
if c.stopping.Load() {
|
||||
return
|
||||
}
|
||||
c.connMu.Lock()
|
||||
if c.connected || c.rebootIssued {
|
||||
c.connMu.Unlock()
|
||||
return
|
||||
}
|
||||
c.rebootIssued = true
|
||||
c.connMu.Unlock()
|
||||
|
||||
c.console.Notify("Control server has been disconnected for 6 minutes. Rebooting the rover now.")
|
||||
c.log.Printf("rebooting pi after prolonged websocket disconnect")
|
||||
cmd := exec.Command("systemctl", "reboot")
|
||||
if err := cmd.Start(); err != nil {
|
||||
c.log.Printf("reboot command failed: %v", err)
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) recoverSensorStream(idleFor time.Duration, cmdPause time.Duration) {
|
||||
c.commandMu.Lock()
|
||||
defer c.commandMu.Unlock()
|
||||
if c.stopping.Load() {
|
||||
return
|
||||
}
|
||||
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 c.openSensorWatchdogEpisode() {
|
||||
c.console.Notify(fmt.Sprintf("Sensor watchdog is restarting the Roomba sensor stream after %.1f seconds without data.", idleFor.Seconds()))
|
||||
}
|
||||
|
||||
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()})
|
||||
// Unlike the restart notice, every concrete command failure is useful
|
||||
// diagnostic information and may change between recovery attempts.
|
||||
c.console.Notify(fmt.Sprintf("Sensor watchdog recovery failed while starting the Roomba OI: %v", err))
|
||||
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()})
|
||||
c.console.Notify(fmt.Sprintf("Sensor watchdog recovery failed while starting the sensor stream: %v", err))
|
||||
return
|
||||
}
|
||||
|
||||
c.emitEvent("sensorWatchdog.ok", map[string]any{
|
||||
"idleMs": idleFor.Milliseconds(),
|
||||
})
|
||||
if c.markSensorWatchdogCommandsOK() {
|
||||
// Match the existing sensorWatchdog.ok contract precisely: this says
|
||||
// the recovery commands succeeded, not that a new frame has arrived.
|
||||
c.console.Notify("Sensor watchdog successfully sent the sensor-stream restart commands.")
|
||||
}
|
||||
}
|
||||
|
||||
// openSensorWatchdogEpisode reports whether this is the first recovery attempt
|
||||
// since sensor frames stopped. The watchdog can retry every few seconds, so
|
||||
// tracking the outage as one episode keeps the login console readable.
|
||||
func (c *WSClient) openSensorWatchdogEpisode() bool {
|
||||
c.watchdogMu.Lock()
|
||||
defer c.watchdogMu.Unlock()
|
||||
|
||||
if c.watchdogOpen {
|
||||
return false
|
||||
}
|
||||
c.watchdogOpen = true
|
||||
c.watchdogOK = false
|
||||
return true
|
||||
}
|
||||
|
||||
// markSensorWatchdogCommandsOK suppresses duplicate success notices while the
|
||||
// rover is still waiting for a real frame to close the current outage.
|
||||
func (c *WSClient) markSensorWatchdogCommandsOK() bool {
|
||||
c.watchdogMu.Lock()
|
||||
defer c.watchdogMu.Unlock()
|
||||
|
||||
if c.watchdogOK {
|
||||
return false
|
||||
}
|
||||
c.watchdogOK = true
|
||||
return true
|
||||
}
|
||||
|
||||
func (c *WSClient) closeSensorWatchdogEpisode() {
|
||||
c.watchdogMu.Lock()
|
||||
c.watchdogOpen = false
|
||||
c.watchdogOK = false
|
||||
c.watchdogMu.Unlock()
|
||||
}
|
||||
|
||||
func isModeOpcode(op byte) bool {
|
||||
switch op {
|
||||
case 128, 131, 132:
|
||||
return true
|
||||
default:
|
||||
return false
|
||||
}
|
||||
}
|
||||
|
||||
// PrepareShutdown blocks new control commands before stopping the hardware.
|
||||
func (c *WSClient) PrepareShutdown() error {
|
||||
c.commandMu.Lock()
|
||||
defer c.commandMu.Unlock()
|
||||
c.stopping.Store(true)
|
||||
if c.autoCharge != nil {
|
||||
c.autoCharge.SetPaused(true)
|
||||
}
|
||||
c.connMu.Lock()
|
||||
if c.disconnectT != nil {
|
||||
c.disconnectT.Stop()
|
||||
}
|
||||
if c.rebootT != nil {
|
||||
c.rebootT.Stop()
|
||||
}
|
||||
c.connMu.Unlock()
|
||||
if c.horn != nil {
|
||||
c.horn.HandlePayload(&hornPayload{Action: "stop"})
|
||||
}
|
||||
return c.stopMotionForSystemCommand("management action")
|
||||
}
|
||||
|
||||
func (c *WSClient) ResumeControl() {
|
||||
c.commandMu.Lock()
|
||||
defer c.commandMu.Unlock()
|
||||
if c.autoCharge != nil {
|
||||
c.autoCharge.SetPaused(false)
|
||||
}
|
||||
c.stopping.Store(false)
|
||||
// A rejected management action must restore disconnect safety timers too.
|
||||
c.connMu.Lock()
|
||||
connected := c.connected
|
||||
c.connMu.Unlock()
|
||||
if !connected {
|
||||
c.markDisconnected()
|
||||
}
|
||||
}
|
||||
func (c *WSClient) SetAutoCharge(controller *AutoChargeController) { c.autoCharge = controller }
|
||||
@@ -0,0 +1,27 @@
|
||||
package roverd
|
||||
|
||||
import "testing"
|
||||
|
||||
func TestSensorWatchdogConsoleEpisodeSuppressesDuplicateStatusMessages(t *testing.T) {
|
||||
client := &WSClient{}
|
||||
|
||||
if !client.openSensorWatchdogEpisode() {
|
||||
t.Fatal("first recovery attempt should announce the watchdog episode")
|
||||
}
|
||||
if client.openSensorWatchdogEpisode() {
|
||||
t.Fatal("repeated recovery attempt should not repeat the outage announcement")
|
||||
}
|
||||
if !client.markSensorWatchdogCommandsOK() {
|
||||
t.Fatal("first successful command restart should be announced")
|
||||
}
|
||||
if client.markSensorWatchdogCommandsOK() {
|
||||
t.Fatal("repeated successful command restart should not be announced")
|
||||
}
|
||||
|
||||
// Receiving a real frame closes the outage. A later silence is a distinct
|
||||
// incident and must therefore be visible on the console again.
|
||||
client.closeSensorWatchdogEpisode()
|
||||
if !client.openSensorWatchdogEpisode() {
|
||||
t.Fatal("new outage after a sensor frame should be announced")
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user