feat: Add BT Locate and GPS modes with IRK auto-detection

New modes:
- BT Locate: SAR Bluetooth device location with GPS-tagged signal trail,
  RSSI-based proximity bands, audio alerts, and IRK auto-extraction from
  paired devices (macOS plist / Linux BlueZ)
- GPS: Real-time position tracking with live map, speed, heading, altitude,
  satellite info, and track recording via gpsd

Bug fixes:
- Fix ABBA deadlock between session lock and aggregator lock in BT Locate
- Fix bleak scan lifecycle tracking in BluetoothScanner (is_scanning property
  now cross-checks backend state)
- Fix map tile persistence when switching modes
- Use 15s max_age window for fresh detections in BT Locate poll loop

Documentation:
- Update README, FEATURES.md, USAGE.md, and GitHub Pages with new modes

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Smittix
2026-02-15 21:59:38 +00:00
parent c60769f795
commit d8d08a8b1e
26 changed files with 4481 additions and 510 deletions

View File

@@ -0,0 +1,198 @@
"""
IRK Extractor — Extract Identity Resolving Keys from paired Bluetooth devices.
Supports macOS (com.apple.Bluetooth.plist) and Linux (BlueZ info files).
"""
from __future__ import annotations
import logging
import platform
import time
from pathlib import Path
logger = logging.getLogger('intercept.bt.irk_extractor')
# Cache paired IRKs for 30 seconds to avoid repeated disk reads
_cache: list[dict] | None = None
_cache_time: float = 0
_CACHE_TTL = 30.0
def get_paired_irks() -> list[dict]:
"""Return paired Bluetooth devices that have IRKs.
Each entry is a dict with keys:
- name: Device name (str or None)
- address: Bluetooth address (str)
- irk_hex: 32-char hex string of the 16-byte IRK
- address_type: 'random' or 'public' (str or None)
Results are cached for 30 seconds.
"""
global _cache, _cache_time
now = time.monotonic()
if _cache is not None and (now - _cache_time) < _CACHE_TTL:
return _cache
system = platform.system()
try:
if system == 'Darwin':
results = _extract_macos()
elif system == 'Linux':
results = _extract_linux()
else:
logger.debug(f"IRK extraction not supported on {system}")
results = []
except Exception:
logger.exception("Failed to extract paired IRKs")
results = []
_cache = results
_cache_time = now
return results
def _extract_macos() -> list[dict]:
"""Extract IRKs from macOS Bluetooth plist."""
import plistlib
plist_path = Path('/Library/Preferences/com.apple.Bluetooth.plist')
if not plist_path.exists():
logger.debug("macOS Bluetooth plist not found")
return []
with open(plist_path, 'rb') as f:
plist = plistlib.load(f)
devices = []
cache_data = plist.get('CoreBluetoothCache', {})
# CoreBluetoothCache contains BLE device info including IRKs
for device_uuid, device_info in cache_data.items():
if not isinstance(device_info, dict):
continue
irk = device_info.get('IRK')
if irk is None:
continue
# IRK is stored as bytes (16 bytes)
if isinstance(irk, bytes) and len(irk) == 16:
irk_hex = irk.hex()
elif isinstance(irk, str):
irk_hex = irk.replace('-', '').replace(' ', '')
if len(irk_hex) != 32:
continue
else:
continue
name = device_info.get('Name') or device_info.get('DeviceName')
address = device_info.get('DeviceAddress', device_uuid)
addr_type = 'random' if device_info.get('AddressType', 1) == 1 else 'public'
devices.append({
'name': name,
'address': str(address),
'irk_hex': irk_hex,
'address_type': addr_type,
})
# Also check LEPairedDevices / PairedDevices structures
for section_key in ('LEPairedDevices', 'PairedDevices'):
section = plist.get(section_key, {})
if not isinstance(section, dict):
continue
for addr, dev_info in section.items():
if not isinstance(dev_info, dict):
continue
irk = dev_info.get('IRK') or dev_info.get('IdentityResolvingKey')
if irk is None:
continue
if isinstance(irk, bytes) and len(irk) == 16:
irk_hex = irk.hex()
elif isinstance(irk, str):
irk_hex = irk.replace('-', '').replace(' ', '')
if len(irk_hex) != 32:
continue
else:
continue
# Skip if we already have this IRK
if any(d['irk_hex'] == irk_hex for d in devices):
continue
name = dev_info.get('Name') or dev_info.get('DeviceName')
addr_type = 'random' if dev_info.get('AddressType', 1) == 1 else 'public'
devices.append({
'name': name,
'address': str(addr),
'irk_hex': irk_hex,
'address_type': addr_type,
})
logger.info(f"Extracted {len(devices)} IRK(s) from macOS paired devices")
return devices
def _extract_linux() -> list[dict]:
"""Extract IRKs from Linux BlueZ info files.
BlueZ stores paired device info at:
/var/lib/bluetooth/<adapter_mac>/<device_mac>/info
"""
import configparser
bt_root = Path('/var/lib/bluetooth')
if not bt_root.exists():
logger.debug("BlueZ bluetooth directory not found")
return []
devices = []
for adapter_dir in bt_root.iterdir():
if not adapter_dir.is_dir():
continue
for device_dir in adapter_dir.iterdir():
if not device_dir.is_dir():
continue
info_file = device_dir / 'info'
if not info_file.exists():
continue
config = configparser.ConfigParser()
try:
config.read(str(info_file))
except (configparser.Error, OSError):
continue
if not config.has_section('IdentityResolvingKey'):
continue
irk_hex = config.get('IdentityResolvingKey', 'Key', fallback=None)
if not irk_hex:
continue
# BlueZ stores as hex string, may or may not have separators
irk_hex = irk_hex.replace(' ', '').replace('-', '')
if len(irk_hex) != 32:
continue
name = config.get('General', 'Name', fallback=None)
address = device_dir.name # Directory name is the MAC address
addr_type = config.get('General', 'AddressType', fallback=None)
devices.append({
'name': name,
'address': address,
'irk_hex': irk_hex,
'address_type': addr_type,
})
logger.info(f"Extracted {len(devices)} IRK(s) from BlueZ paired devices")
return devices

