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.
276 lines
10 KiB
Python
Executable File
276 lines
10 KiB
Python
Executable File
#!/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]) |