code_public/dwarf-go
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
- 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
This commit is contained in:
Jacquin Antoine
2026-07-13 23:00:30 +02:00
parent 8daa9b4d58
commit d319ef6b4a
12 changed files with 522 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
dwarfctl/cmd/dwarfctl/main.go
View File

@ -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()
}

71
dwarfctl/cmd/rtsp-test/main.go
View File

@ -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)
}

34
dwarfctl/internal/api/client.go
View File

@ -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).

2
dwarfctl/internal/daemon/client.go
View File

@ -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},
}
}

110
dwarfctl/internal/daemon/server.go
View File

@ -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) {

35
dwarfctl/internal/odometry/odometry.go
View File

@ -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,
}

53
dwarfctl/internal/odometry/rotate.go Normal file
View File

@ -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
}

187
dwarfctl/internal/odometry/rotation.go Normal file
View File

@ -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
}

49
dwarfctl/internal/odometry/tracker.go
View File

@ -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,

79
dwarfctl/internal/rtspgrab/grabber.go Normal file
View File

@ -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
}

31
dwarfctl/internal/transport/client.go
View File

@ -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)

BIN
dwarfctl/rtsp-test Executable file
View File

Binary file not shown.