View File

@@ -66,7 +66,7 @@ class BluetoothScanner:
self._scan_timer: Optional[threading.Timer] = None
# Callbacks
self._on_device_updated: Optional[Callable[[BTDeviceAggregate], None]] = None
self._on_device_updated_callbacks: list[Callable[[BTDeviceAggregate], None]] = []
# Capability check result
self._capabilities: Optional[SystemCapabilities] = None
@@ -236,9 +236,12 @@ class BluetoothScanner:
'device': device.to_summary_dict(),
})
# Callback
if self._on_device_updated:
self._on_device_updated(device)
# Callbacks
for cb in self._on_device_updated_callbacks:
try:
cb(device)
except Exception as cb_err:
logger.error(f"Device callback error: {cb_err}")
except Exception as e:
logger.error(f"Error handling observation: {e}")
@@ -368,13 +371,39 @@ class BluetoothScanner:
return self._capabilities
def set_on_device_updated(self, callback: Callable[[BTDeviceAggregate], None]) -> None:
"""Set callback for device updates."""
self._on_device_updated = callback
"""Set callback for device updates (legacy, adds to callback list)."""
self.add_device_callback(callback)
def add_device_callback(self, callback: Callable[[BTDeviceAggregate], None]) -> None:
"""Add a callback for device updates."""
if callback not in self._on_device_updated_callbacks:
self._on_device_updated_callbacks.append(callback)
def remove_device_callback(self, callback: Callable[[BTDeviceAggregate], None]) -> None:
"""Remove a device update callback."""
if callback in self._on_device_updated_callbacks:
self._on_device_updated_callbacks.remove(callback)
@property
def is_scanning(self) -> bool:
"""Check if scanning is active."""
return self._status.is_scanning
"""Check if scanning is active.
Cross-checks the backend scanner state, since bleak scans can
expire silently without calling stop_scan().
"""
if not self._status.is_scanning:
return False
# Detect backends that finished on their own (e.g. bleak timeout)
backend_alive = (
(self._dbus_scanner and self._dbus_scanner.is_scanning)
or (self._fallback_scanner and self._fallback_scanner.is_scanning)
)
if not backend_alive:
self._status.is_scanning = False
return False
return True
@property
def device_count(self) -> int:

562
utils/bt_locate.py Normal file
View File

