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:
276
driver/log_to_human.py
Executable file
276
driver/log_to_human.py
Executable file
@@ -0,0 +1,276 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
A script to parse and convert Kogger SB protocol logs into a human-readable format.
|
||||
"""
|
||||
|
||||
import csv
|
||||
import struct
|
||||
import sys
|
||||
import ast
|
||||
import os
|
||||
|
||||
# Based on the Kogger SB protocol specification PDF and user-provided details.
|
||||
|
||||
# (From PDF page 5 and others)
|
||||
ID_MAP = {
|
||||
0x01: "ID_TIMESTAMP",
|
||||
0x02: "ID_DIST",
|
||||
0x03: "ID_CHART",
|
||||
0x04: "ID_ATTITUDE",
|
||||
0x05: "ID_TEMP",
|
||||
0x10: "ID_DATASET",
|
||||
0x11: "ID_DIST_SETUP",
|
||||
0x12: "ID_CHART_SETUP",
|
||||
0x14: "ID_TRANSC",
|
||||
0x15: "ID_SND_SPD",
|
||||
0x18: "ID_UART",
|
||||
0x1B: "ID_IMU_SETUP",
|
||||
0x20: "ID_VERSION",
|
||||
0x21: "ID_MARK",
|
||||
0x22: "ID_DIAG",
|
||||
0x23: "ID_FLASH",
|
||||
0x24: "ID_BOOT",
|
||||
0x25: "ID_UPDATE",
|
||||
0x64: "ID_NAV",
|
||||
0x65: "ID_USBL_SOLUTION",
|
||||
0x66: "ID_SIGNAL_ENCODER",
|
||||
0x67: "ID_SIGNAL_DECODER",
|
||||
0x68: "ID_USBL_CONTROL",
|
||||
0x79: "ID_DVL_VEL",
|
||||
# Some IDs have aliases
|
||||
102: "ID_SIGNAL_ENCODER",
|
||||
103: "ID_SIGNAL_DECODER",
|
||||
121: "ID_DVL_VEL",
|
||||
}
|
||||
|
||||
def parse_route(route_byte):
|
||||
"""Parses the ROUTE byte."""
|
||||
dev_address = route_byte & 0b1111
|
||||
return {"dev_address": dev_address}
|
||||
|
||||
def parse_mode(mode_byte):
|
||||
"""Parses the MODE byte."""
|
||||
type_val = mode_byte & 0b11
|
||||
type_map = {
|
||||
0: "Reserved",
|
||||
1: "CON:DEV→HST",
|
||||
2: "SET:HST→DEV",
|
||||
3: "GET:HST→DEV",
|
||||
}
|
||||
version = (mode_byte >> 3) & 0b111
|
||||
mark = (mode_byte >> 6) & 0b1
|
||||
response = (mode_byte >> 7) & 0b1
|
||||
return {
|
||||
"type": type_map.get(type_val, "Unknown"),
|
||||
"type_val": type_val,
|
||||
"version": version,
|
||||
"mark": mark,
|
||||
"response": response,
|
||||
}
|
||||
|
||||
def parse_payload(msg_id, mode, payload):
|
||||
"""Parses the payload based on message ID, mode, and version."""
|
||||
msg_name = ID_MAP.get(msg_id)
|
||||
|
||||
# Check for generic RESP message first (PDF page 6)
|
||||
if mode['type_val'] == 1 and len(payload) == 3: # CONTENT from DEVICE
|
||||
try:
|
||||
code, _check1, _check2 = struct.unpack('<BBB', payload)
|
||||
resp_codes = {
|
||||
0: "NONE", 1: "OK", 2: "ERR_CHECKSUMM", 3: "ERR_PAYLOAD",
|
||||
4: "ERR_ID", 5: "ERR_VERSION", 6: "ERR_TYPE", 7: "ERR_KEY",
|
||||
8: "ERR_RUNTIME"
|
||||
}
|
||||
if code in resp_codes:
|
||||
return f"RESP: Status={resp_codes.get(code)} ({code})"
|
||||
except struct.error:
|
||||
pass # Not a valid RESP message, fall through
|
||||
|
||||
# Specific payload parsers
|
||||
if msg_name == "ID_USBL_SOLUTION" and mode['version'] == 0:
|
||||
# This is a GET request, no payload is expected.
|
||||
if mode['type_val'] == 3 and len(payload) == 0:
|
||||
return "Requesting UsblSolution"
|
||||
if len(payload) == 152:
|
||||
try:
|
||||
# Format string derived from the user-provided Python code
|
||||
fmt = '<BBHqIqfffffffffddffffddIff8f'
|
||||
sol = struct.unpack(fmt, payload)
|
||||
return (
|
||||
f"Usbl: ID={sol[0]},"
|
||||
f"Dist={sol[6]:.2f}m, "
|
||||
f"Azimuth={sol[8]:.2f}deg, Elev={sol[10]:.2f}deg, "
|
||||
f"SNR={sol[12]:.1f}")
|
||||
except struct.error as e:
|
||||
return f"Error: Failed to unpack UsblSolution: {e}"
|
||||
else:
|
||||
return f"Error: UsblSolution payload wrong size ({len(payload)} bytes)"
|
||||
|
||||
|
||||
if msg_name == "ID_USBL_CONTROL":
|
||||
if mode['version'] == 1 and len(payload) == 5:
|
||||
timeout, address = struct.unpack('<IB', payload)
|
||||
return f"AcousticPing: Timeout={timeout} us, Address={address}"
|
||||
if mode['version'] == 3 and len(payload) == 4:
|
||||
timeout, = struct.unpack('<I', payload)
|
||||
return f"AutoResponseTimeout: Timeout={timeout} us"
|
||||
if mode['version'] == 4 and len(payload) == 1:
|
||||
address, = struct.unpack('<B', payload)
|
||||
return f"AutoResponseAddressFilter: Address={address}"
|
||||
if mode['version'] == 5 and len(payload) == 1:
|
||||
address, = struct.unpack('<B', payload)
|
||||
return f"AutoResponsePayloadAddress: Address={address}"
|
||||
|
||||
if msg_name == "ID_TEMP" and mode['type_val'] == 1 and len(payload) == 2:
|
||||
temp_val, = struct.unpack('<h', payload)
|
||||
return f"Temperature: {temp_val * 0.01:.2f} C"
|
||||
|
||||
if msg_name == "ID_DIST" and mode['type_val'] == 1:
|
||||
if mode['version'] == 0 and len(payload) == 4:
|
||||
distance, = struct.unpack('<I', payload)
|
||||
return f"Distance: {distance} mm"
|
||||
if mode['version'] == 1 and len(payload) == 6:
|
||||
_num, _strong, distance, _width = struct.unpack('<BBIH', payload)
|
||||
return f"Distance (v1): {distance} mm"
|
||||
|
||||
if msg_name == "ID_ATTITUDE" and mode['type_val'] == 1:
|
||||
if mode['version'] == 0 and len(payload) == 6:
|
||||
yaw, pitch, roll = struct.unpack('<hhh', payload)
|
||||
return (
|
||||
f"Attitude (Euler): Yaw={yaw*0.01:.2f}, "
|
||||
f"Pitch={pitch*0.01:.2f}, Roll={roll*0.01:.2f} deg")
|
||||
if mode['version'] == 1 and len(payload) == 16:
|
||||
w0, w1, w2, w3 = struct.unpack('<ffff', payload)
|
||||
return f"Attitude (Quaternion): w0={w0}, w1={w1}, w2={w2}, w3={w3}"
|
||||
|
||||
# Fallback for unknown or unhandled payloads
|
||||
return f"Payload (hex): {payload.hex()}"
|
||||
|
||||
def parse_message(data):
|
||||
"""Parses a full binary message frame."""
|
||||
if len(data) < 8:
|
||||
return "Error: Message too short"
|
||||
|
||||
try:
|
||||
sync1, sync2, route_byte, mode_byte, msg_id, length = struct.unpack('<BBBBBB', data[:6])
|
||||
except struct.error:
|
||||
return "Error: Could not unpack header"
|
||||
|
||||
if sync1 != 0xBB or sync2 != 0x55:
|
||||
return "Error: Invalid sync bytes"
|
||||
|
||||
header_end = 6
|
||||
payload_end = header_end + length
|
||||
checksum_end = payload_end + 2
|
||||
|
||||
if len(data) != checksum_end:
|
||||
return f"Error: Length mismatch. Header says {length}, but packet is {len(data)}"
|
||||
|
||||
payload = data[header_end:payload_end]
|
||||
|
||||
route_info = parse_route(route_byte)
|
||||
mode_info = parse_mode(mode_byte)
|
||||
|
||||
msg_name = ID_MAP.get(msg_id, f"Unknown ID (0x{msg_id:02x})")
|
||||
payload_str = parse_payload(msg_id, mode_info, payload)
|
||||
|
||||
return (
|
||||
f"ID:{msg_name[3:]:<14} | "
|
||||
f"Type:{mode_info['type']:<11} | "
|
||||
f"Vr:{mode_info['version']} | "
|
||||
f"Ln:{length:<3} | "
|
||||
f"Addr:{route_info['dev_address']} | "
|
||||
f"{payload_str}"
|
||||
)
|
||||
|
||||
def print_save(way, timestamp, message, outfile):
|
||||
outfile.write(f"{timestamp} {way}: {message}\n")
|
||||
|
||||
|
||||
def main(log_file):
|
||||
"""Main function to read and process the log file."""
|
||||
base, ext = os.path.splitext(log_file)
|
||||
output_file = f"{base}_hum{ext}"
|
||||
|
||||
print(f"Input file: {log_file}")
|
||||
print(f"Output file: {output_file}")
|
||||
|
||||
reassembly_buffer = b''
|
||||
last_direction = None
|
||||
|
||||
try:
|
||||
with open(log_file, 'r', newline='') as infile, open(output_file, 'w') as outfile:
|
||||
reader = csv.reader(infile)
|
||||
for i, row in enumerate(reader):
|
||||
if len(row) != 3:
|
||||
outfile.write(f"Skipping malformed row {i+1}: {row}\n")
|
||||
continue
|
||||
|
||||
timestamp, direction, data_str = row
|
||||
|
||||
try:
|
||||
data_bytes = ast.literal_eval(data_str)
|
||||
if not isinstance(data_bytes, bytes):
|
||||
raise TypeError("Not a bytes object")
|
||||
except (ValueError, SyntaxError, TypeError):
|
||||
outfile.write(f"{timestamp} {direction.ljust(8)}: Error: Could not parse data literal: {data_str}\n")
|
||||
continue
|
||||
|
||||
# If the direction changes, reset the buffer.
|
||||
if direction != last_direction:
|
||||
reassembly_buffer = b''
|
||||
last_direction = direction
|
||||
|
||||
if direction == "SENT":
|
||||
# SENT messages are assumed to be complete
|
||||
human_readable_msg = parse_message(data_bytes)
|
||||
print_save("TX", timestamp, human_readable_msg, outfile)
|
||||
continue
|
||||
|
||||
# Handle reassembly for RECEIVED messages
|
||||
reassembly_buffer += data_bytes
|
||||
|
||||
while len(reassembly_buffer) >= 8: # Minimum possible message size
|
||||
if not reassembly_buffer.startswith(b'\xbbU'):
|
||||
# Buffer doesn't start with sync bytes, find the next sync sequence
|
||||
sync_pos = reassembly_buffer.find(b'\xbbU')
|
||||
if sync_pos == -1:
|
||||
# No sync bytes, discard the whole buffer
|
||||
reassembly_buffer = b''
|
||||
break
|
||||
else:
|
||||
# Discard garbage before the sync bytes
|
||||
reassembly_buffer = reassembly_buffer[sync_pos:]
|
||||
|
||||
if len(reassembly_buffer) < 6:
|
||||
# Not enough data for a header
|
||||
break
|
||||
|
||||
# Try to read the payload length
|
||||
payload_len = reassembly_buffer[5]
|
||||
full_msg_len = 6 + payload_len + 2 # header + payload + checksum
|
||||
|
||||
if len(reassembly_buffer) >= full_msg_len:
|
||||
# We have a complete message
|
||||
message_to_parse = reassembly_buffer[:full_msg_len]
|
||||
human_readable_msg = parse_message(message_to_parse)
|
||||
print_save("RX", timestamp, human_readable_msg, outfile)
|
||||
|
||||
# Keep the rest of the buffer for the next message
|
||||
reassembly_buffer = reassembly_buffer[full_msg_len:]
|
||||
else:
|
||||
# Not enough data for a full message, wait for more
|
||||
break
|
||||
|
||||
except FileNotFoundError:
|
||||
print(f"Error: File not found at {log_file}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
except Exception as e:
|
||||
print(f"An unexpected error occurred: {e}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) != 2:
|
||||
print(f"Usage: python {sys.argv[0]} <log_file.csv>", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
main(sys.argv[1])
|
||||
Reference in New Issue
Block a user