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:
212
driver/simulation_kogger.py
Normal file
212
driver/simulation_kogger.py
Normal file
@@ -0,0 +1,212 @@
|
||||
#!/usr/bin/env python
|
||||
from loguru import logger
|
||||
import os
|
||||
import time
|
||||
import csv
|
||||
from datetime import datetime
|
||||
import struct
|
||||
|
||||
# 0:Nothing, 1:Only important, 2:All
|
||||
PRINT_LEVEL = 1
|
||||
|
||||
# PATH File for kogger log
|
||||
# AUV log
|
||||
#KOGGER_FILE = "2025-09-01_11-40-00_kogger_log_auv.csv"
|
||||
# USV log
|
||||
KOGGER_FILE = "2025-09-01_11-40-02_kogger_log_usv.csv"
|
||||
|
||||
# --- Frame parsing logic for simulation flexibility ---
|
||||
SYNC1 = 0xBB
|
||||
SYNC2 = 0x55
|
||||
ID_USBL_CONTROL = 0x68
|
||||
TYPE_SETTING = 2
|
||||
|
||||
def _parse_sim_frame(frame_bytes):
|
||||
"""A simple parser for simulation matching flexibility."""
|
||||
if not isinstance(frame_bytes, bytes) or len(frame_bytes) < 8:
|
||||
return None
|
||||
if frame_bytes[0] != SYNC1 or frame_bytes[1] != SYNC2:
|
||||
return None
|
||||
|
||||
try:
|
||||
s1, s2, route, mode, cmd_id, length = struct.unpack_from('<BBBBBB', frame_bytes, 0)
|
||||
|
||||
msg_type = mode & 0x03
|
||||
version = (mode >> 3) & 0x07
|
||||
|
||||
return {
|
||||
'id': cmd_id,
|
||||
'type': msg_type,
|
||||
'version': version,
|
||||
'payload': frame_bytes[6:6+length]
|
||||
}
|
||||
except (struct.error, IndexError):
|
||||
return None
|
||||
# --- End of parsing logic ---
|
||||
|
||||
|
||||
class simu_serial():
|
||||
is_open = True
|
||||
name = ""
|
||||
log_messages = []
|
||||
log_idx = 0
|
||||
in_waiting = 1
|
||||
kogger_next = None
|
||||
received_write = 0
|
||||
start_sim_time = None
|
||||
start_log_time = None
|
||||
|
||||
def _print(text, level):
|
||||
if level <= PRINT_LEVEL:
|
||||
logger.debug(text)
|
||||
|
||||
def _parse_and_merge_csv(filename):
|
||||
messages = []
|
||||
script_dir = os.path.dirname(__file__)
|
||||
file_path = os.path.join(script_dir, filename)
|
||||
if not os.path.exists(file_path):
|
||||
# Fallback to current dir if not found next to script
|
||||
file_path = filename
|
||||
|
||||
with open(file_path, 'r') as f:
|
||||
reader = csv.reader(f)
|
||||
for row in reader:
|
||||
ts_str, msg_type, msg_content = row
|
||||
timestamp = datetime.strptime(ts_str, '%Y-%m-%d %H:%M:%S.%f')
|
||||
try:
|
||||
byte_msg = eval(msg_content)
|
||||
except Exception as e:
|
||||
simu_serial._print(f"Could not eval message content: {msg_content}, error: {e}", 1)
|
||||
byte_msg = msg_content.encode()
|
||||
messages.append({'timestamp': timestamp, 'type': msg_type, 'msg': byte_msg})
|
||||
|
||||
merged = []
|
||||
i = 0
|
||||
while i < len(messages):
|
||||
msg = messages[i]
|
||||
if msg['type'] == 'RECEIVED' and msg['msg'] == b'\xbb':
|
||||
full_msg = b'\xbb'
|
||||
ts = msg['timestamp']
|
||||
i += 1
|
||||
while i < len(messages) and messages[i]['type'] == 'RECEIVED' and messages[i]['msg'] != b'\xbb':
|
||||
full_msg += messages[i]['msg']
|
||||
i += 1
|
||||
merged.append({'timestamp': ts, 'type': 'RECEIVED', 'msg': full_msg})
|
||||
else:
|
||||
merged.append(msg)
|
||||
i += 1
|
||||
return merged
|
||||
|
||||
def Serial(port, baudrate=921600, timeout=1):
|
||||
logger.error("!!!!!!Init kogger in simulation mode!!!!!!!")
|
||||
simu_serial.name = port
|
||||
simu_serial.log_messages = simu_serial._parse_and_merge_csv(KOGGER_FILE)
|
||||
if not simu_serial.log_messages:
|
||||
logger.error(f"No messages loaded from {KOGGER_FILE}")
|
||||
return simu_serial
|
||||
|
||||
simu_serial.log_idx = 0
|
||||
simu_serial.start_sim_time = time.time()
|
||||
simu_serial.start_log_time = simu_serial.log_messages[0]['timestamp']
|
||||
|
||||
return simu_serial
|
||||
|
||||
def close():
|
||||
simu_serial._print("Close kogger", 1)
|
||||
return True
|
||||
|
||||
def read(size=1):
|
||||
if simu_serial.received_write == 1:
|
||||
line = simu_serial.kogger_next
|
||||
simu_serial.kogger_next = None
|
||||
simu_serial.received_write = 0
|
||||
simu_serial._print(f"read (from write): {line}", 2)
|
||||
return line
|
||||
|
||||
if simu_serial.log_idx >= len(simu_serial.log_messages):
|
||||
simu_serial._print("End of log file.", 1)
|
||||
time.sleep(1)
|
||||
return b''
|
||||
|
||||
next_msg = simu_serial.log_messages[simu_serial.log_idx]
|
||||
|
||||
log_time_elapsed = (next_msg['timestamp'] - simu_serial.start_log_time).total_seconds()
|
||||
sim_time_elapsed = time.time() - simu_serial.start_sim_time
|
||||
|
||||
if sim_time_elapsed < log_time_elapsed:
|
||||
sleep_duration = log_time_elapsed - sim_time_elapsed
|
||||
if sleep_duration > 0:
|
||||
time.sleep(sleep_duration)
|
||||
|
||||
# Re-check current message as time has passed
|
||||
if simu_serial.log_idx >= len(simu_serial.log_messages):
|
||||
return b''
|
||||
|
||||
next_msg = simu_serial.log_messages[simu_serial.log_idx]
|
||||
if next_msg['type'] == 'RECEIVED':
|
||||
simu_serial.log_idx += 1
|
||||
simu_serial._print(f"read (unsolicited): {next_msg['msg']}", 2)
|
||||
return next_msg['msg']
|
||||
else: # SENT
|
||||
return b''
|
||||
|
||||
def write(text):
|
||||
simu_serial._print(f"Write: {text}", 2)
|
||||
incoming_frame_info = _parse_sim_frame(text)
|
||||
search_idx = simu_serial.log_idx
|
||||
while search_idx < len(simu_serial.log_messages):
|
||||
log_entry = simu_serial.log_messages[search_idx]
|
||||
if log_entry['type'] == 'SENT':
|
||||
|
||||
match = False
|
||||
# Check for flexible match on acoustic ping (USBL_CONTROL with state)
|
||||
if (incoming_frame_info and
|
||||
incoming_frame_info['id'] == ID_USBL_CONTROL and
|
||||
incoming_frame_info['type'] == TYPE_SETTING and
|
||||
incoming_frame_info['version'] == 1):
|
||||
|
||||
log_frame_info = _parse_sim_frame(log_entry['msg'])
|
||||
if (log_frame_info and
|
||||
log_frame_info['id'] == ID_USBL_CONTROL and
|
||||
log_frame_info['type'] == TYPE_SETTING and
|
||||
log_frame_info['version'] == 1):
|
||||
|
||||
simu_serial._print(f"Flexible match for USBL_CONTROL message. Incoming: {text.hex()}, Logged: {log_entry['msg'].hex()}", 2)
|
||||
match = True
|
||||
|
||||
# Fallback to exact match for all other messages
|
||||
if not match and log_entry['msg'] == text:
|
||||
match = True
|
||||
|
||||
if match:
|
||||
# Sync time
|
||||
log_time_elapsed = (log_entry['timestamp'] - simu_serial.start_log_time).total_seconds()
|
||||
simu_serial.start_sim_time = time.time() - log_time_elapsed
|
||||
|
||||
response_idx = search_idx + 1
|
||||
while response_idx < len(simu_serial.log_messages):
|
||||
if simu_serial.log_messages[response_idx]['type'] == 'RECEIVED':
|
||||
simu_serial.kogger_next = simu_serial.log_messages[response_idx]['msg']
|
||||
simu_serial.received_write = 1
|
||||
|
||||
# Advance log time to response time
|
||||
log_time_elapsed_resp = (simu_serial.log_messages[response_idx]['timestamp'] - simu_serial.start_log_time).total_seconds()
|
||||
sim_time_elapsed = time.time() - simu_serial.start_sim_time
|
||||
if sim_time_elapsed < log_time_elapsed_resp:
|
||||
sleep_duration = log_time_elapsed_resp - sim_time_elapsed
|
||||
if sleep_duration > 0:
|
||||
time.sleep(sleep_duration)
|
||||
|
||||
simu_serial.log_idx = response_idx + 1
|
||||
return
|
||||
response_idx += 1
|
||||
|
||||
logger.warning(f"No RECEIVED message found after SENT: {text}")
|
||||
simu_serial.log_idx = search_idx + 1
|
||||
return
|
||||
search_idx += 1
|
||||
|
||||
logger.error(f"Unexpected SENT message: {text} from index {simu_serial.log_idx}")
|
||||
|
||||
def reset_output_buffer():
|
||||
pass
|
||||
Reference in New Issue
Block a user