diff --git a/dwarfctl/cmd/dwarfctl/main.go b/dwarfctl/cmd/dwarfctl/main.go index 6767205..be5981b 100644 --- a/dwarfctl/cmd/dwarfctl/main.go +++ b/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() } diff --git a/dwarfctl/cmd/rtsp-test/main.go b/dwarfctl/cmd/rtsp-test/main.go deleted file mode 100644 index 98555a9..0000000 --- a/dwarfctl/cmd/rtsp-test/main.go +++ /dev/null @@ -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) -} diff --git a/dwarfctl/internal/api/client.go b/dwarfctl/internal/api/client.go index 48ffc2c..0010c71 100644 --- a/dwarfctl/internal/api/client.go +++ b/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. -// Some firmwares don't send a type=3 reply; fire-and-forget is safe. +// OpenCamera opens the specified camera and starts RTSP streaming. +// 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). diff --git a/dwarfctl/internal/daemon/client.go b/dwarfctl/internal/daemon/client.go index 4700dcd..a15d02d 100644 --- a/dwarfctl/internal/daemon/client.go +++ b/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}, } } diff --git a/dwarfctl/internal/daemon/server.go b/dwarfctl/internal/daemon/server.go index bdb3349..eb8e52f 100644 --- a/dwarfctl/internal/daemon/server.go +++ b/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 - Addr string - FoVH float64 - FoVV float64 + IP string + Addr string + FoVH 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 - camera api.Camera - camOpen bool - stopCh chan struct{} - wg sync.WaitGroup + mu sync.Mutex + camera api.Camera + camOpen bool + stopCh chan struct{} + 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", - "-i", url, "-frames:v", "1", "-q:v", "2", "-y", out) - if err := cmd.Run(); err != nil { + if _, err := rtspgrab.GrabFrame(url, out); 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) { diff --git a/dwarfctl/internal/odometry/odometry.go b/dwarfctl/internal/odometry/odometry.go index 10c8138..5fd33a1 100644 --- a/dwarfctl/internal/odometry/odometry.go +++ b/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 - // 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 / TiltPx are the image-plane translation (in resized pixels). 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 - // is better; below ~0.05 the frames are likely too flat or the motion too - // large to be trusted. + // RotDeg is the estimated field rotation (degrees). Positive = scene rotated + // counter-clockwise. Dominant when the telescope points near zenith on an + // 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, } diff --git a/dwarfctl/internal/odometry/rotate.go b/dwarfctl/internal/odometry/rotate.go new file mode 100644 index 0000000..8bc136d --- /dev/null +++ b/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 +} diff --git a/dwarfctl/internal/odometry/rotation.go b/dwarfctl/internal/odometry/rotation.go new file mode 100644 index 0000000..83e84bc --- /dev/null +++ b/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 +} diff --git a/dwarfctl/internal/odometry/tracker.go b/dwarfctl/internal/odometry/tracker.go index 8e3f477..8011355 100644 --- a/dwarfctl/internal/odometry/tracker.go +++ b/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 - TiltDeg float64 // cumulative tilt (up positive), degrees - Frames int // number of frames integrated - Confidence float64 // EMA of per-frame phase-correlation confidence [0,1] - Primed bool // true once at least one frame has been processed + PanDeg float64 + TiltDeg float64 + RotDeg float64 // cumulative field rotation (degrees) + Frames int + 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]", - o.PanDeg, o.TiltDeg, o.Confidence, o.Frames, state) + return fmt.Sprintf("pan=%+.4f° tilt=%+.4f° rot=%+.4f° conf=%.3f frames=%d [%s]", + 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 - // motion: if the telescope pans right, the background moves left in the - // image. We negate so PanDeg/TiltDeg track the optical axis, not the scene. + // Use the higher confidence for the EMA. + conf := confTrans + 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, diff --git a/dwarfctl/internal/rtspgrab/grabber.go b/dwarfctl/internal/rtspgrab/grabber.go new file mode 100644 index 0000000..b6b2624 --- /dev/null +++ b/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 +} diff --git a/dwarfctl/internal/transport/client.go b/dwarfctl/internal/transport/client.go index 06bbb58..dc574ef 100644 --- a/dwarfctl/internal/transport/client.go +++ b/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) diff --git a/dwarfctl/rtsp-test b/dwarfctl/rtsp-test new file mode 100755 index 0000000..010b27b Binary files /dev/null and b/dwarfctl/rtsp-test differ