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.
207 lines
9.9 KiB
Python
Executable File
207 lines
9.9 KiB
Python
Executable File
#! /usr/bin/env python
|
|
# Test all commands from kogger driver
|
|
|
|
import sys
|
|
import time
|
|
import struct # For unpacking in callbacks, if needed
|
|
from loguru import logger
|
|
|
|
# Ensure the kogger_protocol_driver.py is in the Python path
|
|
try:
|
|
from kogger_protocol_driver import (
|
|
KoggerSBPDevice,
|
|
ID_TIMESTAMP,
|
|
# Add other specific IDs if you want specific callbacks for them
|
|
)
|
|
except ImportError:
|
|
logger.critical("Failed to import KoggerSBPDevice. Make sure kogger_protocol_driver.py is in the same directory or Python path.")
|
|
sys.exit(1)
|
|
|
|
# --- Loguru Setup for this script ---
|
|
logger.remove() # Remove default handler
|
|
LOG_LEVEL = "INFO" # Change to "DEBUG" for more detailed driver logs
|
|
logger.add(sys.stderr, level=LOG_LEVEL, format="<green>{time:YYYY-MM-DD HH:mm:ss.SSS}</green> | <level>{level: <8}</level> | <cyan>{name}</cyan>:<cyan>{function}</cyan>:<cyan>{line}</cyan> - <level>{message}</level>")
|
|
|
|
# --- Configuration for Real Device ---
|
|
SERIAL_PORT = "/dev/ttyUSB0"
|
|
BAUDRATE = 115200
|
|
DEVICE_ADDRESS = 0
|
|
REQUEST_INTERVAL_SECONDS = 10 # How often to run the full cycle of get requests
|
|
COMMAND_PAUSE_SECONDS = 0.01 # Small pause between individual commands
|
|
|
|
# --- Callback for Unsolicited Messages ---
|
|
def handle_unsolicited_message(frame):
|
|
"""Default callback for unsolicited messages."""
|
|
#logger.info(f"[UNSOLICITED MSG] ID: {frame['id']:#02x}, Mode: {frame['mode']:#02x}, Len: {frame['length']}, Payload: {frame['payload'].hex().upper()}")
|
|
if frame['id'] == ID_TIMESTAMP and frame.get('checksum_ok') and \
|
|
(frame['mode'] & 0x03) == 1 and frame['length'] == 4: # TYPE_CONTENT = 1
|
|
try:
|
|
timestamp = struct.unpack('<I', frame['payload'])[0]
|
|
logger.info(f" -> Unsolicited Timestamp: {timestamp} ms")
|
|
except struct.error as e:
|
|
logger.warning(f" -> Failed to unpack unsolicited timestamp: {e}")
|
|
# Add more specific parsing here if you expect other unsolicited messages frequently
|
|
|
|
def UpdateCounter(data, string, str_valid, str_timeout):
|
|
if data:
|
|
logger.success(" Success to get "+str(string)+":"+str(data))
|
|
str_valid.append(string)
|
|
else:
|
|
logger.warning(" Failed to get "+str(string))
|
|
str_timeout.append(string)
|
|
|
|
|
|
def main():
|
|
logger.info(f"Attempting to connect to Kogger device on {SERIAL_PORT} at {BAUDRATE} baud.")
|
|
|
|
driver = KoggerSBPDevice(
|
|
port=SERIAL_PORT,
|
|
baudrate=BAUDRATE,
|
|
device_address=DEVICE_ADDRESS,
|
|
default_timeout=0.1 # Wait up to 2s for a solicited response
|
|
)
|
|
|
|
if not driver.connect():
|
|
logger.error(f"Failed to connect to the device. Please check connections and permissions for {SERIAL_PORT}.")
|
|
return
|
|
|
|
driver.register_default_callback(handle_unsolicited_message)
|
|
logger.info("Registered default callback for unsolicited messages.")
|
|
|
|
try:
|
|
logger.info(f"Successfully connected. Starting data polling cycle every {REQUEST_INTERVAL_SECONDS} seconds. Press Ctrl+C to stop.")
|
|
loop_count = 0
|
|
while True:
|
|
str_valid = []
|
|
str_timeout = []
|
|
loop_count += 1
|
|
logger.info(f"\n========= Request Cycle {loop_count} =========")
|
|
|
|
# --- Measurement Data ---
|
|
logger.info("--- Requesting Timestamp (get_timestamp) ---")
|
|
data = driver.get_timestamp()
|
|
UpdateCounter(data, "get_timestamp", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Distance v0 (get_distance) ---")
|
|
data = driver.get_distance(version=0)
|
|
UpdateCounter(data, "get_distance(version=0)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Distance v1 (get_distance) ---")
|
|
data = driver.get_distance(version=1)
|
|
UpdateCounter(data, "get_distance(version=1)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Chart Data (get_chart_data) ---")
|
|
data = driver.get_chart_data()
|
|
UpdateCounter(data, "get_chart_data()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Attitude v0 - Euler (get_attitude) ---")
|
|
data = driver.get_attitude(version=0)
|
|
UpdateCounter(data, "get_attitude(version=0)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Attitude v1 - Quaternion (get_attitude) ---")
|
|
data = driver.get_attitude(version=1)
|
|
UpdateCounter(data, "get_attitude(version=1)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Temperature (get_temperature) ---")
|
|
data = driver.get_temperature()
|
|
UpdateCounter(data, "get_temperature()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
# --- Settings Data (GET methods) ---
|
|
UART_ID_TO_QUERY = 1 # Example UART ID from PDF
|
|
CHANNEL_ID_TO_QUERY = 0 # Example: 0 for "all active" or default dataset
|
|
|
|
logger.info(f"--- Requesting Dataset Config (Channel {CHANNEL_ID_TO_QUERY}) (get_dataset_config) ---")
|
|
data = driver.get_dataset_config(channel_id_to_request=CHANNEL_ID_TO_QUERY)
|
|
UpdateCounter(data, "get_dataset_config(channel_id_to_request=CHANNEL_ID_TO_QUERY)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Distance Setup (get_distance_setup) ---")
|
|
data = driver.get_distance_setup()
|
|
UpdateCounter(data, "get_distance_setup()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Chart Setup (get_chart_setup) ---")
|
|
data = driver.get_chart_setup()
|
|
UpdateCounter(data, "get_chart_setup()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Transceiver Settings (get_transceiver_settings) ---")
|
|
data = driver.get_transceiver_settings()
|
|
UpdateCounter(data, "get_transceiver_settings()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
data = driver.get_sound_speed()
|
|
UpdateCounter(data, "get_sound_speed()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info(f"--- Requesting UART Config v0 (Baudrate, UART {UART_ID_TO_QUERY}) (get_uart_config) ---")
|
|
data = driver.get_uart_config(uart_id=UART_ID_TO_QUERY, version=0)
|
|
UpdateCounter(data, "get_uart_config(uart_id=1, version=0)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info(f"--- Requesting UART Config v1 (Dev Address, UART {UART_ID_TO_QUERY}) (get_uart_config) ---")
|
|
data = driver.get_uart_config(uart_id=UART_ID_TO_QUERY, version=1)
|
|
UpdateCounter(data, "get_uart_config(uart_id=1, version=1)", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
# --- System Data (GET methods) ---
|
|
logger.info("--- Requesting Version Info (get_version_info) ---")
|
|
data = driver.get_version_info()
|
|
UpdateCounter(data, "get_version_info()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Mark Status (get_mark_status) ---")
|
|
data = driver.get_mark_status()
|
|
UpdateCounter(data, "get_mark_status()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting Diagnostics (get_diagnostics) ---")
|
|
data = driver.get_diagnostics()
|
|
UpdateCounter(data, "get_diagnostics()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
# --- Navigation Data (GET methods) ---
|
|
logger.info("--- Requesting Navigation Data (get_navigation_data) ---")
|
|
data = driver.get_navigation_data()
|
|
UpdateCounter(data, "get_navigation_data()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info("--- Requesting DVL Velocity Data (get_dvl_velocity_data) ---")
|
|
data = driver.get_dvl_velocity_data()
|
|
UpdateCounter(data, "get_dvl_velocity_data()", str_valid, str_timeout)
|
|
time.sleep(COMMAND_PAUSE_SECONDS)
|
|
|
|
logger.info(f"========= End of Request Cycle {loop_count}. Waiting {REQUEST_INTERVAL_SECONDS} seconds... =========")
|
|
logger.info("str_valid "+str(len(str_valid)) +"="+str(str_valid))
|
|
logger.info("str_timeout "+str(len(str_timeout))+"="+str(str_timeout))
|
|
time.sleep(REQUEST_INTERVAL_SECONDS)
|
|
|
|
except KeyboardInterrupt:
|
|
logger.info("Keyboard interrupt received. Stopping application...")
|
|
except Exception as e:
|
|
logger.critical(f"An unexpected error occurred during main loop: {e}", exc_info=True)
|
|
finally:
|
|
logger.info("Attempting to disconnect from the device...")
|
|
if 'driver' in locals() and driver and hasattr(driver, 'serial_conn') and driver.serial_conn :
|
|
if hasattr(driver.serial_conn, 'is_open') and driver.serial_conn.is_open:
|
|
driver.disconnect()
|
|
logger.info("Disconnected successfully.")
|
|
else: # Port was not open, but driver object exists
|
|
if hasattr(driver, '_reader_thread') and driver._reader_thread and driver._reader_thread.is_alive():
|
|
logger.info("Port was not open, ensuring reader thread is stopped if it was started.")
|
|
driver._stop_event.set()
|
|
driver._reader_thread.join(timeout=1.0)
|
|
else:
|
|
logger.info("Driver was not connected or instance not fully available for disconnect.")
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
logger.info("Real device interaction script finished.")
|