Poulpe
9a158f5c5f
High-level Python wrapper around the upstream cosma-tech/kogger_acousticAntenna
driver. Configures a Kogger acoustic antenna as a permanent slave transponder
in a single start() call: address filter, echo filter, optional TDMA sync slot,
permanent response window, and Python callbacks for each ping received.
No modification to the upstream driver — only composes existing public methods
in the right order. Snapshot of upstream driver included read-only under driver/
for reference.
Includes:
- transponder_continu.py (302 lines): the wrapper class + CLI
- examples/auv_slave.py (79 lines): usage example with logging
- README.md: design rationale, usage, multi-AUV TDMA, watchdog, hardware wiring
- driver/: snapshot of cosma-tech/kogger_acousticAntenna at commit 1b539f9
('Add index slot for multi pinger', 2025-03-11)
Built for Cosma context (USV master + N AUVs slaves) following the design
conversation in Discord #ping-pong-ping (2026-04-27). See poulpe/ping-pong-ping
on Gitea for the interactive demo of the protocol.
303 lines
11 KiB
Python
303 lines
11 KiB
Python
#!/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())
|