@@ -0,0 +1,562 @@
"""
BT Locate — Bluetooth SAR Device Location System.
Provides GPS-tagged signal trail mapping, RPA resolution, environment-aware
distance estimation, and proximity alerts for search and rescue operations.
"""
from __future__ import annotations
import logging
import queue
import threading
from dataclasses import dataclass
from datetime import datetime
from enum import Enum
from utils.bluetooth.models import BTDeviceAggregate
from utils.bluetooth.scanner import BluetoothScanner, get_bluetooth_scanner
from utils.gps import get_current_position
logger = logging.getLogger('intercept.bt_locate')
# Maximum trail points to retain
MAX_TRAIL_POINTS = 500
# EMA smoothing factor for RSSI
EMA_ALPHA = 0.3
class Environment(Enum):
"""RF propagation environment presets."""
FREE_SPACE = 2.0
OUTDOOR = 2.2
INDOOR = 3.0
CUSTOM = 0.0 # user-provided exponent
def resolve_rpa(irk: bytes, address: str) -> bool:
"""
Resolve a BLE Resolvable Private Address against an Identity Resolving Key.
Implements the Bluetooth Core Spec ah() function using AES-128-ECB.
Args:
irk: 16-byte Identity Resolving Key.
address: BLE address string (e.g. 'AA:BB:CC:DD:EE:FF').
Returns:
True if the address resolves against the IRK.
"""
try:
from cryptography.hazmat.primitives.ciphers import Cipher, algorithms, modes
except ImportError:
logger.error("cryptography package required for RPA resolution")
return False
# Parse address bytes (remove colons, convert to bytes)
addr_bytes = bytes.fromhex(address.replace(':', '').replace('-', ''))
if len(addr_bytes) != 6:
return False
# RPA: upper 2 bits of MSB must be 01 (resolvable)
if (addr_bytes[0] >> 6) != 1:
return False
# prand = upper 3 bytes (MSB first), hash = lower 3 bytes
prand = addr_bytes[0:3]
expected_hash = addr_bytes[3:6]
# ah(k, r) = e(k, r') mod 2^24
# r' is prand zero-padded to 16 bytes (MSB)
plaintext = b'\x00' * 13 + prand
cipher = Cipher(algorithms.AES(irk), modes.ECB())
encryptor = cipher.encryptor()
encrypted = encryptor.update(plaintext) + encryptor.finalize()
# Take last 3 bytes as hash
computed_hash = encrypted[13:16]
return computed_hash == expected_hash
@dataclass
class LocateTarget:
"""Target device specification for locate session."""
mac_address: str | None = None
name_pattern: str | None = None
irk_hex: str | None = None
device_id: str | None = None
# Hand-off metadata from Bluetooth mode
known_name: str | None = None
known_manufacturer: str | None = None
last_known_rssi: int | None = None
def matches(self, device: BTDeviceAggregate) -> bool:
"""Check if a device matches this target."""
# Match by device_id (exact)
if self.device_id and device.device_id == self.device_id:
return True
# Match by device_id address portion (without :address_type suffix)
if self.device_id and ':' in self.device_id:
target_addr_part = self.device_id.rsplit(':', 1)[0].upper()
dev_addr = (device.address or '').upper()
if target_addr_part and dev_addr == target_addr_part:
return True
# Match by MAC/address (case-insensitive, normalize separators)
if self.mac_address:
dev_addr = (device.address or '').upper().replace('-', ':')
target_addr = self.mac_address.upper().replace('-', ':')
if dev_addr == target_addr:
return True
# Match by RPA resolution
if self.irk_hex:
try:
irk = bytes.fromhex(self.irk_hex)
if len(irk) == 16 and device.address and resolve_rpa(irk, device.address):
return True
except (ValueError, TypeError):
pass
# Match by name pattern
if self.name_pattern and device.name and self.name_pattern.lower() in device.name.lower():
return True
# Match by known_name from handoff (exact name match)
return bool(self.known_name and device.name and self.known_name.lower() == device.name.lower())
def to_dict(self) -> dict:
return {
'mac_address': self.mac_address,
'name_pattern': self.name_pattern,
'irk_hex': self.irk_hex,
'device_id': self.device_id,
'known_name': self.known_name,
'known_manufacturer': self.known_manufacturer,
'last_known_rssi': self.last_known_rssi,
}
class DistanceEstimator:
"""Estimate distance from RSSI using log-distance path loss model."""
# Reference RSSI at 1 meter (typical BLE)
RSSI_AT_1M = -59
def __init__(self, path_loss_exponent: float = 2.0, rssi_at_1m: int = -59):
self.n = path_loss_exponent
self.rssi_at_1m = rssi_at_1m
def estimate(self, rssi: int) -> float:
"""Estimate distance in meters from RSSI."""
if rssi >= 0 or self.n <= 0:
return 0.0
return 10 ** ((self.rssi_at_1m - rssi) / (10 * self.n))
@staticmethod
def proximity_band(distance: float) -> str:
"""Classify distance into proximity band."""
if distance <= 1.0:
return 'IMMEDIATE'
elif distance <= 5.0:
return 'NEAR'
else:
return 'FAR'
@dataclass
class DetectionPoint:
"""A single GPS-tagged BLE detection."""
timestamp: str
rssi: int
rssi_ema: float
estimated_distance: float
proximity_band: str
lat: float | None = None
lon: float | None = None
gps_accuracy: float | None = None
rpa_resolved: bool = False
def to_dict(self) -> dict:
return {
'timestamp': self.timestamp,
'rssi': self.rssi,
'rssi_ema': round(self.rssi_ema, 1),
'estimated_distance': round(self.estimated_distance, 2),
'proximity_band': self.proximity_band,
'lat': self.lat,
'lon': self.lon,
'gps_accuracy': self.gps_accuracy,
'rpa_resolved': self.rpa_resolved,
}
class LocateSession:
"""Active locate session tracking a target device."""
def __init__(
self,
target: LocateTarget,
environment: Environment = Environment.OUTDOOR,
custom_exponent: float | None = None,
fallback_lat: float | None = None,
fallback_lon: float | None = None,
):
self.target = target
self.environment = environment
self.fallback_lat = fallback_lat
self.fallback_lon = fallback_lon
self._lock = threading.Lock()
# Distance estimator
n = custom_exponent if environment == Environment.CUSTOM and custom_exponent else environment.value
self.estimator = DistanceEstimator(path_loss_exponent=n)
# Signal trail
self.trail: list[DetectionPoint] = []
# RSSI EMA state
self._rssi_ema: float | None = None
# SSE event queue
self.event_queue: queue.Queue = queue.Queue(maxsize=500)
# Session state
self.active = False
self.started_at: datetime | None = None
self.detection_count = 0
self.last_detection: datetime | None = None
# Debug counters
self.callback_call_count = 0
self.poll_count = 0
self._last_seen_device: str | None = None
# Scanner reference
self._scanner: BluetoothScanner | None = None
self._poll_thread: threading.Thread | None = None
self._stop_event = threading.Event()
# Track last RSSI per device to detect changes
self._last_cb_rssi: dict[str, int] = {} # Dedup for rapid callbacks only
def start(self) -> bool:
"""Start the locate session.
Subscribes to scanner callbacks AND runs a polling thread that
checks the aggregator directly (handles bleak scan timeout).
"""
self._scanner = get_bluetooth_scanner()
self._scanner.add_device_callback(self._on_device)
# Ensure BLE scanning is active
if not self._scanner.is_scanning:
logger.info("BT scanner not running, starting scan for locate session")
self._scanner_started_by_us = True
if not self._scanner.start_scan(mode='auto'):
logger.warning("Failed to start BT scanner for locate session")
else:
self._scanner_started_by_us = False
self.active = True
self.started_at = datetime.now()
self._stop_event.clear()
# Start polling thread as reliable fallback
self._poll_thread = threading.Thread(
target=self._poll_loop, daemon=True, name='bt-locate-poll'
)
self._poll_thread.start()
logger.info(f"Locate session started for target: {self.target.to_dict()}")
return True
def stop(self) -> None:
"""Stop the locate session."""
self.active = False
self._stop_event.set()
if self._scanner:
self._scanner.remove_device_callback(self._on_device)
if getattr(self, '_scanner_started_by_us', False) and self._scanner.is_scanning:
self._scanner.stop_scan()
logger.info("Stopped BT scanner (was started by locate session)")
if self._poll_thread:
self._poll_thread.join(timeout=3.0)
logger.info("Locate session stopped")
def _poll_loop(self) -> None:
"""Poll scanner aggregator for target device updates."""
while not self._stop_event.is_set():
self._stop_event.wait(timeout=1.5)
if self._stop_event.is_set():
break
try:
self._check_aggregator()
except Exception as e:
logger.error(f"Locate poll error: {e}")
def _check_aggregator(self) -> None:
"""Check the scanner's aggregator for the target device."""
if not self._scanner:
return
self.poll_count += 1
# Restart scan if it expired (bleak 10s timeout)
if not self._scanner.is_scanning:
logger.info("Scanner stopped, restarting for locate session")
self._scanner.start_scan(mode='auto')
# Check devices seen within a recent window. Using a short window
# (rather than the aggregator's full 120s) so that once a device
# goes silent its stale RSSI stops producing detections. The window
# must survive bleak's 10s scan cycle + restart gap (~3s).
devices = self._scanner.get_devices(max_age_seconds=15)
found_target = False
for device in devices:
if not self.target.matches(device):
continue
found_target = True
rssi = device.rssi_current
if rssi is None:
continue
self._record_detection(device, rssi)
break # One match per poll cycle is sufficient
# Log periodically for debugging
if self.poll_count % 20 == 0 or (self.poll_count <= 5) or not found_target:
logger.info(
f"Poll #{self.poll_count}: {len(devices)} devices, "
f"target_found={found_target}, "
f"detections={self.detection_count}, "
f"scanning={self._scanner.is_scanning}"
)
def _on_device(self, device: BTDeviceAggregate) -> None:
"""Scanner callback: check if device matches target."""
if not self.active:
return
self.callback_call_count += 1
self._last_seen_device = f"{device.device_id}|{device.name}"
if not self.target.matches(device):
return
rssi = device.rssi_current
if rssi is None:
return
# Dedup rapid callbacks (bleak can fire many times per second)
prev = self._last_cb_rssi.get(device.device_id)
if prev == rssi:
return
self._last_cb_rssi[device.device_id] = rssi
self._record_detection(device, rssi)
def _record_detection(self, device: BTDeviceAggregate, rssi: int) -> None:
"""Record a target detection with GPS tagging."""
logger.info(f"Target detected: {device.address} RSSI={rssi} name={device.name}")
# Update EMA
if self._rssi_ema is None:
self._rssi_ema = float(rssi)
else:
self._rssi_ema = EMA_ALPHA * rssi + (1 - EMA_ALPHA) * self._rssi_ema
# Estimate distance
distance = self.estimator.estimate(rssi)
band = DistanceEstimator.proximity_band(distance)
# Check RPA resolution
rpa_resolved = False
if self.target.irk_hex and device.address:
try:
irk = bytes.fromhex(self.target.irk_hex)
rpa_resolved = resolve_rpa(irk, device.address)
except (ValueError, TypeError):
pass
# GPS tag — prefer live GPS, fall back to user-set coordinates
gps_pos = get_current_position()
lat = gps_pos.latitude if gps_pos else None
lon = gps_pos.longitude if gps_pos else None
gps_acc = None
if gps_pos:
epx = gps_pos.epx or 0
epy = gps_pos.epy or 0
if epx or epy:
gps_acc = round(max(epx, epy), 1)
elif self.fallback_lat is not None and self.fallback_lon is not None:
lat = self.fallback_lat
lon = self.fallback_lon
now = datetime.now()
point = DetectionPoint(
timestamp=now.isoformat(),
rssi=rssi,
rssi_ema=self._rssi_ema,
estimated_distance=distance,
proximity_band=band,
lat=lat,
lon=lon,
gps_accuracy=gps_acc,
rpa_resolved=rpa_resolved,
)
with self._lock:
self.trail.append(point)
if len(self.trail) > MAX_TRAIL_POINTS:
self.trail = self.trail[-MAX_TRAIL_POINTS:]
self.detection_count += 1
self.last_detection = now
# Queue SSE event
event = {
'type': 'detection',
'data': point.to_dict(),
'device_name': device.name,
'device_address': device.address,
}
try:
self.event_queue.put_nowait(event)
except queue.Full:
try:
self.event_queue.get_nowait()
self.event_queue.put_nowait(event)
except queue.Empty:
pass
def get_trail(self) -> list[dict]:
"""Get the full detection trail."""
with self._lock:
return [p.to_dict() for p in self.trail]
def get_gps_trail(self) -> list[dict]:
"""Get only trail points that have GPS coordinates."""
with self._lock:
return [p.to_dict() for p in self.trail if p.lat is not None]
def get_status(self) -> dict:
"""Get session status."""
gps_pos = get_current_position()
# Collect scanner/aggregator data OUTSIDE self._lock to avoid ABBA
# deadlock: get_status would hold self._lock then wait on
# aggregator._lock, while _poll_loop holds aggregator._lock then
# waits on self._lock in _record_detection.
debug_devices = self._debug_device_sample()
scanner_running = self._scanner.is_scanning if self._scanner else False
scanner_device_count = self._scanner.device_count if self._scanner else 0
callback_registered = (
self._on_device in self._scanner._on_device_updated_callbacks
if self._scanner else False
)
with self._lock:
return {
'active': self.active,
'target': self.target.to_dict(),
'environment': self.environment.name,
'path_loss_exponent': self.estimator.n,
'started_at': self.started_at.isoformat() if self.started_at else None,
'detection_count': self.detection_count,
'gps_trail_count': sum(1 for p in self.trail if p.lat is not None),
'last_detection': self.last_detection.isoformat() if self.last_detection else None,
'scanner_running': scanner_running,
'scanner_device_count': scanner_device_count,
'callback_registered': callback_registered,
'event_queue_size': self.event_queue.qsize(),
'callback_call_count': self.callback_call_count,
'poll_count': self.poll_count,
'poll_thread_alive': self._poll_thread.is_alive() if self._poll_thread else False,
'last_seen_device': self._last_seen_device,
'gps_available': gps_pos is not None,
'gps_source': 'live' if gps_pos else (
'manual' if self.fallback_lat is not None else 'none'
),
'fallback_lat': self.fallback_lat,
'fallback_lon': self.fallback_lon,
'latest_rssi': self.trail[-1].rssi if self.trail else None,
'latest_rssi_ema': round(self.trail[-1].rssi_ema, 1) if self.trail else None,
'latest_distance': round(self.trail[-1].estimated_distance, 2) if self.trail else None,
'latest_band': self.trail[-1].proximity_band if self.trail else None,
'debug_devices': debug_devices,
}
def set_environment(self, environment: Environment, custom_exponent: float | None = None) -> None:
"""Update the environment and recalculate distance estimator."""
with self._lock:
self.environment = environment
n = custom_exponent if environment == Environment.CUSTOM and custom_exponent else environment.value
self.estimator = DistanceEstimator(path_loss_exponent=n)
def _debug_device_sample(self) -> list[dict]:
"""Return a sample of scanner devices for debugging matching issues."""
if not self._scanner:
return []
try:
devices = self._scanner.get_devices(max_age_seconds=30)
return [
{
'id': d.device_id,
'addr': d.address,
'name': d.name,
'rssi': d.rssi_current,
'match': self.target.matches(d),
}
for d in devices[:8]
]
except Exception:
return []
def clear_trail(self) -> None:
"""Clear the detection trail."""
with self._lock:
self.trail.clear()
self.detection_count = 0
# Module-level session management (single active session)
_session: LocateSession | None = None
_session_lock = threading.Lock()
def start_locate_session(
target: LocateTarget,
environment: Environment = Environment.OUTDOOR,
custom_exponent: float | None = None,
fallback_lat: float | None = None,
fallback_lon: float | None = None,
) -> LocateSession:
"""Start a new locate session, stopping any existing one."""
global _session
with _session_lock:
if _session and _session.active:
_session.stop()
_session = LocateSession(
target, environment, custom_exponent, fallback_lat, fallback_lon
)
_session.start()
return _session
def stop_locate_session() -> None:
"""Stop the active locate session."""
global _session
with _session_lock:
if _session:
_session.stop()
_session = None
def get_locate_session() -> LocateSession | None:
"""Get the current locate session (if any)."""
with _session_lock:
return _session

