testings are going goods

This commit is contained in:
legop3
2026-09-08 21:10:04 -04:00
parent f654394636
commit 3859806fca
30 changed files with 3893 additions and 113 deletions
+6 -4
View File
@@ -25,10 +25,6 @@ type CameraServo struct {
closed bool
}
const maxServoDegPerSec = 60.0
const servoStepInterval = 20 * time.Millisecond
const servoAngleEpsilon = 0.01
func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, error) {
if !cfg.Enabled {
return nil, fmt.Errorf("camera servo disabled")
@@ -132,6 +128,12 @@ func (s *CameraServo) CurrentAngle() float64 {
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) applyPulseLocked(micros int) {
micros = clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs)
s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen))
+7 -4
View File
@@ -11,10 +11,9 @@ type CameraServo struct{}
func NewCameraServo(_ CameraServoConfig, _ *log.Logger) (*CameraServo, error) {
/*
The Debian laptop profile starts with the laptop's built-in webcam and no
Pi PWM servo. If a laptop rover eventually grows an external servo board,
it should get its own implementation instead of reusing Raspberry Pi GPIO
assumptions.
This constructor represents only native host GPIO. The shared startup
resolver selects the normal Firmata implementation when an ESP32 provides
the role, so external hardware is not laptop-specific code.
*/
return nil, fmt.Errorf("camera servo not supported in the debian-laptop build")
}
@@ -36,3 +35,7 @@ func (c *CameraServo) SetPulseWidth(micros int) error {
func (c *CameraServo) CurrentAngle() float64 {
return 0
}
func (c *CameraServo) Configuration() CameraServoConfig {
return CameraServoConfig{}
}
+4
View File
@@ -30,3 +30,7 @@ func (c *CameraServo) SetPulseWidth(micros int) error {
func (c *CameraServo) CurrentAngle() float64 {
return 0
}
func (c *CameraServo) Configuration() CameraServoConfig {
return CameraServoConfig{}
}
+151
View File
@@ -0,0 +1,151 @@
package main
import (
"context"
"encoding/json"
"errors"
"flag"
"fmt"
"log"
"os"
"time"
roverd "multiroombarover/pi/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)
}
}
+16 -26
View File
@@ -37,6 +37,15 @@ func main() {
}
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 {
logger.Fatalf("discover rover peripherals: %v", err)
}
defer peripherals.Close()
var pulser *roverd.BRCPulser
if cfg.BRC.Enabled() {
pulser, err = roverd.NewBRCPulser(cfg.BRC, logger)
@@ -61,37 +70,18 @@ func main() {
mediaSupervisor.Start(ctx)
}
var cameraServo *roverd.CameraServo
if cfg.CameraServo.Enabled {
cameraServo, err = roverd.NewCameraServo(cfg.CameraServo, logger)
if err != nil {
logger.Fatalf("init camera servo: %v", err)
}
defer cameraServo.Close()
}
var headlight *roverd.GPIOToggle
if cfg.Headlight.Enabled {
headlight, err = roverd.NewGPIOToggle("headlight", cfg.Headlight, logger)
if err != nil {
logger.Fatalf("init headlight: %v", err)
}
defer headlight.Close()
}
var laser *roverd.GPIOToggle
if cfg.Laser.Enabled {
laser, err = roverd.NewGPIOToggle("laser", cfg.Laser, logger)
if err != nil {
logger.Fatalf("init laser: %v", err)
}
defer laser.Close()
// Backend selection is identical on Pi and laptop hosts: enabled native
// GPIO wins, otherwise a discovered ESP32 may provide the built-in role.
hardwareControllers, err := roverd.ResolveRoverHardwareControllers(cfg, peripherals, logger)
if err != nil {
logger.Fatalf("resolve rover hardware controllers: %v", err)
}
defer hardwareControllers.Close()
autoCharge := roverd.NewAutoChargeController(adapter, eventStream, logger)
go autoCharge.Run(ctx, sensorSamples)
client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, cameraServo, headlight, laser, logger, console)
client := roverd.NewWSClient(cfg, adapter, sensorFrames, eventStream, mediaSupervisor, hardwareControllers.CameraServo, hardwareControllers.Headlight, hardwareControllers.Laser, peripherals, logger, console)
// Startup is announced only after every configured hardware dependency has
// initialized successfully. A message here therefore means the control loop
+23 -13
View File
@@ -1,19 +1,22 @@
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 MediaConfig `json:"media"`
CameraServo CameraServoConfig `json:"cameraServo"`
Audio AudioConfig `json:"audio"`
Horn HornConfig `json:"horn"`
Headlight GPIOToggleConfig `json:"headlight"`
Laser GPIOToggleConfig `json:"laser"`
Private PrivateConfig `json:"private"`
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 MediaConfig `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 {
@@ -45,6 +48,7 @@ type inboundMessage struct {
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"`
Reboot *rebootPayload `json:"reboot,omitempty"`
// Update is intentionally just a marker payload. The server can request the
@@ -103,6 +107,12 @@ 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"`
+628
View File
@@ -0,0 +1,628 @@
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
}
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) {
client.stateMu.Lock()
if client.terminalErr == nil {
client.terminalErr = err
}
client.stateMu.Unlock()
select {
case client.errors <- err:
case <-ctx.Done():
default:
}
}
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 {
return err
}
if written != len(message) {
return fmt.Errorf("short Firmata write %d/%d", written, len(message))
}
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)
}
+286
View File
@@ -0,0 +1,286 @@
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
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),
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) 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
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), 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) 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,153 @@
package roverd
import (
"bytes"
"context"
"log"
"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()
// All native entries are disabled, exactly as they can be on either a Pi or
// laptop rover. The shared resolver must therefore select every ESP32 role.
baseline := len(connection.Bytes())
controllers, err := ResolveRoverHardwareControllers(&Config{}, 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")
}
// 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 TestEnabledNativeRolesWinEvenWithSeveralFirmataProviders(t *testing.T) {
roleDescription := PeripheralDescription{RoverControls: PeripheralRoverControls{
CameraServo: &PeripheralCameraServo{},
Headlight: &PeripheralDigitalRole{},
Laser: &PeripheralDigitalRole{},
}}
manager := &PeripheralManager{
byID: make(map[string]*managedPeripheral),
peripherals: []*managedPeripheral{
{metadata: RoverPeripheralMetadata{ID: "firmata-0"}, description: roleDescription},
{metadata: RoverPeripheralMetadata{ID: "firmata-1"}, description: roleDescription},
},
}
cfg := &Config{
CameraServo: CameraServoConfig{Enabled: true},
Headlight: GPIOToggleConfig{Enabled: true},
Laser: GPIOToggleConfig{Enabled: true},
}
nativeCamera := &testCameraServoController{cfg: cfg.CameraServo}
nativeToggles := map[string]*testToggleController{}
factories := nativeHardwareControllerFactories{
newCameraServo: func(_ CameraServoConfig, _ *log.Logger) (CameraServoController, error) {
return nativeCamera, nil
},
newToggle: func(name string, config GPIOToggleConfig, _ *log.Logger) (ToggleController, error) {
controller := &testToggleController{cfg: config}
nativeToggles[name] = controller
return controller, nil
},
}
// Duplicate Firmata declarations are irrelevant when native hardware wins;
// selection must neither fail nor initialize either ESP32 provider.
controllers, err := resolveRoverHardwareControllers(cfg, manager, discardLogger(), factories)
if err != nil {
t.Fatalf("resolve native precedence: %v", err)
}
if controllers.CameraServo != nativeCamera || controllers.Headlight != nativeToggles["headlight"] || controllers.Laser != nativeToggles["laser"] {
t.Fatal("resolver did not retain native controllers")
}
}
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) 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) Close() {}
+438
View File
@@ -0,0 +1,438 @@
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
}
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.recordingConnection.Close()
close(connection.reads)
return nil
}
+5
View File
@@ -87,6 +87,11 @@ func (g *GPIOToggle) On() bool {
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) 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
+8 -4
View File
@@ -13,10 +13,10 @@ type GPIOToggle struct {
func NewGPIOToggle(name string, _ GPIOToggleConfig, _ *log.Logger) (*GPIOToggle, error) {
/*
A Debian laptop has no Raspberry Pi GPIO character-device contract for
headlights or lasers. Returning an error when enabled makes bad laptop
configs fail during startup instead of advertising controls that cannot
change any hardware.
A Debian laptop has no native Raspberry Pi GPIO contract. Returning an
error here catches an invalid native configuration; the shared resolver
selects an ESP32 Firmata toggle before this constructor when native GPIO
is disabled.
*/
return nil, fmt.Errorf("%s not supported in the debian-laptop build", name)
}
@@ -30,3 +30,7 @@ func (g *GPIOToggle) HandleAction(action string) error {
func (g *GPIOToggle) On() bool {
return false
}
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
return GPIOToggleConfig{}
}
+4
View File
@@ -24,3 +24,7 @@ func (g *GPIOToggle) HandleAction(action string) error {
func (g *GPIOToggle) On() bool {
return false
}
func (g *GPIOToggle) Configuration() GPIOToggleConfig {
return GPIOToggleConfig{}
}
+122
View File
@@ -0,0 +1,122 @@
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
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
Close()
}
// RoverHardwareControllers is the result of the single startup-time backend
// decision. Its effective configurations are derived from whichever backend
// won, making the normal rover hello accurate on both Pi and laptop hosts.
type RoverHardwareControllers struct {
CameraServo CameraServoController
Headlight ToggleController
Laser ToggleController
}
type nativeHardwareControllerFactories struct {
newCameraServo func(CameraServoConfig, *log.Logger) (CameraServoController, error)
newToggle func(string, GPIOToggleConfig, *log.Logger) (ToggleController, error)
}
// ResolveRoverHardwareControllers applies one rule on every real rover build:
// enabled native GPIO wins, otherwise one discovered ESP32 may fill the role.
// The rule is intentionally not selected by GOARCH or the debian_laptop tag.
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
var err error
controllers.CameraServo, err = resolveCameraServoController(cfg.CameraServo, peripherals, logger, factories.newCameraServo)
if err != nil {
return RoverHardwareControllers{}, fmt.Errorf("init camera servo: %w", err)
}
controllers.Headlight, err = resolveToggleController("headlight", cfg.Headlight, peripherals, logger, factories.newToggle)
if err != nil {
controllers.Close()
return RoverHardwareControllers{}, fmt.Errorf("init headlight: %w", err)
}
controllers.Laser, err = resolveToggleController("laser", cfg.Laser, peripherals, logger, factories.newToggle)
if err != nil {
controllers.Close()
return RoverHardwareControllers{}, fmt.Errorf("init laser: %w", err)
}
return controllers, nil
}
func resolveCameraServoController(nativeConfig CameraServoConfig, peripherals *PeripheralManager, logger *log.Logger, newNative func(CameraServoConfig, *log.Logger) (CameraServoController, error)) (CameraServoController, error) {
if nativeConfig.Enabled {
if peripherals.HasRoverRole("cameraServo") {
logger.Printf("ignoring ESP32 cameraServo because native camera servo is enabled")
}
return newNative(nativeConfig, logger)
}
return peripherals.NewFirmataCameraServo(logger)
}
func resolveToggleController(name string, nativeConfig GPIOToggleConfig, peripherals *PeripheralManager, logger *log.Logger, newNative func(string, GPIOToggleConfig, *log.Logger) (ToggleController, error)) (ToggleController, error) {
if nativeConfig.Enabled {
if peripherals.HasRoverRole(name) {
logger.Printf("ignoring ESP32 %s because native %s is enabled", name, name)
}
return newNative(name, nativeConfig, logger)
}
return peripherals.NewFirmataToggle(name, logger)
}
// 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()
}
}
+57
View File
@@ -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)
}
}
+24
View File
@@ -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)
}
+38
View File
@@ -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)
}
+534
View File
@@ -0,0 +1,534 @@
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 {
mu sync.RWMutex
peripherals []*managedPeripheral
byID map[string]*managedPeripheral
cancel context.CancelFunc
closeOnce sync.Once
logger *log.Logger
}
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,
}
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
logger.Printf("discovered rover peripheral %s on %s with %d generic controls", description.Name, devicePath, len(description.Controls))
}
return manager, nil
}
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)
}
// HasRoverRole reports whether discovery found an ESP32 implementation of one
// established rover control. It is used only for startup selection and logging;
// commands continue to target the selected controller interface directly.
func (manager *PeripheralManager) HasRoverRole(role string) bool {
return len(manager.roverRoleProviders(role)) > 0
}
func (manager *PeripheralManager) roverRoleProviders(role string) []*managedPeripheral {
if manager == nil {
return nil
}
manager.mu.RLock()
defer manager.mu.RUnlock()
var providers []*managedPeripheral
for _, peripheral := range manager.peripherals {
switch role {
case "cameraServo":
if peripheral.description.RoverControls.CameraServo != nil {
providers = append(providers, peripheral)
}
case "headlight":
if peripheral.description.RoverControls.Headlight != nil {
providers = append(providers, peripheral)
}
case "laser":
if peripheral.description.RoverControls.Laser != nil {
providers = append(providers, peripheral)
}
}
}
return providers
}
// NewFirmataCameraServo constructs the shared camera controller only when a
// discovered peripheral declared that standardized role. Absence is a normal
// disabled-feature result rather than an error.
func (manager *PeripheralManager) NewFirmataCameraServo(logger *log.Logger) (CameraServoController, error) {
providers := manager.roverRoleProviders("cameraServo")
if len(providers) == 0 {
return nil, nil
}
if len(providers) > 1 {
return nil, duplicateRoverRoleError("cameraServo", providers)
}
peripheral := providers[0]
return newFirmataCameraServo(peripheral, *peripheral.description.RoverControls.CameraServo, logger)
}
// NewFirmataToggle resolves either standardized digital role without exposing
// the peripheral connection or ESP32 pin to WSClient.
func (manager *PeripheralManager) NewFirmataToggle(role string, logger *log.Logger) (ToggleController, error) {
providers := manager.roverRoleProviders(role)
if len(providers) == 0 {
return nil, nil
}
if len(providers) > 1 {
return nil, duplicateRoverRoleError(role, providers)
}
peripheral := providers[0]
var declaration *PeripheralDigitalRole
switch role {
case "headlight":
declaration = peripheral.description.RoverControls.Headlight
case "laser":
declaration = peripheral.description.RoverControls.Laser
default:
return nil, fmt.Errorf("unknown Firmata toggle role %q", role)
}
return newFirmataToggle(role, peripheral, *declaration, logger)
}
func duplicateRoverRoleError(role string, providers []*managedPeripheral) error {
providerIDs := make([]string, 0, len(providers))
for _, provider := range providers {
providerIDs = append(providerIDs, provider.metadata.ID)
}
return fmt.Errorf("rover peripheral role %s has multiple providers: %s", role, strings.Join(providerIDs, ", "))
}
// 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)
}
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() {
manager.cancel()
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)
}
}
})
}
+374
View File
@@ -0,0 +1,374 @@
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])
}
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 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.NewFirmataToggle("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)
}
}
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)
}
+27 -8
View File
@@ -19,10 +19,11 @@ type WSClient struct {
sensorFrames <-chan []byte
events chan RoverEvent
media *MediaSupervisor
servo *CameraServo
servo CameraServoController
horn *HornSynth
headlight *GPIOToggle
laser *GPIOToggle
headlight ToggleController
laser ToggleController
peripherals *PeripheralManager
log *log.Logger
console *ConsoleNotifier
recoverMu sync.Mutex
@@ -45,7 +46,7 @@ type WSClient struct {
audioMu sync.RWMutex
}
func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, events chan RoverEvent, media *MediaSupervisor, servo *CameraServo, headlight *GPIOToggle, laser *GPIOToggle, logger *log.Logger, console *ConsoleNotifier) *WSClient {
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)
@@ -68,6 +69,7 @@ func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, even
horn: horn,
headlight: headlight,
laser: laser,
peripherals: peripherals,
log: logger,
console: console,
ttsQueue: ttsQueue,
@@ -129,6 +131,20 @@ func (c *WSClient) Run(ctx context.Context) error {
}
func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
// Built-in metadata comes from the selected controller, not necessarily
// YAML. 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,
@@ -137,11 +153,12 @@ func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
Battery: c.cfg.Battery,
MaxWheelSpeed: c.cfg.MaxWheelMMs,
Media: c.cfg.Media,
CameraServo: c.cfg.CameraServo,
CameraServo: cameraServoConfig,
Audio: c.cfg.Audio,
Horn: c.cfg.Horn,
Headlight: c.cfg.Headlight,
Laser: c.cfg.Laser,
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)
@@ -238,6 +255,8 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
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 {
@@ -253,7 +272,7 @@ func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
}
}
func (c *WSClient) handleToggleCommand(name string, toggle *GPIOToggle, payload *togglePayload) error {
func (c *WSClient) handleToggleCommand(name string, toggle ToggleController, payload *togglePayload) error {
if toggle == nil {
return fmt.Errorf("%s disabled", name)
}