Initial: ContinuousTransponder wrapper for Kogger USBL
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.
This commit is contained in:
302
transponder_continu.py
Normal file
302
transponder_continu.py
Normal file
@@ -0,0 +1,302 @@
|
||||
#!/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())
|
||||
Reference in New Issue
Block a user