diff --git a/dist/roverd b/dist/roverd index ac7b0fc0..9a244bec 100755 Binary files a/dist/roverd and b/dist/roverd differ diff --git a/dist/servoverifier b/dist/servoverifier index 95979933..aee99dbf 100755 Binary files a/dist/servoverifier and b/dist/servoverifier differ diff --git a/pi/roverd/camera_servo.go b/pi/roverd/camera_servo.go index 9cc6896c..b0adccff 100644 --- a/pi/roverd/camera_servo.go +++ b/pi/roverd/camera_servo.go @@ -15,6 +15,8 @@ type CameraServo struct { cfg CameraServoConfig logger *log.Logger pin rpio.Pin + pigpio *pigpioClient + usePigpio bool mu sync.Mutex currentAngle float64 closed bool @@ -24,6 +26,30 @@ func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, er if !cfg.Enabled { return nil, fmt.Errorf("camera servo disabled") } + + servo := &CameraServo{ + cfg: cfg, + logger: logger, + } + + if client, err := newPigpioClient(defaultPigpioAddr); err == nil { + if err := setPigpioMode(client, cfg.Pin, piOutput); err == nil { + servo.pigpio = client + servo.usePigpio = true + } else { + _ = client.Close() + } + } + + if servo.usePigpio { + if err := servo.setAngleLocked(cfg.HomeAngle); err != nil { + _ = servo.pigpio.Close() + return nil, err + } + logger.Printf("camera servo initialized on GPIO %d (pigpio, %.1f..%.1f deg, %d..%d us, invert=%v)", cfg.Pin, cfg.MinAngle, cfg.MaxAngle, cfg.MinPulseUs, cfg.MaxPulseUs, cfg.Invert) + return servo, nil + } + if err := rpio.Open(); err != nil { return nil, fmt.Errorf("open gpio: %w", err) } @@ -32,12 +58,8 @@ func NewCameraServo(cfg CameraServoConfig, logger *log.Logger) (*CameraServo, er pin.Mode(rpio.Pwm) targetClock := cfg.FreqHz * cfg.CycleLen pin.Freq(targetClock) + servo.pin = pin - servo := &CameraServo{ - cfg: cfg, - logger: logger, - pin: pin, - } if err := servo.setAngleLocked(cfg.HomeAngle); err != nil { rpio.Close() return nil, err @@ -52,8 +74,12 @@ func (s *CameraServo) Close() { if s.closed { return } - s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle)) - rpio.Close() + _ = s.applyPulseLocked(s.angleToPulse(s.cfg.HomeAngle)) + if s.usePigpio { + _ = s.pigpio.Close() + } else { + rpio.Close() + } s.closed = true } @@ -68,7 +94,9 @@ func (s *CameraServo) setAngleLocked(angle float64) error { return fmt.Errorf("servo closed") } clamped := clampFloat(angle, s.cfg.MinAngle, s.cfg.MaxAngle) - s.applyPulseLocked(s.angleToPulse(clamped)) + if err := s.applyPulseLocked(s.angleToPulse(clamped)); err != nil { + return err + } s.currentAngle = clamped return nil } @@ -95,7 +123,9 @@ func (s *CameraServo) SetPulseWidth(micros int) error { if micros <= 0 { return fmt.Errorf("pulse width must be > 0") } - s.applyPulseLocked(micros) + if err := s.applyPulseLocked(micros); err != nil { + return err + } s.currentAngle = s.pulseToAngle(micros) return nil } @@ -106,9 +136,13 @@ func (s *CameraServo) CurrentAngle() float64 { return s.currentAngle } -func (s *CameraServo) applyPulseLocked(micros int) { +func (s *CameraServo) applyPulseLocked(micros int) error { micros = clampInt(micros, s.cfg.MinPulseUs, s.cfg.MaxPulseUs) + if s.usePigpio { + return setPigpioServo(s.pigpio, s.cfg.Pin, micros) + } s.pin.DutyCycle(uint32(micros), uint32(s.cfg.CycleLen)) + return nil } func (s *CameraServo) angleToPulse(angle float64) int { diff --git a/pi/roverd/ir_tx.go b/pi/roverd/ir_tx.go index cfa953d2..1e6490c1 100644 --- a/pi/roverd/ir_tx.go +++ b/pi/roverd/ir_tx.go @@ -6,88 +6,17 @@ import ( "bytes" "encoding/binary" "fmt" - "io" "log" - "net" "sync" "time" ) -const ( - piCmdSetMode = 0 - piCmdWrite = 4 - piCmdWaveClear = 27 - piCmdWaveAddGeneric = 28 - piCmdWaveTxBusy = 32 - piCmdWaveCreate = 49 - piCmdWaveDelete = 50 - piCmdWaveTxSend = 51 - piOutput = 1 - pigpioConnectRetries = 20 - pigpioConnectDelay = 250 * time.Millisecond -) - -type pigpioCmd struct { - Cmd uint32 - P1 uint32 - P2 uint32 - P3 uint32 -} - type gpioPulse struct { GpioOn uint32 GpioOff uint32 DelayUs uint32 } -type pigpioClient struct { - conn net.Conn - mu sync.Mutex -} - -func newPigpioClient(addr string) (*pigpioClient, error) { - conn, err := net.Dial("tcp", addr) - if err != nil { - return nil, err - } - if tcpConn, ok := conn.(*net.TCPConn); ok { - _ = tcpConn.SetNoDelay(true) - } - return &pigpioClient{conn: conn}, nil -} - -func (c *pigpioClient) Close() error { - if c.conn == nil { - return nil - } - return c.conn.Close() -} - -func (c *pigpioClient) command(cmd, p1, p2, p3 uint32, ext []byte) (int32, error) { - c.mu.Lock() - defer c.mu.Unlock() - - var buf [16]byte - binary.LittleEndian.PutUint32(buf[0:], cmd) - binary.LittleEndian.PutUint32(buf[4:], p1) - binary.LittleEndian.PutUint32(buf[8:], p2) - binary.LittleEndian.PutUint32(buf[12:], p3) - - if _, err := c.conn.Write(buf[:]); err != nil { - return -1, err - } - if len(ext) > 0 { - if _, err := c.conn.Write(ext); err != nil { - return -1, err - } - } - if _, err := io.ReadFull(c.conn, buf[:]); err != nil { - return -1, err - } - res := int32(binary.LittleEndian.Uint32(buf[12:])) - return res, nil -} - type IRTransmitter struct { cfg IRConfig logger *log.Logger @@ -103,10 +32,7 @@ func NewIRTransmitter(cfg IRConfig, logger *log.Logger) (*IRTransmitter, error) return nil, fmt.Errorf("ir disabled") } - addr := cfg.PigpioAddr - if addr == "" { - addr = "localhost:8888" - } + addr := ensurePigpioAddr(cfg.PigpioAddr) client, err := connectPigpioWithRetry(addr, logger) if err != nil { return nil, fmt.Errorf("connect pigpio: %w", err) @@ -129,22 +55,6 @@ func NewIRTransmitter(cfg IRConfig, logger *log.Logger) (*IRTransmitter, error) return tx, nil } -func connectPigpioWithRetry(addr string, logger *log.Logger) (*pigpioClient, error) { - var lastErr error - for attempt := 1; attempt <= pigpioConnectRetries; attempt++ { - client, err := newPigpioClient(addr) - if err == nil { - return client, nil - } - lastErr = err - if attempt == 1 || attempt%4 == 0 { - logger.Printf("pigpio connect attempt %d/%d failed: %v", attempt, pigpioConnectRetries, err) - } - time.Sleep(pigpioConnectDelay) - } - return nil, lastErr -} - func (t *IRTransmitter) Close() { t.mu.Lock() defer t.mu.Unlock() @@ -169,17 +79,12 @@ func (t *IRTransmitter) Send(code byte, repeat int) error { if len(pulses) == 0 { return nil } - if err := t.writeWave(pulses, time.Duration(totalUs)*time.Microsecond); err != nil { - return err - } - return nil + return t.writeWave(pulses, time.Duration(totalUs)*time.Microsecond) } func (t *IRTransmitter) configureLine() error { - if res, err := t.pigpio.command(piCmdSetMode, uint32(t.cfg.Pin), piOutput, 0, nil); err != nil { - return fmt.Errorf("pigpio set mode: %w", err) - } else if res < 0 { - return fmt.Errorf("pigpio set mode: %d", res) + if err := setPigpioMode(t.pigpio, t.cfg.Pin, piOutput); err != nil { + return err } return t.setInactive() } @@ -189,14 +94,7 @@ func (t *IRTransmitter) setInactive() error { if t.activeLow { level = 1 } - res, err := t.pigpio.command(piCmdWrite, uint32(t.cfg.Pin), level, 0, nil) - if err != nil { - return fmt.Errorf("pigpio write: %w", err) - } - if res < 0 { - return fmt.Errorf("pigpio write: %d", res) - } - return nil + return writePigpio(t.pigpio, t.cfg.Pin, level) } func (t *IRTransmitter) buildWaveform(code byte, repeat int) ([]gpioPulse, int) { @@ -303,7 +201,7 @@ func (t *IRTransmitter) writeWave(pulses []gpioPulse, duration time.Duration) er _ = binary.Write(buf, binary.LittleEndian, pulse.DelayUs) } data := buf.Bytes() - if res, err := t.pigpio.command(piCmdWaveAddGeneric, 0, 0, uint32(len(data)), data); err != nil { + if res, err := t.pigpio.command(piCmdWaveAdd, 0, 0, uint32(len(data)), data); err != nil { return fmt.Errorf("pigpio wave add: %w", err) } else if res < 0 { return fmt.Errorf("pigpio wave add: %d", res) diff --git a/pi/roverd/pigpio_client.go b/pi/roverd/pigpio_client.go new file mode 100644 index 00000000..4d1ab3c1 --- /dev/null +++ b/pi/roverd/pigpio_client.go @@ -0,0 +1,134 @@ +//go:build !dummy + +package roverd + +import ( + "encoding/binary" + "fmt" + "io" + "log" + "net" + "sync" + "time" +) + +const ( + defaultPigpioAddr = "localhost:8888" + piCmdSetMode = 0 + piCmdWrite = 4 + piCmdServo = 8 + piCmdWaveClear = 27 + piCmdWaveAdd = 28 + piCmdWaveTxBusy = 32 + piCmdWaveCreate = 49 + piCmdWaveDelete = 50 + piCmdWaveTxSend = 51 + piOutput = 1 + + pigpioConnectRetries = 20 + pigpioConnectDelay = 250 * time.Millisecond +) + +type pigpioClient struct { + conn net.Conn + mu sync.Mutex +} + +func newPigpioClient(addr string) (*pigpioClient, error) { + conn, err := net.Dial("tcp", addr) + if err != nil { + return nil, err + } + if tcpConn, ok := conn.(*net.TCPConn); ok { + _ = tcpConn.SetNoDelay(true) + } + return &pigpioClient{conn: conn}, nil +} + +func connectPigpioWithRetry(addr string, logger *log.Logger) (*pigpioClient, error) { + var lastErr error + for attempt := 1; attempt <= pigpioConnectRetries; attempt++ { + client, err := newPigpioClient(addr) + if err == nil { + return client, nil + } + lastErr = err + if attempt == 1 || attempt%4 == 0 { + logger.Printf("pigpio connect attempt %d/%d failed: %v", attempt, pigpioConnectRetries, err) + } + time.Sleep(pigpioConnectDelay) + } + return nil, lastErr +} + +func (c *pigpioClient) Close() error { + if c.conn == nil { + return nil + } + return c.conn.Close() +} + +func (c *pigpioClient) command(cmd, p1, p2, p3 uint32, ext []byte) (int32, error) { + c.mu.Lock() + defer c.mu.Unlock() + + var buf [16]byte + binary.LittleEndian.PutUint32(buf[0:], cmd) + binary.LittleEndian.PutUint32(buf[4:], p1) + binary.LittleEndian.PutUint32(buf[8:], p2) + binary.LittleEndian.PutUint32(buf[12:], p3) + + if _, err := c.conn.Write(buf[:]); err != nil { + return -1, err + } + if len(ext) > 0 { + if _, err := c.conn.Write(ext); err != nil { + return -1, err + } + } + if _, err := io.ReadFull(c.conn, buf[:]); err != nil { + return -1, err + } + res := int32(binary.LittleEndian.Uint32(buf[12:])) + return res, nil +} + +func ensurePigpioAddr(addr string) string { + if addr != "" { + return addr + } + return defaultPigpioAddr +} + +func setPigpioMode(client *pigpioClient, pin int, mode uint32) error { + res, err := client.command(piCmdSetMode, uint32(pin), mode, 0, nil) + if err != nil { + return fmt.Errorf("pigpio set mode: %w", err) + } + if res < 0 { + return fmt.Errorf("pigpio set mode: %d", res) + } + return nil +} + +func writePigpio(client *pigpioClient, pin int, level uint32) error { + res, err := client.command(piCmdWrite, uint32(pin), level, 0, nil) + if err != nil { + return fmt.Errorf("pigpio write: %w", err) + } + if res < 0 { + return fmt.Errorf("pigpio write: %d", res) + } + return nil +} + +func setPigpioServo(client *pigpioClient, pin int, pulseWidth int) error { + res, err := client.command(piCmdServo, uint32(pin), uint32(pulseWidth), 0, nil) + if err != nil { + return fmt.Errorf("pigpio servo: %w", err) + } + if res < 0 { + return fmt.Errorf("pigpio servo: %d", res) + } + return nil +}