View File

@@ -6,28 +6,86 @@ Provides GPS location data by connecting to the gpsd daemon.
from __future__ import annotations
import contextlib
import logging
import socket as _socket_mod
import threading
import time
from dataclasses import dataclass
from dataclasses import dataclass, field
from datetime import datetime
from typing import Optional, Callable
from typing import Callable
logger = logging.getLogger('intercept.gps')
@dataclass
class GPSSatellite:
"""Individual satellite data from gpsd SKY message."""
prn: int
elevation: float | None = None # degrees
azimuth: float | None = None # degrees
snr: float | None = None # dB-Hz
used: bool = False
constellation: str = 'GPS' # GPS, GLONASS, Galileo, BeiDou, SBAS, QZSS
def to_dict(self) -> dict:
return {
'prn': self.prn,
'elevation': self.elevation,
'azimuth': self.azimuth,
'snr': self.snr,
'used': self.used,
'constellation': self.constellation,
}
@dataclass
class GPSSkyData:
"""Sky view data from gpsd SKY message."""
satellites: list[GPSSatellite] = field(default_factory=list)
hdop: float | None = None
vdop: float | None = None
pdop: float | None = None
tdop: float | None = None
gdop: float | None = None
xdop: float | None = None
ydop: float | None = None
nsat: int = 0 # total visible
usat: int = 0 # total used
def to_dict(self) -> dict:
return {
'satellites': [s.to_dict() for s in self.satellites],
'hdop': self.hdop,
'vdop': self.vdop,
'pdop': self.pdop,
'tdop': self.tdop,
'gdop': self.gdop,
'xdop': self.xdop,
'ydop': self.ydop,
'nsat': self.nsat,
'usat': self.usat,
}
@dataclass
class GPSPosition:
"""GPS position data."""
latitude: float
longitude: float
altitude: Optional[float] = None
speed: Optional[float] = None # m/s
heading: Optional[float] = None # degrees
satellites: Optional[int] = None
altitude: float | None = None
speed: float | None = None # m/s
heading: float | None = None # degrees
climb: float | None = None # m/s vertical speed
satellites: int | None = None
fix_quality: int = 0 # 0=unknown, 1=no fix, 2=2D fix, 3=3D fix
timestamp: Optional[datetime] = None
device: Optional[str] = None
timestamp: datetime | None = None
device: str | None = None
# Error estimates
epx: float | None = None # lon error (m)
epy: float | None = None # lat error (m)
epv: float | None = None # vertical error (m)
eps: float | None = None # speed error (m/s)
ept: float | None = None # time error (s)
def to_dict(self) -> dict:
"""Convert to dictionary for JSON serialization."""
@@ -37,13 +95,45 @@ class GPSPosition:
'altitude': self.altitude,
'speed': self.speed,
'heading': self.heading,
'climb': self.climb,
'satellites': self.satellites,
'fix_quality': self.fix_quality,
'timestamp': self.timestamp.isoformat() if self.timestamp else None,
'device': self.device,
'epx': self.epx,
'epy': self.epy,
'epv': self.epv,
'eps': self.eps,
'ept': self.ept,
}
def _classify_constellation(prn: int, gnssid: int | None = None) -> str:
"""Classify satellite constellation from PRN or gnssid."""
if gnssid is not None:
mapping = {
0: 'GPS', 1: 'SBAS', 2: 'Galileo', 3: 'BeiDou',
4: 'IMES', 5: 'QZSS', 6: 'GLONASS', 7: 'NavIC',
}
return mapping.get(gnssid, 'GPS')
# Fall back to PRN range heuristic
if 1 <= prn <= 32:
return 'GPS'
elif 33 <= prn <= 64:
return 'SBAS'
elif 65 <= prn <= 96:
return 'GLONASS'
elif 120 <= prn <= 158:
return 'SBAS'
elif 201 <= prn <= 264:
return 'BeiDou'
elif 301 <= prn <= 336:
return 'Galileo'
elif 193 <= prn <= 200:
return 'QZSS'
return 'GPS'
class GPSDClient:
"""
Connects to gpsd daemon for GPS data.
@@ -58,35 +148,43 @@ class GPSDClient:
def __init__(self, host: str = DEFAULT_HOST, port: int = DEFAULT_PORT):
self.host = host
self.port = port
self._position: Optional[GPSPosition] = None
self._position: GPSPosition | None = None
self._sky: GPSSkyData | None = None
self._lock = threading.Lock()
self._running = False
self._thread: Optional[threading.Thread] = None
self._socket: Optional['socket.socket'] = None
self._last_update: Optional[datetime] = None
self._error: Optional[str] = None
self._thread: threading.Thread | None = None
self._socket: _socket_mod.socket | None = None
self._last_update: datetime | None = None
self._error: str | None = None
self._callbacks: list[Callable[[GPSPosition], None]] = []
self._device: Optional[str] = None
self._sky_callbacks: list[Callable[[GPSSkyData], None]] = []
self._device: str | None = None
@property
def position(self) -> Optional[GPSPosition]:
def position(self) -> GPSPosition | None:
"""Get the current GPS position."""
with self._lock:
return self._position
@property
def sky(self) -> GPSSkyData | None:
"""Get the current sky view data."""
with self._lock:
return self._sky
@property
def is_running(self) -> bool:
"""Check if the client is running."""
return self._running
@property
def last_update(self) -> Optional[datetime]:
def last_update(self) -> datetime | None:
"""Get the time of the last position update."""
with self._lock:
return self._last_update
@property
def error(self) -> Optional[str]:
def error(self) -> str | None:
"""Get any error message."""
with self._lock:
return self._error
@@ -105,6 +203,15 @@ class GPSDClient:
if callback in self._callbacks:
self._callbacks.remove(callback)
def add_sky_callback(self, callback: Callable[[GPSSkyData], None]) -> None:
"""Add a callback to be called on sky data updates."""
self._sky_callbacks.append(callback)
def remove_sky_callback(self, callback: Callable[[GPSSkyData], None]) -> None:
"""Remove a sky data update callback."""
if callback in self._sky_callbacks:
self._sky_callbacks.remove(callback)
def start(self) -> bool:
"""Start receiving GPS data from gpsd."""
import socket
@@ -135,10 +242,8 @@ class GPSDClient:
self._error = str(e)
logger.error(f"Failed to connect to gpsd at {self.host}:{self.port}: {e}")
if self._socket:
try:
with contextlib.suppress(Exception):
self._socket.close()
except Exception:
pass
self._socket = None
return False
@@ -169,7 +274,7 @@ class GPSDClient:
buffer = ""
message_count = 0
print(f"[GPS] gpsd read loop started", flush=True)
print("[GPS] gpsd read loop started", flush=True)
while self._running and self._socket:
try:
@@ -202,6 +307,8 @@ class GPSDClient:
if msg_class == 'TPV':
self._handle_tpv(msg)
elif msg_class == 'SKY':
self._handle_sky(msg)
elif msg_class == 'DEVICES':
# Track connected device
devices = msg.get('devices', [])
@@ -239,11 +346,9 @@ class GPSDClient:
timestamp = None
time_str = msg.get('time')
if time_str:
try:
with contextlib.suppress(ValueError, AttributeError):
# gpsd uses ISO format: 2024-01-01T12:00:00.000Z
timestamp = datetime.fromisoformat(time_str.replace('Z', '+00:00'))
except (ValueError, AttributeError):
pass
position = GPSPosition(
latitude=lat,
@@ -251,14 +356,58 @@ class GPSDClient:
altitude=msg.get('alt'),
speed=msg.get('speed'), # m/s in gpsd
heading=msg.get('track'),
climb=msg.get('climb'),
fix_quality=mode,
timestamp=timestamp,
device=self._device or f"gpsd://{self.host}:{self.port}",
epx=msg.get('epx'),
epy=msg.get('epy'),
epv=msg.get('epv'),
eps=msg.get('eps'),
ept=msg.get('ept'),
)
print(f"[GPS] gpsd FIX: {lat:.6f}, {lon:.6f} (mode: {mode})", flush=True)
self._update_position(position)
def _handle_sky(self, msg: dict) -> None:
"""Handle SKY (satellite sky view) message from gpsd."""
sats = []
for sat in msg.get('satellites', []):
prn = sat.get('PRN', 0)
gnssid = sat.get('gnssid')
sats.append(GPSSatellite(
prn=prn,
elevation=sat.get('el'),
azimuth=sat.get('az'),
snr=sat.get('ss'),
used=sat.get('used', False),
constellation=_classify_constellation(prn, gnssid),
))
sky_data = GPSSkyData(
satellites=sats,
hdop=msg.get('hdop'),
vdop=msg.get('vdop'),
pdop=msg.get('pdop'),
tdop=msg.get('tdop'),
gdop=msg.get('gdop'),
xdop=msg.get('xdop'),
ydop=msg.get('ydop'),
nsat=len(sats),
usat=sum(1 for s in sats if s.used),
)
with self._lock:
self._sky = sky_data
# Notify sky callbacks
for callback in self._sky_callbacks:
try:
callback(sky_data)
except Exception as e:
logger.error(f"GPS sky callback error: {e}")
def _update_position(self, position: GPSPosition) -> None:
"""Update the current position and notify callbacks."""
with self._lock:
@@ -275,18 +424,19 @@ class GPSDClient:
# Global GPS client instance
_gps_client: Optional[GPSDClient] = None
_gps_client: GPSDClient | None = None
_gps_lock = threading.Lock()
def get_gps_reader() -> Optional[GPSDClient]:
def get_gps_reader() -> GPSDClient | None:
"""Get the global GPS client instance."""
with _gps_lock:
return _gps_client
def start_gpsd(host: str = 'localhost', port: int = 2947,
callback: Optional[Callable[[GPSPosition], None]] = None) -> bool:
callback: Callable[[GPSPosition], None] | None = None,
sky_callback: Callable[[GPSSkyData], None] | None = None) -> bool:
"""
Start the global GPS client connected to gpsd.
@@ -294,6 +444,7 @@ def start_gpsd(host: str = 'localhost', port: int = 2947,
host: gpsd host (default localhost)
port: gpsd port (default 2947)
callback: Optional callback for position updates
sky_callback: Optional callback for sky data updates
Returns:
True if started successfully
@@ -307,9 +458,11 @@ def start_gpsd(host: str = 'localhost', port: int = 2947,
_gps_client = GPSDClient(host, port)
# Register callback BEFORE starting to avoid race condition
# Register callbacks BEFORE starting to avoid race condition
if callback:
_gps_client.add_callback(callback)
if sky_callback:
_gps_client.add_sky_callback(sky_callback)
return _gps_client.start()
@@ -324,7 +477,7 @@ def stop_gps() -> None:
_gps_client = None
def get_current_position() -> Optional[GPSPosition]:
def get_current_position() -> GPSPosition | None:
"""Get the current GPS position from the global client."""
client = get_gps_reader()
if client: