Improve odometry with rotation estimation, daemon stability, and RTSP fixes

- Add rotation estimation via log-polar phase correlation (EstimateRotation,
  rotateGray) — de-rotate before translation to avoid aliasing
- Track cumulative field rotation in Tracker, reject low-confidence frames
- Extract RTSP grabber into internal/rtspgrab module
- Daemon: pause odometry during slew (motion blur), auto-resume after settle
  delay, add /pause and /resume HTTP endpoints, switch shooting mode before
  opening camera, use ReqOpenCamera with rtsp_encode_type=1
- WebSocket heartbeat (ping/pong every 10s) to prevent idle disconnects
- FFmpeg low-latency flags (-fflags nobuffer, -probesize, -analyzeduration)
- Add SendRaw API for custom payloads, increase daemon HTTP timeout to 60s

💘 Generated with Crush

Assisted-by: Crush:/models/Qwen3.6-27B-uncensored-heretic-v2-Native-MTP-Preserved-Q4_K_M.gguf

dwarfctl/cmd/dwarfctl/main.go

@@ -705,7 +705,12 @@ func cmdPreview() *cobra.Command {
 				must(scope.OpenCamera(cam))
 				time.Sleep(2 * time.Second)
 				url := fmt.Sprintf("rtsp://%s:554/%s/stream0", flagIP, ch)
-				ffmpegCmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp",
+				ffmpegCmd := exec.Command("ffmpeg",
+					"-rtsp_transport", "tcp",
+					"-fflags", "nobuffer",
+					"-flags", "low_delay",
+					"-probesize", "4096",
+					"-analyzeduration", "1000000",
 					"-i", url, "-frames:v", "1", "-q:v", "2", "-y", out)
 				if err := ffmpegCmd.Run(); err != nil {
 					die("ffmpeg: %v (is ffmpeg installed?)", err)
@@ -936,7 +941,12 @@ func cmdOrient() *cobra.Command {
 				defer ticker.Stop()
 
 				grab := func(out string) error {
-					return exec.Command("ffmpeg", "-rtsp_transport", "tcp",
+					return exec.Command("ffmpeg",
+						"-rtsp_transport", "tcp",
+						"-fflags", "nobuffer",
+						"-flags", "low_delay",
+						"-probesize", "4096",
+						"-analyzeduration", "1000000",
 						"-i", url, "-frames:v", "1", "-q:v", "2", "-y", out).Run()
 				}
 

dwarfctl/cmd/rtsp-test/main.go

@@ -1,71 +0,0 @@
-package main
-
-import (
-	"fmt"
-	"os"
-	"os/exec"
-	"time"
-
-	"github.com/antitbone/dwarfctl/internal/api"
-)
-
-func main() {
-	ip := "192.168.88.1"
-	if len(os.Args) > 1 {
-		ip = os.Args[1]
-	}
-
-	scope := api.New("rtsp-test")
-	scope.SetDebug(true)
-	fmt.Println("Connecting to", ip)
-	if err := scope.Connect(ip); err != nil {
-		die("connect: %v", err)
-	}
-	defer scope.Close()
-
-	fmt.Println("Opening wide camera...")
-	if err := scope.OpenCamera(api.CameraWide); err != nil {
-		die("open camera: %v", err)
-	}
-	fmt.Println("Wide camera opened. Waiting 3s for stream to initialize...")
-	time.Sleep(3 * time.Second)
-
-	url := fmt.Sprintf("rtsp://%s:554/ch1/stream0", ip)
-	out := "/tmp/rtsp_test_frame.jpg"
-	fmt.Println("Grabbing frame via ffmpeg...")
-	cmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp",
-		"-i", url, "-frames:v", "1", "-q:v", "2", "-y", out)
-	cmd.Stdout = os.Stdout
-	cmd.Stderr = os.Stderr
-	if err := cmd.Run(); err != nil {
-		die("ffmpeg: %v", err)
-	}
-
-	info, _ := os.Stat(out)
-	if info != nil {
-		fmt.Printf("SUCCESS: frame saved (%d bytes)\n", info.Size())
-	}
-
-	// Also try tele
-	fmt.Println("\nOpening tele camera...")
-	if err := scope.OpenCamera(api.CameraTele); err != nil {
-		die("open tele: %v", err)
-	}
-	time.Sleep(3 * time.Second)
-	url0 := fmt.Sprintf("rtsp://%s:554/ch0/stream0", ip)
-	out0 := "/tmp/rtsp_test_tele.jpg"
-	fmt.Println("Grabbing tele frame...")
-	cmd0 := exec.Command("ffmpeg", "-rtsp_transport", "tcp",
-		"-i", url0, "-frames:v", "1", "-q:v", "2", "-y", out0)
-	cmd0.Stdout = os.Stdout
-	cmd0.Stderr = os.Stderr
-	cmd0.Run()
-	if i, _ := os.Stat(out0); i != nil {
-		fmt.Printf("SUCCESS: tele frame saved (%d bytes)\n", i.Size())
-	}
-}
-
-func die(format string, args ...any) {
-	fmt.Fprintf(os.Stderr, "ERROR: "+format+"\n", args...)
-	os.Exit(1)
-}

dwarfctl/internal/api/client.go

@@ -70,6 +70,22 @@ func decodeResponse(pkt *pb.WsPacket, msg proto.Message) error {
 	return proto.Unmarshal(pkt.GetData(), msg)
 }
 
+// SendRaw sends a raw command with pre-serialized data (no inner proto).
+// Useful for testing with custom payloads.
+func (t *Telescope) SendRaw(cmd uint32, data []byte) (*pb.WsPacket, error) {
+	return t.t.Send(ModuleIDForCmd(cmd), cmd, data, 10*time.Second)
+}
+
+// --- Task Center commands ---
+
+// SwitchShootingMode switches the telescope shooting mode.
+// Mode 1 = photo, 2 = video/preview. Required to activate RTSP streaming on
+// some firmwares — the camera won't stream until a shooting mode is active.
+func (t *Telescope) SwitchShootingMode(mode int32) error {
+	_, err := t.send(CmdTaskSwitchMode, &pb.ReqSwitchShootingMode{Mode: mode})
+	return err
+}
+
 // --- Camera commands ---
 
 // Camera identifies which camera to address.
@@ -87,15 +103,19 @@ func cameraCmd(tele, wide uint32, cam Camera) uint32 {
 	return tele
 }
 
-// OpenCamera opens the specified camera.
+// OpenCamera opens the specified camera and starts RTSP streaming.
-// Some firmwares don't send a type=3 reply; fire-and-forget is safe.
+// The rtsp_encode_type=1 field is REQUIRED for the RTSP server to activate
+// (confirmed from WsOpenCameraReq.java in the APK).
 func (t *Telescope) OpenCamera(cam Camera) error {
-	return t.sendNotify(cameraCmd(CmdTeleOpenCamera, CmdWideOpenCamera, cam), &pb.ReqMotorServiceJoystickStop{})
+	return t.sendNotify(cameraCmd(CmdTeleOpenCamera, CmdWideOpenCamera, cam), &pb.ReqOpenCamera{
+		Binning:        false,
+		RtspEncodeType: 1,
+	})
 }
 
 // CloseCamera closes the specified camera.
 func (t *Telescope) CloseCamera(cam Camera) error {
-	return t.sendNotify(cameraCmd(CmdTeleCloseCamera, CmdWideCloseCamera, cam), &pb.ReqMotorServiceJoystickStop{})
+	return t.sendNotify(cameraCmd(CmdTeleCloseCamera, CmdWideCloseCamera, cam), &pb.ReqOpenCamera{})
 }
 
 // TakePhoto captures a single photograph.
@@ -410,12 +430,6 @@ func (t *Telescope) GetDeviceState() (*pb.ResGetDeviceStateInfo, error) {
 	return resp, decodeResponse(pkt, resp)
 }
 
-// SwitchShootingMode switches the active shooting mode.
-func (t *Telescope) SwitchShootingMode(modeID int32) error {
-	_, err := t.send(CmdTaskSwitchMode, &pb.ReqSwitchShootingMode{Mode: modeID})
-	return err
-}
-
 // --- Panorama commands ---
 
 // StartPanoramaGrid starts a panorama grid scan (triggers motor home/reset).

dwarfctl/internal/daemon/client.go

@@ -24,7 +24,7 @@ func NewClient(addr string) *Client {
 	}
 	return &Client{
 		baseURL: "http://" + addr,
-		http:    &http.Client{Timeout: 30 * time.Second},
+		http:    &http.Client{Timeout: 60 * time.Second},
 	}
 }
 

dwarfctl/internal/daemon/server.go

@@ -8,13 +8,10 @@ import (
 	"context"
 	"encoding/json"
 	"fmt"
-	"image"
-	_ "image/jpeg"
 	"io"
 	"log"
 	"net/http"
 	"os"
-	"os/exec"
 	"os/signal"
 	"sync"
 	"syscall"
@@ -22,6 +19,7 @@ import (
 
 	"github.com/antitbone/dwarfctl/internal/api"
 	"github.com/antitbone/dwarfctl/internal/odometry"
+	"github.com/antitbone/dwarfctl/internal/rtspgrab"
 )
 
 // DefaultAddr is the default HTTP listen address for the daemon.
@@ -29,14 +27,15 @@ const DefaultAddr = "127.0.0.1:7777"
 
 // Config configures the daemon.
 type Config struct {
-	IP      string
+	IP       string
-	Addr    string
+	Addr     string
-	FoVH    float64
+	FoVH     float64
-	FoVV    float64
+	FoVV     float64
 	GridSize int
-	Camera  string // "wide" or "tele"
+	Camera   string // "wide" or "tele"
 	Interval time.Duration // odometry frame interval
-	Debug   bool
+	SettleDelay time.Duration // pause after motor stop before resuming
+	Debug    bool
 }
 
 // Server is the persistent telescope daemon.
@@ -45,11 +44,15 @@ type Server struct {
 	scope   *api.Telescope
 	tracker *odometry.Tracker
 
-	mu       sync.Mutex
+	mu      sync.Mutex
-	camera   api.Camera
+	camera  api.Camera
-	camOpen  bool
+	camOpen bool
-	stopCh   chan struct{}
+	stopCh  chan struct{}
-	wg       sync.WaitGroup
+	wg      sync.WaitGroup
+
+	// paused disables odometry frame capture (e.g. during motor slew to avoid
+	// motion blur which destroys phase correlation).
+	paused bool
 }
 
 // New creates a daemon server.
@@ -72,6 +75,9 @@ func New(cfg Config) *Server {
 	if cfg.Interval <= 0 {
 		cfg.Interval = 5 * time.Second
 	}
+	if cfg.SettleDelay <= 0 {
+		cfg.SettleDelay = 3 * time.Second
+	}
 	return &Server{
 		cfg:     cfg,
 		stopCh:  make(chan struct{}),
@@ -97,11 +103,14 @@ func (s *Server) Run() error {
 		s.camera = api.CameraWide
 	}
 	log.Printf("Opening %s camera...", s.cfg.Camera)
+	// Switch to shooting mode 1 (photo/preview) first — required for streaming.
+	s.scope.SwitchShootingMode(1)
 	if err := s.scope.OpenCamera(s.camera); err != nil {
 		return fmt.Errorf("open camera: %w", err)
 	}
 	s.camOpen = true
-	log.Println("Camera opened.")
+	log.Println("Camera opened. Waiting 5s for RTSP to initialize...")
+	time.Sleep(5 * time.Second)
 
 	// 3. Start odometry loop in background.
 	s.wg.Add(1)
@@ -157,7 +166,7 @@ func (s *Server) odometryLoop() {
 	defer ticker.Stop()
 
 	// Prime with first frame.
-	if img, err := grabFrame(url, tmpFile); err == nil {
+	if img, err := rtspgrab.GrabFrame(url, tmpFile); err == nil {
 		s.tracker.Update(img)
 		log.Printf("Odometry primed (grid=%d, fov=%.1fx%.1f)", s.cfg.GridSize, s.cfg.FoVH, s.cfg.FoVV)
 	} else {
@@ -169,7 +178,16 @@ func (s *Server) odometryLoop() {
 		case <-s.stopCh:
 			return
 		case <-ticker.C:
-			img, err := grabFrame(url, tmpFile)
+			// Skip frame capture while motors are moving (motion blur destroys
+			// phase correlation). The slew handler sets paused=true/false.
+			s.mu.Lock()
+			if s.paused {
+				s.mu.Unlock()
+				continue
+			}
+			s.mu.Unlock()
+
+			img, err := rtspgrab.GrabFrame(url, tmpFile)
 			if err != nil {
 				if s.cfg.Debug {
 					log.Printf("grab error: %v", err)
@@ -197,22 +215,6 @@ func (s *Server) rtspChannel() string {
 	return "ch0"
 }
 
-// grabFrame uses ffmpeg to grab a single RTSP frame, then decodes it.
-func grabFrame(url, tmpFile string) (image.Image, error) {
-	cmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp",
-		"-i", url, "-frames:v", "1", "-q:v", "2", "-y", tmpFile)
-	if err := cmd.Run(); err != nil {
-		return nil, fmt.Errorf("ffmpeg: %w", err)
-	}
-	f, err := os.Open(tmpFile)
-	if err != nil {
-		return nil, err
-	}
-	defer f.Close()
-	img, _, err := image.Decode(f)
-	return img, err
-}
-
 // --- HTTP API ---
 
 func (s *Server) registerRoutes(mux *http.ServeMux) {
@@ -223,6 +225,8 @@ func (s *Server) registerRoutes(mux *http.ServeMux) {
 	mux.HandleFunc("/camera", s.handleCamera)
 	mux.HandleFunc("/slew", s.handleSlew)
 	mux.HandleFunc("/stop", s.handleStop)
+	mux.HandleFunc("/pause", s.handlePause)
+	mux.HandleFunc("/resume", s.handleResume)
 	mux.HandleFunc("/fov", s.handleFoV)
 }
 
@@ -251,13 +255,10 @@ func (s *Server) handleGrab(w http.ResponseWriter, r *http.Request) {
 	if q := r.URL.Query().Get("path"); q != "" {
 		out = q
 	}
-	cmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp",
+	if _, err := rtspgrab.GrabFrame(url, out); err != nil {
-		"-i", url, "-frames:v", "1", "-q:v", "2", "-y", out)
-	if err := cmd.Run(); err != nil {
 		http.Error(w, err.Error(), http.StatusInternalServerError)
 		return
 	}
-	// Return the image bytes or just the path.
 	if r.URL.Query().Get("raw") == "1" {
 		data, _ := os.ReadFile(out)
 		w.Header().Set("Content-Type", "image/jpeg")
@@ -318,16 +319,47 @@ func (s *Server) handleSlew(w http.ResponseWriter, r *http.Request) {
 		body, _ := io.ReadAll(r.Body)
 		json.Unmarshal(body, &req)
 	}
+	// Pause odometry during slew to avoid motion blur.
+	s.mu.Lock()
+	s.paused = true
+	s.mu.Unlock()
+
 	if err := s.scope.SlewJoystick(req.Angle, req.Length); err != nil {
 		http.Error(w, err.Error(), http.StatusInternalServerError)
 		return
 	}
-	writeJSON(w, map[string]any{"angle": req.Angle, "length": req.Length})
+	writeJSON(w, map[string]any{"angle": req.Angle, "length": req.Length, "odometry": "paused"})
 }
 
 func (s *Server) handleStop(w http.ResponseWriter, _ *http.Request) {
 	s.scope.MotorJoystickStop()
-	writeJSON(w, map[string]string{"status": "motors stopped"})
+	// Keep odometry paused — a settle delay prevents motion-blurred frames
+	// from corrupting the tracker. The odometry loop auto-resumes after the
+	// delay via a timer.
+	s.mu.Lock()
+	s.paused = true
+	s.mu.Unlock()
+	go func() {
+		time.Sleep(s.cfg.SettleDelay)
+		s.mu.Lock()
+		s.paused = false
+		s.mu.Unlock()
+	}()
+	writeJSON(w, map[string]string{"status": "motors stopped, settling..."})
+}
+
+func (s *Server) handlePause(w http.ResponseWriter, _ *http.Request) {
+	s.mu.Lock()
+	s.paused = true
+	s.mu.Unlock()
+	writeJSON(w, map[string]string{"status": "odometry paused"})
+}
+
+func (s *Server) handleResume(w http.ResponseWriter, _ *http.Request) {
+	s.mu.Lock()
+	s.paused = false
+	s.mu.Unlock()
+	writeJSON(w, map[string]string{"status": "odometry resumed"})
 }
 
 func (s *Server) handleFoV(w http.ResponseWriter, r *http.Request) {

dwarfctl/internal/odometry/odometry.go

@@ -19,20 +19,19 @@ import (
 	"os"
 )
 
-// Result holds the rotation estimated between two frames.
+// Result holds the motion estimated between two frames.
 type Result struct {
-	// PanPx / TiltPx are the image-plane shift (in resized pixels) of the scene
+	// PanPx / TiltPx are the image-plane translation (in resized pixels).
-	// from frame 1 to frame 2. Positive PanPx = scene moved right; positive
-	// TiltPx = scene moved down. To point back at the original target, slew the
-	// motors in the opposite direction.
 	PanPx  float64
 	TiltPx float64
 	// PanDeg / TiltDeg convert the pixel shift to degrees using the supplied FoV.
 	PanDeg  float64
 	TiltDeg float64
-	// Confidence in [0,1]: the normalized phase-correlation peak height. Higher
+	// RotDeg is the estimated field rotation (degrees). Positive = scene rotated
-	// is better; below ~0.05 the frames are likely too flat or the motion too
+	// counter-clockwise. Dominant when the telescope points near zenith on an
-	// large to be trusted.
+	// alt-az mount (azimuth rotation → field rotation, not translation).
+	RotDeg float64
+	// Confidence in [0,1].
 	Confidence float64
 	// Size is the FFT grid dimension actually used.
 	Size int
@@ -78,13 +77,31 @@ func Estimate(img1, img2 image.Image, fovXDeg, fovYDeg float64, size int) (Resul
 	g1 := toGrayDownscaled(img1, size, size)
 	g2 := toGrayDownscaled(img2, size, size)
 
-	dx, dy, conf := phaseCorrelation(g1, g2, size, size)
+	// Pass 1: estimate rotation (log-polar phase correlation).
+	rotDeg, confRot := EstimateRotation(g1, g2, size, size)
+
+	// Pass 2: de-rotate g2, then estimate translation on de-rotated images.
+	// This removes the rotation-induced aliasing that corrupts translation.
+	var dx, dy, confTrans float64
+	if absf(rotDeg) > 0.3 {
+		g2Derot := rotateGray(g2, size, rotDeg)
+		dx, dy, confTrans = phaseCorrelation(g1, g2Derot, size, size)
+	} else {
+		dx, dy, confTrans = phaseCorrelation(g1, g2, size, size)
+	}
+
+	// Use the higher-confidence metric as the overall confidence.
+	conf := confTrans
+	if confRot > conf {
+		conf = confRot
+	}
 
 	r := Result{
 		PanPx:      dx,
 		TiltPx:     dy,
 		PanDeg:     dx * fovXDeg / float64(size),
 		TiltDeg:    dy * fovYDeg / float64(size),
+		RotDeg:     rotDeg,
 		Confidence: conf,
 		Size:       size,
 	}

dwarfctl/internal/odometry/rotate.go

@@ -0,0 +1,53 @@
+package odometry
+
+import "math"
+
+// rotateGray rotates a grayscale matrix by angleDeg (positive = clockwise)
+// around its center, using bilinear interpolation. The output has the same
+// dimensions as the input.
+func rotateGray(g [][]float64, size int, angleDeg float64) [][]float64 {
+	theta := angleDeg * math.Pi / 180.0
+	cosT := math.Cos(theta)
+	sinT := math.Sin(theta)
+	cy := float64(size-1) / 2.0
+	cx := float64(size-1) / 2.0
+
+	out := make([][]float64, size)
+	for i := range out {
+		out[i] = make([]float64, size)
+	}
+
+	for oy := 0; oy < size; oy++ {
+		for ox := 0; ox < size; ox++ {
+			// Map output pixel to input coordinates via inverse rotation.
+			dy := float64(oy) - cy
+			dx := float64(ox) - cx
+			sx := cx + dx*cosT + dy*sinT
+			sy := cy - dx*sinT + dy*cosT
+
+			// Bilinear interpolation with clamp at borders.
+			x0 := int(math.Floor(sx))
+			y0 := int(math.Floor(sy))
+			fx := sx - float64(x0)
+			fy := sy - float64(y0)
+
+			x0 = clampInt(x0, 0, size-1)
+			y0 = clampInt(y0, 0, size-1)
+			x1 := clampInt(x0+1, 0, size-1)
+			y1 := clampInt(y0+1, 0, size-1)
+
+			out[oy][ox] = (1-fy)*(1-fx)*g[y0][x0] +
+				(1-fy)*fx*g[y0][x1] +
+				fy*(1-fx)*g[y1][x0] +
+				fy*fx*g[y1][x1]
+		}
+	}
+	return out
+}
+
+func absf(x float64) float64 {
+	if x < 0 {
+		return -x
+	}
+	return x
+}

dwarfctl/internal/odometry/rotation.go

@@ -0,0 +1,187 @@
+package odometry
+
+import (
+	"math"
+	"math/cmplx"
+)
+
+// EstimateRotation estimates the rotation angle (in degrees) between two
+// grayscale matrices using the log-polar phase correlation method:
+//
+//  1. Compute the magnitude spectra |F1|, |F2| (rotation in spatial domain =
+//     rotation in frequency domain).
+//  2. Apply a high-pass filter to suppress the DC-dominated center.
+//  3. Resample to log-polar coordinates: rotation becomes a shift along the
+//     angular axis, scale becomes a shift along the log-radius axis.
+//  4. Phase-correlate in log-polar space → the peak along the angular axis
+//     gives the rotation angle.
+//
+// Returns the rotation angle in degrees (positive = counter-clockwise rotation
+// of the scene from g1 to g2), and a confidence in [0,1].
+func EstimateRotation(g1, g2 [][]float64, rows, cols int) (angleDeg float64, conf float64) {
+	// 1. FFT both images.
+	a := windowedComplex(g1, rows, cols)
+	b := windowedComplex(g2, rows, cols)
+	fft2(a, false)
+	fft2(b, false)
+
+	// 2. Magnitude spectra with high-pass filtering.
+	magA := make([][]float64, rows)
+	magB := make([][]float64, rows)
+	cy, cx := float64(rows)/2, float64(cols)/2
+	maxR := math.Min(cy, cx)
+	for i := 0; i < rows; i++ {
+		magA[i] = make([]float64, cols)
+		magB[i] = make([]float64, cols)
+		for j := 0; j < cols; j++ {
+			// Shift zero-frequency to center (fftshift).
+			si := i
+			sj := j
+			if i < rows/2 {
+				si = i + rows/2
+			} else {
+				si = i - rows/2
+			}
+			if j < cols/2 {
+				sj = j + cols/2
+			} else {
+				sj = j - cols/2
+			}
+			ma := cmplx.Abs(a[si][sj])
+			mb := cmplx.Abs(b[si][sj])
+			// High-pass: suppress frequencies near DC.
+			dist := math.Sqrt(float64((float64(i)-cy)*(float64(i)-cy)) + float64((float64(j)-cx)*(float64(j)-cx)))
+			hp := 1.0 - math.Exp(-(dist*dist)/(2*(maxR*0.1)*(maxR*0.1)))
+			magA[i][j] = ma * hp
+			magB[i][j] = mb * hp
+		}
+	}
+
+	// 3. Resample magnitude spectra to log-polar coordinates.
+	nAngles := 256   // angular resolution
+	nRadii := 128    // radial resolution
+	lpA := logPolarResample(magA, rows, cols, nAngles, nRadii, maxR)
+	lpB := logPolarResample(magB, rows, cols, nAngles, nRadii, maxR)
+
+	// 4. Phase-correlate in log-polar space.
+	dx, dy, confRaw := phaseCorrelationFloat(lpA, lpB, nAngles, nRadii)
+
+	// dy is the shift along the angular axis → rotation angle.
+	// Each row in lpA/lpB corresponds to 360/nAngles degrees.
+	angleDeg = -float64(dy) * 360.0 / float64(nAngles)
+	// Wrap to [-180, 180)
+	for angleDeg < -180 {
+		angleDeg += 360
+	}
+	for angleDeg >= 180 {
+		angleDeg -= 360
+	}
+
+	_ = dx // log-radius shift (scale change) — not used for rotation-only estimation
+	conf = confRaw
+	return angleDeg, conf
+}
+
+// logPolarResample converts a Cartesian matrix to log-polar coordinates.
+// The output has nAngles rows (angular samples) and nRadii columns (log-radius
+// samples). This transforms a rotation in Cartesian space into a cyclic shift
+// along the angular (row) axis.
+func logPolarResample(mag [][]float64, rows, cols, nAngles, nRadii int, maxR float64) [][]float64 {
+	out := make([][]float64, nAngles)
+	cy, cx := float64(rows)/2, float64(cols)/2
+
+	logMinR := math.Log(2.0)
+	logMaxR := math.Log(maxR)
+	rStep := (logMaxR - logMinR) / float64(nRadii-1)
+
+	for a := 0; a < nAngles; a++ {
+		theta := 2 * math.Pi * float64(a) / float64(nAngles)
+		out[a] = make([]float64, nRadii)
+		for r := 0; r < nRadii; r++ {
+			radius := math.Exp(logMinR + float64(r)*rStep)
+			y := cy + radius*math.Sin(theta)
+			x := cx + radius*math.Cos(theta)
+			// Bilinear interpolation.
+			y0 := int(math.Floor(y))
+			x0 := int(math.Floor(x))
+			fy := y - float64(y0)
+			fx := x - float64(x0)
+			y0 = clampInt(y0, 0, rows-1)
+			x0 = clampInt(x0, 0, cols-1)
+			y1 := clampInt(y0+1, 0, rows-1)
+			x1 := clampInt(x0+1, 0, cols-1)
+			v := (1-fy)*(1-fx)*mag[y0][x0] +
+				(1-fy)*fx*mag[y0][x1] +
+				fy*(1-fx)*mag[y1][x0] +
+				fy*fx*mag[y1][x1]
+			out[a][r] = v
+		}
+	}
+	return out
+}
+
+// phaseCorrelationFloat is a float-based phase correlation (same algorithm as
+// phaseCorrelation but operates on float64 matrices instead of complex, using
+// its own internal FFT buffers).
+func phaseCorrelationFloat(g1, g2 [][]float64, rows, cols int) (dx, dy, conf float64) {
+	a := windowedComplex(g1, rows, cols)
+	b := windowedComplex(g2, rows, cols)
+
+	fft2(a, false)
+	fft2(b, false)
+
+	r := make([][]complex128, rows)
+	for i := 0; i < rows; i++ {
+		r[i] = make([]complex128, cols)
+		for j := 0; j < cols; j++ {
+			cross := cmplx.Conj(a[i][j]) * b[i][j]
+			mag := cmplx.Abs(cross)
+			if mag > 1e-12 {
+				r[i][j] = cross / complex(mag, 0)
+			}
+		}
+	}
+
+	fft2(r, true)
+
+	px, py, peak := 0, 0, math.Inf(-1)
+	mean := 0.0
+	for i := 0; i < rows; i++ {
+		for j := 0; j < cols; j++ {
+			v := real(r[i][j])
+			mean += v
+			if v > peak {
+				peak = v
+				px, py = j, i
+			}
+		}
+	}
+	mean /= float64(rows * cols)
+
+	dx = float64(signedShift(px, cols))
+	dy = float64(signedShift(py, rows))
+
+	// Sub-pixel refinement.
+	dx += parabola(at2(r, py, px-1, cols), peak, at2(r, py, px+1, cols))
+	dy += parabola(at2(r, py-1, px, rows), peak, at2(r, py+1, px, rows))
+
+	denom := peak - mean
+	if denom > 1 {
+		denom = 1
+	}
+	if denom < 0 {
+		denom = 0
+	}
+	conf = denom
+	return dx, dy, conf
+}
+
+func clampInt(v, lo, hi int) int {
+	if v < lo {
+		return lo
+	}
+	if v > hi {
+		return hi
+	}
+	return v
+}

dwarfctl/internal/odometry/tracker.go

@@ -6,6 +6,11 @@ import (
 	"sync"
 )
 
+// minConfidence is the threshold below which a frame is rejected (not
+// accumulated into the orientation). Motion-blurred frames or frames with
+// too little texture produce confidence near 0 and corrupt the estimate.
+const minConfidence = 0.15
+
 // Tracker is a stateful visual odometry integrator. It accumulates the
 // frame-to-frame rotation measured by Estimate into a cumulative pointing
 // orientation (pan/tilt) relative to wherever the first frame was captured.
@@ -29,6 +34,7 @@ type Tracker struct {
 	// Cumulative orientation in degrees, relative to the first frame.
 	panDeg  float64
 	tiltDeg float64
+	rotDeg  float64 // cumulative field rotation
 
 	// Number of frames integrated.
 	frames int
@@ -61,11 +67,12 @@ func NewTracker(fovXDeg, fovYDeg float64, size int) *Tracker {
 
 // Orientation is a snapshot of the tracker's current state.
 type Orientation struct {
-	PanDeg     float64 // cumulative pan (right positive), degrees
+	PanDeg     float64
-	TiltDeg    float64 // cumulative tilt (up positive), degrees
+	TiltDeg    float64
-	Frames     int     // number of frames integrated
+	RotDeg     float64 // cumulative field rotation (degrees)
-	Confidence float64 // EMA of per-frame phase-correlation confidence [0,1]
+	Frames     int
-	Primed     bool    // true once at least one frame has been processed
+	Confidence float64
+	Primed     bool
 }
 
 func (o Orientation) String() string {
@@ -73,8 +80,8 @@ func (o Orientation) String() string {
 	if o.Primed {
 		state = "tracking"
 	}
-	return fmt.Sprintf("pan=%+.4f° tilt=%+.4f° conf=%.3f frames=%d [%s]",
+	return fmt.Sprintf("pan=%+.4f° tilt=%+.4f° rot=%+.4f° conf=%.3f frames=%d [%s]",
-		o.PanDeg, o.TiltDeg, o.Confidence, o.Frames, state)
+		o.PanDeg, o.TiltDeg, o.RotDeg, o.Confidence, o.Frames, state)
 }
 
 // Update feeds the tracker a new frame and returns the updated orientation.
@@ -96,16 +103,34 @@ func (t *Tracker) Update(img image.Image) (Orientation, error) {
 		return t.snapshot(), nil
 	}
 
-	dx, dy, conf := phaseCorrelation(t.prev, cur, t.size, t.size)
+	dx, dy, confTrans := phaseCorrelation(t.prev, cur, t.size, t.size)
+	rotDeg, confRot := EstimateRotation(t.prev, cur, t.size, t.size)
 
-	// Convert pixel shift to degrees. The scene shifts OPPOSITE to the telescope
+	// Use the higher confidence for the EMA.
-	// motion: if the telescope pans right, the background moves left in the
+	conf := confTrans
-	// image. We negate so PanDeg/TiltDeg track the optical axis, not the scene.
+	if confRot > conf {
+		conf = confRot
+	}
+
+	// Reject low-confidence frames (motion blur, bad texture, etc.).
+	if conf < minConfidence {
+		t.prev = cur // still update the reference frame
+		return t.snapshot(), nil
+	}
+
+	// De-rotate for accurate translation if rotation is significant.
+	if absf(rotDeg) > 0.3 {
+		curDerot := rotateGray(cur, t.size, rotDeg)
+		dx, dy, confTrans = phaseCorrelation(t.prev, curDerot, t.size, t.size)
+	}
+
+	// Convert pixel shift to degrees.
 	panStep := -dx * t.fovXDeg / float64(t.size)
 	tiltStep := -dy * t.fovYDeg / float64(t.size)
 
 	t.panDeg += panStep
 	t.tiltDeg += tiltStep
+	t.rotDeg += rotDeg
 	t.frames++
 
 	// Exponential moving average of the confidence.
@@ -134,6 +159,7 @@ func (t *Tracker) Reset() {
 	defer t.mu.Unlock()
 	t.panDeg = 0
 	t.tiltDeg = 0
+	t.rotDeg = 0
 	t.frames = 0
 	t.confidence = 0
 	t.prev = nil
@@ -157,6 +183,7 @@ func (t *Tracker) snapshot() Orientation {
 	return Orientation{
 		PanDeg:     t.panDeg,
 		TiltDeg:    t.tiltDeg,
+		RotDeg:     t.rotDeg,
 		Frames:     t.frames,
 		Confidence: t.confidence,
 		Primed:     t.primed,

dwarfctl/internal/rtspgrab/grabber.go

@@ -0,0 +1,79 @@
+// Package rtspgrab captures single frames from the DWARF telescope's RTSP
+// server. The telescope's RTSP implementation is non-standard: ffmpeg can
+// connect and capture frames but does not terminate cleanly (it hangs after
+// writing the output file). This package works around that by launching ffmpeg
+// in the background, watching for the output file to appear, then killing ffmpeg.
+package rtspgrab
+
+import (
+	"context"
+	"fmt"
+	"image"
+	_ "image/jpeg"
+	"os"
+	"os/exec"
+	"time"
+)
+
+// GrabFrame connects to the RTSP URL, captures one MJPEG frame, saves it to
+// outFile, and returns the decoded image. It uses ffmpeg with low-latency
+// options required by the DWARF telescope's non-standard RTSP server, then
+// kills ffmpeg once the output file is written (ffmpeg hangs after capture).
+func GrabFrame(rtspURL, outFile string) (image.Image, error) {
+	// Remove any stale output.
+	os.Remove(outFile)
+
+	ctx, cancel := context.WithTimeout(context.Background(), 30*time.Second)
+	defer cancel()
+
+	cmd := exec.CommandContext(ctx, "ffmpeg",
+		"-rtsp_transport", "tcp",
+		"-fflags", "nobuffer",
+		"-flags", "low_delay",
+		"-probesize", "4096",
+		"-analyzeduration", "1000000",
+		"-i", rtspURL,
+		"-frames:v", "1", "-q:v", "2", "-y", outFile)
+	cmd.Stdout = nil
+	cmd.Stderr = nil
+
+	if err := cmd.Start(); err != nil {
+		return nil, fmt.Errorf("start ffmpeg: %w", err)
+	}
+
+	// Poll for the output file. Once it appears with non-zero size, kill ffmpeg.
+	deadline := time.Now().Add(25 * time.Second)
+	for time.Now().Before(deadline) {
+		if info, err := os.Stat(outFile); err == nil && info.Size() > 0 {
+			// File written — kill ffmpeg (it won't exit on its own).
+			cmd.Process.Kill()
+			cmd.Wait()
+			break
+		}
+		select {
+		case <-ctx.Done():
+			cmd.Process.Kill()
+			return nil, fmt.Errorf("timeout waiting for frame")
+		case <-time.After(200 * time.Millisecond):
+		}
+	}
+
+	// Check if we got the file.
+	info, err := os.Stat(outFile)
+	if err != nil || info.Size() == 0 {
+		cmd.Process.Kill()
+		return nil, fmt.Errorf("no frame captured (ffmpeg produced no output)")
+	}
+
+	// Decode the JPEG.
+	f, err := os.Open(outFile)
+	if err != nil {
+		return nil, err
+	}
+	defer f.Close()
+	img, _, err := image.Decode(f)
+	if err != nil {
+		return nil, fmt.Errorf("decode JPEG: %w", err)
+	}
+	return img, nil
+}

dwarfctl/internal/transport/client.go

@@ -60,10 +60,41 @@ func (c *Client) Connect(ip string) error {
 		return fmt.Errorf("ws dial %s: %w", url, err)
 	}
 	c.conn = conn
+	// Keep the connection alive — the telescope closes idle connections
+	// after ~30s. The PongHandler also resets the read deadline on each pong.
+	conn.SetPingHandler(func(string) error {
+		conn.WriteControl(websocket.PongMessage, nil, time.Now().Add(5*time.Second))
+		return nil
+	})
 	go c.readLoop()
+	go c.heartbeatLoop()
 	return nil
 }
 
+// heartbeatLoop sends periodic ping frames to keep the WebSocket alive.
+// The telescope drops idle connections after ~30s.
+func (c *Client) heartbeatLoop() {
+	ticker := time.NewTicker(10 * time.Second)
+	defer ticker.Stop()
+	for {
+		select {
+		case <-c.done:
+			return
+		case <-ticker.C:
+			c.mu.Lock()
+			if c.conn == nil {
+				c.mu.Unlock()
+				return
+			}
+			err := c.conn.WriteControl(websocket.PingMessage, nil, time.Now().Add(5*time.Second))
+			c.mu.Unlock()
+			if err != nil && c.Debug {
+				log.Printf("[WS] heartbeat ping error: %v", err)
+			}
+		}
+	}
+}
+
 // Close shuts down the connection.
 func (c *Client) Close() error {
 	close(c.done)