gps manipulations tailored to sim7600h hat
This commit is contained in:
@@ -1,269 +1,305 @@
|
||||
"""GPS service - connects to gpsd, buffers data, handles reconnection."""
|
||||
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from typing import Any
|
||||
|
||||
# ============================================================================
|
||||
# DEBUG MODE - Set True for development without GPS hardware
|
||||
# When True: skips gpsd entirely, generates realistic mock data
|
||||
# When False: connects to real gpsd (requires GPS device)
|
||||
# ============================================================================
|
||||
_GPS_DEBUG = True
|
||||
|
||||
# gpsdclient is a modern, simple gpsd client
|
||||
# Install gpsd on Pi: sudo apt install gpsd gpsd-clients
|
||||
# Configure: sudo nano /etc/default/gpsd (set DEVICES="/dev/ttyUSB0" or similar)
|
||||
try:
|
||||
from gpsdclient import GPSDClient
|
||||
except ImportError:
|
||||
GPSDClient = None # Allow import without gpsd for testing structure
|
||||
|
||||
|
||||
class GPSService:
|
||||
"""Threaded GPS reader with buffering and auto-reconnect."""
|
||||
|
||||
def __init__(self, host: str = "127.0.0.1", port: int = 2947, buffer_size: int = 100):
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.buffer_size = buffer_size
|
||||
|
||||
self._buffer: deque[dict[str, Any]] = deque(maxlen=buffer_size)
|
||||
self._latest: dict[str, Any] = {}
|
||||
self._connected = False
|
||||
self._running = False
|
||||
self._thread: threading.Thread | None = None
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# Callback for push-based updates
|
||||
self._on_data_callback = None
|
||||
|
||||
# Periodic status logging
|
||||
self._last_status_log = 0.0
|
||||
self._fix_count = 0
|
||||
|
||||
def set_on_data(self, callback):
|
||||
"""Set callback for new GPS fix. Called with fix dict."""
|
||||
self._on_data_callback = callback
|
||||
|
||||
@property
|
||||
def connected(self) -> bool:
|
||||
return self._connected
|
||||
|
||||
def get_latest(self) -> dict[str, Any]:
|
||||
"""Get most recent GPS fix."""
|
||||
with self._lock:
|
||||
return self._latest.copy() if self._latest else {"error": "no data"}
|
||||
|
||||
def get_buffer(self) -> list[dict[str, Any]]:
|
||||
"""Get buffered GPS history."""
|
||||
with self._lock:
|
||||
return list(self._buffer)
|
||||
|
||||
def start(self):
|
||||
"""Start background GPS reader thread."""
|
||||
if self._running:
|
||||
return
|
||||
self._running = True
|
||||
self._thread = threading.Thread(target=self._reader_loop, daemon=True)
|
||||
self._thread.start()
|
||||
print("[GPS] Service started")
|
||||
|
||||
def stop(self):
|
||||
"""Stop background reader."""
|
||||
self._running = False
|
||||
if self._thread:
|
||||
self._thread.join(timeout=2.0)
|
||||
|
||||
def _reader_loop(self):
|
||||
"""Main reader loop with reconnection logic."""
|
||||
print("[GPS] Reader thread running")
|
||||
while self._running:
|
||||
try:
|
||||
self._connect_and_read()
|
||||
except Exception as e:
|
||||
self._connected = False
|
||||
print(f"[GPS] Connection error: {e}, retrying in 5s...")
|
||||
time.sleep(5)
|
||||
|
||||
def _connect_and_read(self):
|
||||
"""Connect to gpsd and read data."""
|
||||
# Debug mode: skip gpsd entirely, use stub data
|
||||
if _GPS_DEBUG:
|
||||
print("[GPS] Debug mode enabled, using stub data")
|
||||
self._stub_mode()
|
||||
return
|
||||
|
||||
if GPSDClient is None:
|
||||
print("[GPS] gpsdclient not installed, running in stub mode")
|
||||
self._stub_mode()
|
||||
return
|
||||
|
||||
# Quick check if gpsd is reachable before attempting connection
|
||||
import socket
|
||||
try:
|
||||
sock = socket.create_connection((self.host, self.port), timeout=2.0)
|
||||
sock.close()
|
||||
except (socket.timeout, socket.error, OSError) as e:
|
||||
print(f"[GPS] gpsd not reachable at {self.host}:{self.port}: {e}")
|
||||
raise ConnectionError(f"gpsd not reachable: {e}")
|
||||
|
||||
try:
|
||||
client = GPSDClient(host=self.host, port=self.port)
|
||||
except Exception as e:
|
||||
print(f"[GPS] Cannot connect to gpsd at {self.host}:{self.port}: {e}")
|
||||
raise ConnectionError(f"gpsd connection failed: {e}")
|
||||
|
||||
with client:
|
||||
self._connected = True
|
||||
print(f"[GPS] Connected to gpsd at {self.host}:{self.port}")
|
||||
|
||||
self._last_status_log = time.time()
|
||||
self._fix_count = 0
|
||||
first_fix_timeout = time.time() + 5.0 # 5s to get first fix
|
||||
|
||||
for result in client.dict_stream(filter=["TPV"]):
|
||||
if not self._running:
|
||||
break
|
||||
|
||||
# TPV = Time-Position-Velocity report
|
||||
fix = {
|
||||
"time": result.get("time"),
|
||||
"lat": result.get("lat"),
|
||||
"lon": result.get("lon"),
|
||||
"alt": result.get("alt"),
|
||||
"speed": result.get("speed"), # m/s
|
||||
"track": result.get("track"), # heading in degrees
|
||||
"mode": result.get("mode"), # 0=no fix, 2=2D, 3=3D
|
||||
"satellites": result.get("satellites"), # from SKY messages
|
||||
}
|
||||
|
||||
# Check if this is a real fix (has position) or just empty TPV
|
||||
if fix.get("lat") is None and fix.get("mode") in (None, 0, 1):
|
||||
# No real data yet, check timeout
|
||||
if time.time() > first_fix_timeout:
|
||||
print("[GPS] No GPS fix after 5s, will retry connection")
|
||||
raise ConnectionError("No GPS fix within timeout")
|
||||
continue # Skip empty fixes
|
||||
|
||||
# Got real data, disable timeout
|
||||
first_fix_timeout = float('inf')
|
||||
|
||||
with self._lock:
|
||||
self._latest = fix
|
||||
if fix.get("lat") is not None:
|
||||
self._buffer.append(fix)
|
||||
|
||||
# Invoke callback with new fix
|
||||
if self._on_data_callback:
|
||||
self._on_data_callback(fix)
|
||||
|
||||
# Periodic status log (every 5s)
|
||||
self._fix_count += 1
|
||||
now = time.time()
|
||||
if now - self._last_status_log >= 5.0:
|
||||
elapsed = now - self._last_status_log
|
||||
fps = self._fix_count / elapsed
|
||||
speed = fix.get('speed', 0) or 0
|
||||
track = fix.get('track', 0) or 0
|
||||
mode = fix.get('mode', 0) or 0
|
||||
sats = fix.get('satellites', '?')
|
||||
print(f"[GPS] {fps:.1f} fix/s | {speed:.1f}m/s hdg={track:.0f}° mode={mode} sats={sats}")
|
||||
self._last_status_log = now
|
||||
self._fix_count = 0
|
||||
|
||||
def _stub_mode(self):
|
||||
"""Generate realistic mock GPS data for development/testing.
|
||||
|
||||
Simulates:
|
||||
- Normal 3D fix with satellites
|
||||
- Occasional signal loss (~3% chance per second, lasts ~5s)
|
||||
- Wandering position near Tokyo
|
||||
"""
|
||||
self._last_status_log = time.time()
|
||||
self._fix_count = 0
|
||||
|
||||
# Signal loss state
|
||||
signal_lost = False
|
||||
signal_lost_until = 0.0
|
||||
|
||||
# Base position (Tokyo area)
|
||||
base_lat = 35.6762
|
||||
base_lon = 139.6503
|
||||
base_alt = 40.0
|
||||
|
||||
# Smoothly varying heading/speed
|
||||
heading = random.uniform(0, 360)
|
||||
speed = random.uniform(5, 15)
|
||||
|
||||
while self._running:
|
||||
self._connected = True
|
||||
now = time.time()
|
||||
|
||||
# Check for signal loss simulation
|
||||
if signal_lost:
|
||||
if now >= signal_lost_until:
|
||||
signal_lost = False
|
||||
print("[GPS] Signal recovered (stub)")
|
||||
else:
|
||||
# ~30% chance per second to lose signal
|
||||
if random.random() < 0.3:
|
||||
signal_lost = True
|
||||
signal_lost_until = now + 2 # fixed 2s loss
|
||||
print("[GPS] Signal loss simulation (stub)")
|
||||
|
||||
if signal_lost:
|
||||
# No fix - mode 1, no satellites, no track
|
||||
# Note: use None, not float('nan') - NaN doesn't serialize to valid JSON
|
||||
fix = {
|
||||
"time": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"lat": None,
|
||||
"lon": None,
|
||||
"alt": None,
|
||||
"speed": None,
|
||||
"track": None,
|
||||
"mode": 1,
|
||||
"satellites": 0,
|
||||
}
|
||||
else:
|
||||
# Smoothly vary heading and speed
|
||||
heading = (heading + random.uniform(1, 3)) % 360
|
||||
speed = max(0, min(30, speed + random.uniform(-2, 2)))
|
||||
|
||||
fix = {
|
||||
"time": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"lat": base_lat + random.uniform(-0.001, 0.001),
|
||||
"lon": base_lon + random.uniform(-0.001, 0.001),
|
||||
"alt": base_alt + random.uniform(-5, 5),
|
||||
"speed": speed,
|
||||
"track": heading,
|
||||
"mode": 3,
|
||||
"satellites": random.randint(6, 12),
|
||||
}
|
||||
|
||||
with self._lock:
|
||||
self._latest = fix
|
||||
if fix.get("lat") is not None:
|
||||
self._buffer.append(fix)
|
||||
|
||||
# Invoke callback with new fix
|
||||
if self._on_data_callback:
|
||||
self._on_data_callback(fix)
|
||||
|
||||
# Periodic status log (every 5s)
|
||||
self._fix_count += 1
|
||||
if now - self._last_status_log >= 5.0:
|
||||
elapsed = now - self._last_status_log
|
||||
fps = self._fix_count / elapsed
|
||||
speed_val = fix.get('speed') or 0
|
||||
track_val = fix.get('track')
|
||||
track_str = f"{track_val:.0f}" if track_val is not None else "---"
|
||||
mode = fix.get('mode', 0)
|
||||
sats = fix.get('satellites', 0)
|
||||
print(f"[GPS] {fps:.1f} fix/s | {speed_val:.1f}m/s hdg={track_str} mode={mode} sats={sats} (stub)")
|
||||
self._last_status_log = now
|
||||
self._fix_count = 0
|
||||
|
||||
time.sleep(1)
|
||||
"""GPS service - connects to gpsd, buffers data, handles reconnection."""
|
||||
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
from collections import deque
|
||||
from typing import Any
|
||||
|
||||
# ============================================================================
|
||||
# DEBUG MODE - Set True for development without GPS hardware
|
||||
# When True: skips gpsd entirely, generates realistic mock data
|
||||
# When False: connects to real gpsd (requires GPS device)
|
||||
# ============================================================================
|
||||
_GPS_DEBUG = True
|
||||
|
||||
# gpsdclient is a modern, simple gpsd client
|
||||
# Install gpsd on Pi: sudo apt install gpsd gpsd-clients
|
||||
# Configure: sudo nano /etc/default/gpsd (set DEVICES="/dev/ttyUSB0" or similar)
|
||||
try:
|
||||
from gpsdclient import GPSDClient
|
||||
except ImportError:
|
||||
GPSDClient = None # Allow import without gpsd for testing structure
|
||||
|
||||
|
||||
class GPSService:
|
||||
"""Threaded GPS reader with buffering and auto-reconnect."""
|
||||
|
||||
def __init__(self, host: str = "127.0.0.1", port: int = 2947, buffer_size: int = 100):
|
||||
self.host = host
|
||||
self.port = port
|
||||
self.buffer_size = buffer_size
|
||||
|
||||
self._buffer: deque[dict[str, Any]] = deque(maxlen=buffer_size)
|
||||
self._latest: dict[str, Any] = {}
|
||||
self._connected = False
|
||||
self._running = False
|
||||
self._thread: threading.Thread | None = None
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# Callback for push-based updates
|
||||
self._on_data_callback = None
|
||||
|
||||
# GPS state tracking (NMEA can't distinguish "acquiring" from "lost")
|
||||
self._has_ever_fixed = False # True after first valid fix this session
|
||||
|
||||
# Periodic status logging
|
||||
self._last_status_log = 0.0
|
||||
self._fix_count = 0
|
||||
|
||||
def set_on_data(self, callback):
|
||||
"""Set callback for new GPS fix. Called with fix dict."""
|
||||
self._on_data_callback = callback
|
||||
|
||||
@property
|
||||
def connected(self) -> bool:
|
||||
return self._connected
|
||||
|
||||
def get_latest(self) -> dict[str, Any]:
|
||||
"""Get most recent GPS fix."""
|
||||
with self._lock:
|
||||
return self._latest.copy() if self._latest else {"error": "no data"}
|
||||
|
||||
def _gps_state(self, fix: dict) -> str:
|
||||
"""Determine GPS state: acquiring, fix, or lost.
|
||||
|
||||
NMEA doesn't distinguish 'never had fix' from 'lost signal' — both
|
||||
report mode 1 with no position. We track it ourselves.
|
||||
"""
|
||||
has_fix = fix.get("mode") in (2, 3) and fix.get("lat") is not None
|
||||
if has_fix:
|
||||
return "fix"
|
||||
return "lost" if self._has_ever_fixed else "acquiring"
|
||||
|
||||
def get_buffer(self) -> list[dict[str, Any]]:
|
||||
"""Get buffered GPS history."""
|
||||
with self._lock:
|
||||
return list(self._buffer)
|
||||
|
||||
def start(self):
|
||||
"""Start background GPS reader thread."""
|
||||
if self._running:
|
||||
return
|
||||
self._running = True
|
||||
self._thread = threading.Thread(target=self._reader_loop, daemon=True)
|
||||
self._thread.start()
|
||||
print("[GPS] Service started")
|
||||
|
||||
def stop(self):
|
||||
"""Stop background reader."""
|
||||
self._running = False
|
||||
if self._thread:
|
||||
self._thread.join(timeout=2.0)
|
||||
|
||||
def _reader_loop(self):
|
||||
"""Main reader loop with reconnection logic."""
|
||||
print("[GPS] Reader thread running")
|
||||
while self._running:
|
||||
try:
|
||||
self._connect_and_read()
|
||||
except Exception as e:
|
||||
self._connected = False
|
||||
print(f"[GPS] Connection error: {e}, retrying in 5s...")
|
||||
time.sleep(5)
|
||||
|
||||
def _connect_and_read(self):
|
||||
"""Connect to gpsd and read data."""
|
||||
# Debug mode: skip gpsd entirely, use stub data
|
||||
if _GPS_DEBUG:
|
||||
print("[GPS] Debug mode enabled, using stub data")
|
||||
self._stub_mode()
|
||||
return
|
||||
|
||||
if GPSDClient is None:
|
||||
print("[GPS] gpsdclient not installed, running in stub mode")
|
||||
self._stub_mode()
|
||||
return
|
||||
|
||||
# Quick check if gpsd is reachable before attempting connection
|
||||
import socket
|
||||
try:
|
||||
sock = socket.create_connection((self.host, self.port), timeout=2.0)
|
||||
sock.close()
|
||||
except (socket.timeout, socket.error, OSError) as e:
|
||||
print(f"[GPS] gpsd not reachable at {self.host}:{self.port}: {e}")
|
||||
raise ConnectionError(f"gpsd not reachable: {e}")
|
||||
|
||||
try:
|
||||
client = GPSDClient(host=self.host, port=self.port)
|
||||
except Exception as e:
|
||||
print(f"[GPS] Cannot connect to gpsd at {self.host}:{self.port}: {e}")
|
||||
raise ConnectionError(f"gpsd connection failed: {e}")
|
||||
|
||||
with client:
|
||||
self._connected = True
|
||||
print(f"[GPS] Connected to gpsd at {self.host}:{self.port}")
|
||||
|
||||
self._last_status_log = time.time()
|
||||
self._fix_count = 0
|
||||
# 120s for initial cold fix, 10s for signal loss after first fix
|
||||
fix_timeout = time.time() + 120.0
|
||||
|
||||
for result in client.dict_stream(filter=["TPV"]):
|
||||
if not self._running:
|
||||
break
|
||||
|
||||
# TPV = Time-Position-Velocity report
|
||||
fix = {
|
||||
"time": result.get("time"),
|
||||
"lat": result.get("lat"),
|
||||
"lon": result.get("lon"),
|
||||
"alt": result.get("alt"),
|
||||
"speed": result.get("speed"), # m/s
|
||||
"track": result.get("track"), # heading in degrees
|
||||
"mode": result.get("mode"), # 0=no fix, 2=2D, 3=3D
|
||||
"satellites": result.get("satellites"), # from SKY messages
|
||||
}
|
||||
|
||||
# Compute state and attach to fix
|
||||
fix["gps_state"] = self._gps_state(fix)
|
||||
|
||||
# Check if this is a real fix (has position) or just empty TPV
|
||||
if fix.get("lat") is None and fix.get("mode") in (None, 0, 1):
|
||||
# No real data yet, check timeout
|
||||
if time.time() > fix_timeout:
|
||||
timeout_s = "120s" if not self._has_ever_fixed else "10s"
|
||||
print(f"[GPS] No GPS fix after {timeout_s}, will retry connection")
|
||||
raise ConnectionError("No GPS fix within timeout")
|
||||
continue # Skip empty fixes
|
||||
|
||||
# Got real data — mark first fix, reset timeout to shorter window
|
||||
if not self._has_ever_fixed:
|
||||
self._has_ever_fixed = True
|
||||
print("[GPS] First fix acquired")
|
||||
fix_timeout = time.time() + 10.0 # 10s timeout for signal loss
|
||||
|
||||
with self._lock:
|
||||
self._latest = fix
|
||||
if fix.get("lat") is not None:
|
||||
self._buffer.append(fix)
|
||||
|
||||
# Invoke callback with new fix
|
||||
if self._on_data_callback:
|
||||
self._on_data_callback(fix)
|
||||
|
||||
# Periodic status log (every 5s)
|
||||
self._fix_count += 1
|
||||
now = time.time()
|
||||
if now - self._last_status_log >= 5.0:
|
||||
elapsed = now - self._last_status_log
|
||||
fps = self._fix_count / elapsed
|
||||
speed = fix.get('speed', 0) or 0
|
||||
track = fix.get('track', 0) or 0
|
||||
mode = fix.get('mode', 0) or 0
|
||||
sats = fix.get('satellites', '?')
|
||||
print(f"[GPS] {fps:.1f} fix/s | {speed:.1f}m/s hdg={track:.0f}° mode={mode} sats={sats}")
|
||||
self._last_status_log = now
|
||||
self._fix_count = 0
|
||||
|
||||
def _stub_mode(self):
|
||||
"""Generate realistic mock GPS data for development/testing.
|
||||
|
||||
Simulates:
|
||||
- Initial acquisition delay (~3s before first fix)
|
||||
- Normal 3D fix with satellites
|
||||
- Occasional signal loss (~30% chance per second, lasts ~2s)
|
||||
- Wandering position near Tokyo
|
||||
"""
|
||||
self._last_status_log = time.time()
|
||||
self._fix_count = 0
|
||||
|
||||
# Signal loss state
|
||||
signal_lost = False
|
||||
signal_lost_until = 0.0
|
||||
|
||||
# Simulate cold start acquisition (~3s)
|
||||
acquiring_until = time.time() + 3.0
|
||||
|
||||
# Base position (Tokyo area)
|
||||
base_lat = 35.6762
|
||||
base_lon = 139.6503
|
||||
base_alt = 40.0
|
||||
|
||||
# Smoothly varying heading/speed
|
||||
heading = random.uniform(0, 360)
|
||||
speed = random.uniform(5, 15)
|
||||
|
||||
while self._running:
|
||||
self._connected = True
|
||||
now = time.time()
|
||||
|
||||
# Simulate initial acquisition period
|
||||
if now < acquiring_until:
|
||||
signal_lost = True # No fix yet
|
||||
elif signal_lost and now >= signal_lost_until:
|
||||
signal_lost = False
|
||||
if self._has_ever_fixed:
|
||||
print("[GPS] Signal recovered (stub)")
|
||||
else:
|
||||
print("[GPS] First fix acquired (stub)")
|
||||
elif not signal_lost:
|
||||
# ~30% chance per second to lose signal
|
||||
if random.random() < 0.3:
|
||||
signal_lost = True
|
||||
signal_lost_until = now + 2 # fixed 2s loss
|
||||
print("[GPS] Signal loss simulation (stub)")
|
||||
|
||||
if signal_lost:
|
||||
# No fix - mode 1, no satellites, no track
|
||||
# Note: use None, not float('nan') - NaN doesn't serialize to valid JSON
|
||||
fix = {
|
||||
"time": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"lat": None,
|
||||
"lon": None,
|
||||
"alt": None,
|
||||
"speed": None,
|
||||
"track": None,
|
||||
"mode": 1,
|
||||
"satellites": 0,
|
||||
}
|
||||
else:
|
||||
# Smoothly vary heading and speed
|
||||
heading = (heading + random.uniform(1, 3)) % 360
|
||||
speed = max(0, min(30, speed + random.uniform(-2, 2)))
|
||||
|
||||
if not self._has_ever_fixed:
|
||||
self._has_ever_fixed = True
|
||||
|
||||
fix = {
|
||||
"time": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
|
||||
"lat": base_lat + random.uniform(-0.001, 0.001),
|
||||
"lon": base_lon + random.uniform(-0.001, 0.001),
|
||||
"alt": base_alt + random.uniform(-5, 5),
|
||||
"speed": speed,
|
||||
"track": heading,
|
||||
"mode": 3,
|
||||
"satellites": random.randint(6, 12),
|
||||
}
|
||||
|
||||
# Attach state — same logic as real GPS path
|
||||
fix["gps_state"] = self._gps_state(fix)
|
||||
|
||||
with self._lock:
|
||||
self._latest = fix
|
||||
if fix.get("lat") is not None:
|
||||
self._buffer.append(fix)
|
||||
|
||||
# Invoke callback with new fix
|
||||
if self._on_data_callback:
|
||||
self._on_data_callback(fix)
|
||||
|
||||
# Periodic status log (every 5s)
|
||||
self._fix_count += 1
|
||||
if now - self._last_status_log >= 5.0:
|
||||
elapsed = now - self._last_status_log
|
||||
fps = self._fix_count / elapsed
|
||||
speed_val = fix.get('speed') or 0
|
||||
track_val = fix.get('track')
|
||||
track_str = f"{track_val:.0f}" if track_val is not None else "---"
|
||||
mode = fix.get('mode', 0)
|
||||
sats = fix.get('satellites', 0)
|
||||
print(f"[GPS] {fps:.1f} fix/s | {speed_val:.1f}m/s hdg={track_str} mode={mode} sats={sats} (stub)")
|
||||
self._last_status_log = now
|
||||
self._fix_count = 0
|
||||
|
||||
time.sleep(1)
|
||||
|
||||
Reference in New Issue
Block a user