#!/usr/bin/env python3 """ Kogger USBL — mode transpondeur continu. Wrapper de haut niveau autour de `kogger_protocol_driver.KoggerSBPDevice` qui configure l'antenne acoustique Kogger comme un *slave transpondeur permanent* : elle écoute en continu les pings adressés à son ID, répond automatiquement (turnaround géré côté hardware Kogger), ignore les pings destinés aux autres, et expose un callback Python pour chaque ping reçu. Pas de re-armement par ping. Pas de boucle d'interrogation. Une seule init, puis le device répond tant que le process tourne. Usage minimal : from transponder_continu import ContinuousTransponder t = ContinuousTransponder(port="/dev/ttyUSB0", my_address=2, vehicle_name="AUV-2") t.on_ping_received(lambda solution: print("ping →", solution)) t.start() try: t.run_forever() # bloque le main thread finally: t.stop() CLI : python3 transponder_continu.py --port /dev/ttyUSB0 --address 2 Conception : - Aucune modification du driver Kogger d'origine. - N'utilise que des méthodes publiques déjà présentes (`set_usbl_transponder`, `set_usbl_request_address_filter`, `set_usbl_monitor_config`, `set_sync_mode`, `register_callback`, `set_auto_response_filter`). - Compose les bons appels dans le bon ordre, en exposant une API "1-shot config". - Watchdog optionnel : ré-applique la config si le device a rebooté (détecté par perte de réponse > `watchdog_timeout_s`). Auteur : Poulpe (2026-04-27) pour Flag. Licence : même que driver upstream (cosma-tech/kogger_acousticAntenna). """ from __future__ import annotations import argparse import sys import threading import time from dataclasses import dataclass, field from typing import Callable, Optional # Le driver upstream est attendu adjacent (cf. README pour le clone). sys.path.insert(0, "driver") from kogger_protocol_driver import KoggerSBPDevice, ID_USBL_SOLUTION, ID_DIST @dataclass class SyncSlot: """Configuration TDMA optionnelle si plusieurs slaves se partagent le canal.""" slot_total: int # nombre total de slots dans un cycle slot_index: int # mon index de slot (0-indexed) slot_duration: float # durée d'un slot en secondes @dataclass class TransponderState: last_ping_at: float = 0.0 last_distance_m: Optional[float] = None last_snr: Optional[float] = None last_ping_id: Optional[int] = None pings_received: int = 0 class ContinuousTransponder: """ Configure une antenne Kogger en transpondeur permanent sur une adresse donnée. Args ---- port : port série (ex. "/dev/ttyUSB0") my_address : ID de cette antenne, 1..7 (0 = promiscuous) baudrate : baudrate UART, défaut 921600 (recommandé Kogger) vehicle_name : étiquette utilisée dans les logs CSV echo_filter_us : suppression des échos en µs, défaut 400 ms sync : SyncSlot ou None. Si fourni, le device attend son slot. watchdog_timeout_s: si > 0, ré-applique la config en cas de silence prolongé. Notes ----- Adresse Kogger autorisées en transpondeur : 1..7. La valeur 0 est promiscuous (répond à tous), généralement à éviter en multi-AUV. La valeur 0xFF = "slot désactivé" dans le filtre 8-byte. Côté master : envoyez `set_usbl_ping_request_direct(address=N, cmd_id=K, ...)` où N est l'adresse de ce transpondeur. Le hardware Kogger gère automatiquement le pong (turnaround_B) — vous n'avez rien à faire côté slave une fois `start()` appelé. """ def __init__( self, port: str, my_address: int, baudrate: int = 921600, vehicle_name: Optional[str] = None, echo_filter_us: int = 400_000, sync: Optional[SyncSlot] = None, watchdog_timeout_s: float = 0.0, log_level: str = "INFO", log_file: str = "", ): if not 1 <= my_address <= 7: raise ValueError(f"my_address must be 1..7, got {my_address}") self._device = KoggerSBPDevice( port=port, baudrate=baudrate, device_address=my_address, vehicleName=vehicle_name, log_level=log_level, log_file=log_file, ) self._my_address = my_address self._echo_filter_us = echo_filter_us self._sync = sync self._watchdog_timeout_s = watchdog_timeout_s self.state = TransponderState() self._on_ping: Optional[Callable] = None self._on_distance: Optional[Callable] = None self._stop_event = threading.Event() self._watchdog_thread: Optional[threading.Thread] = None # ------------------------------------------------------------------ public def on_ping_received(self, fn: Callable[[dict], None]) -> None: """Callback (message: dict) appelé pour chaque USBL_SOLUTION reçu.""" self._on_ping = fn def on_distance(self, fn: Callable[[dict], None]) -> None: """Callback (message: dict) appelé pour chaque ID_DIST reçu.""" self._on_distance = fn def start(self) -> None: """Connecte le device, applique la configuration transpondeur continu.""" self._device.connect() # 1. Filtre d'adresses : seules les requêtes pour my_address déclenchent une # réponse. Les 7 autres slots sont 0xFF (désactivé). addresses = [self._my_address] + [0xFF] * 7 self._device.set_usbl_request_address_filter(addresses[:8]) # 2. Filtre d'écho actif (évite de répondre à son propre écho réverbéré). self._device.set_usbl_monitor_config( enable=True, echo_filter_response_us=self._echo_filter_us, echo_filter_request_us=self._echo_filter_us, ) # 3. Sync slot TDMA optionnel (utile en multi-slave pour éviter collisions). if self._sync is not None: self._device.set_sync_mode( slot_total=self._sync.slot_total, slot_index=self._sync.slot_index, slot_duration=self._sync.slot_duration, enable_delay=False, # False côté slave (le master ordonne) ) # 4. Active la fenêtre de réponse permanente (timeout = 0xFFFFFFFF µs). self._device.set_usbl_transponder(enable=True) # 5. Branche les callbacks Python sur les frames asynchrones. self._device.register_callback(ID_USBL_SOLUTION, self._handle_usbl_solution) self._device.register_callback(ID_DIST, self._handle_distance) # 6. Watchdog optionnel. if self._watchdog_timeout_s > 0: self._watchdog_thread = threading.Thread( target=self._watchdog_loop, daemon=True, name="kogger-watchdog" ) self._watchdog_thread.start() def stop(self) -> None: """Désactive le transpondeur, déconnecte proprement.""" self._stop_event.set() try: self._device.set_usbl_transponder(enable=False) except Exception: pass # le device peut déjà être déconnecté self._device.unregister_callback(ID_USBL_SOLUTION) self._device.unregister_callback(ID_DIST) self._device.disconnect() def run_forever(self, tick_s: float = 0.5) -> None: """Bloque le main thread jusqu'à `stop()` ou KeyboardInterrupt.""" try: while not self._stop_event.is_set(): time.sleep(tick_s) except KeyboardInterrupt: pass # ------------------------------------------------------------------ internals def _handle_usbl_solution(self, message: dict) -> None: self.state.last_ping_at = time.time() self.state.pings_received += 1 self.state.last_ping_id = message.get("id") self.state.last_snr = message.get("snr") # Distance hardware-computed côté antenne, exposée dans l'USBL solution. for k in ("distance_m", "slant_range_m", "distance"): if k in message: self.state.last_distance_m = message[k] break if self._on_ping: try: self._on_ping(message) except Exception as e: # On n'interrompt jamais le reader thread du driver pour une # exception applicative. print(f"[ContinuousTransponder] on_ping callback error: {e}") def _handle_distance(self, message: dict) -> None: if self._on_distance: try: self._on_distance(message) except Exception as e: print(f"[ContinuousTransponder] on_distance callback error: {e}") def _watchdog_loop(self) -> None: """Si on n'a rien reçu depuis `watchdog_timeout_s`, ré-applique la config.""" while not self._stop_event.wait(self._watchdog_timeout_s / 2): silent_for = time.time() - self.state.last_ping_at if ( self.state.last_ping_at > 0 and silent_for > self._watchdog_timeout_s ): print( f"[ContinuousTransponder] watchdog: silent {silent_for:.1f}s, " f"re-applying config" ) try: self._device.set_usbl_transponder(enable=True) except Exception as e: print(f"[ContinuousTransponder] watchdog re-arm failed: {e}") # ----------------------------------------------------------------------- CLI def _main() -> int: p = argparse.ArgumentParser(description="Kogger USBL — transpondeur continu") p.add_argument("--port", required=True, help="Port série (ex. /dev/ttyUSB0)") p.add_argument("--address", type=int, required=True, help="Mon ID Kogger (1..7)") p.add_argument("--baudrate", type=int, default=921600) p.add_argument("--vehicle", default=None, help="Étiquette véhicule (logs CSV)") p.add_argument("--echo-filter-us", type=int, default=400_000) p.add_argument("--watchdog-s", type=float, default=0.0, help="Re-arm si silence > X secondes (0 = off)") p.add_argument("--slot-total", type=int, default=0) p.add_argument("--slot-index", type=int, default=0) p.add_argument("--slot-duration", type=float, default=0.0) p.add_argument("--log-level", default="INFO") args = p.parse_args() sync = None if args.slot_total > 0 and args.slot_duration > 0: sync = SyncSlot(args.slot_total, args.slot_index, args.slot_duration) t = ContinuousTransponder( port=args.port, my_address=args.address, baudrate=args.baudrate, vehicle_name=args.vehicle, echo_filter_us=args.echo_filter_us, sync=sync, watchdog_timeout_s=args.watchdog_s, log_level=args.log_level, ) def on_ping(msg): d = t.state.last_distance_m d_str = f"{d:.2f} m" if d is not None else "—" print( f"ping #{t.state.pings_received} from id={msg.get('id')} " f"snr={msg.get('snr')} dist={d_str}" ) t.on_ping_received(on_ping) print(f"Starting transponder address={args.address} on {args.port} (Ctrl-C pour arrêter)") t.start() try: t.run_forever() finally: t.stop() print(f"Stopped. Total pings received: {t.state.pings_received}") return 0 if __name__ == "__main__": sys.exit(_main())