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:
230
driver/test/test_multi_antenna.py
Executable file
230
driver/test/test_multi_antenna.py
Executable file
@@ -0,0 +1,230 @@
|
||||
#! /usr/bin/env python
|
||||
# Test to run multi antenna.
|
||||
# arg 1 is the USB number
|
||||
# arg 2 is the index slot
|
||||
# arg 3 is the total slot
|
||||
|
||||
import time
|
||||
import sys
|
||||
import tty
|
||||
import termios
|
||||
import select
|
||||
import json
|
||||
import os
|
||||
from loguru import logger
|
||||
import datetime
|
||||
|
||||
# Add the parent directory to the Python path to find the driver module
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||||
|
||||
try:
|
||||
#from kogger_protocol_driver import KoggerSBPDevice, setup_logging
|
||||
import kogger_protocol_driver
|
||||
except ImportError:
|
||||
logger.critical("Failed to import KoggerSBPDevice. Make sure kogger_protocol_driver.py is in the parent directory.")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
# --- Script Configuration ---
|
||||
# Set the desired logging level: "DEBUG", "INFO", "WARNING", "ERROR", "CRITICAL"
|
||||
LOG_LEVEL = "INFO"
|
||||
timestamp = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
|
||||
LOG_FILE = "log/"+str(timestamp)+"_log_usv.log"
|
||||
# Default serial port if not provided via command line
|
||||
DEFAULT_SERIAL_PORT = "/dev/ttyUSB0"
|
||||
#DEFAULT_SERIAL_PORT = "/dev/serial/by-path/platform-fd500000.pcie-pci-0000:01:00.0-usb-0:1.3:1.0"
|
||||
DEFAULT_SERIAL_SPEED = 921600
|
||||
|
||||
antenna = None
|
||||
|
||||
|
||||
def print_message(message):
|
||||
"""
|
||||
This function is a callback that will be executed for each message
|
||||
received from the antenna that is not a direct response to a command.
|
||||
"""
|
||||
parsed = json.loads(str(message).replace("nan", "'nan'").replace("'",'"'))
|
||||
logger.info("printer:"+json.dumps(parsed, indent=2))
|
||||
|
||||
def test_callback(message):
|
||||
global antenna
|
||||
data=antenna.get_usbl_data()
|
||||
global response_received
|
||||
response_received = message["id"]
|
||||
logger.info("test!!!!!!"+str(data))
|
||||
return message
|
||||
|
||||
def is_data():
|
||||
"""
|
||||
Checks if there is data to be read from stdin.
|
||||
"""
|
||||
return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])
|
||||
|
||||
def check_all_getters(antenna):
|
||||
"""
|
||||
Calls all get_* methods on the antenna object and prints the results.
|
||||
"""
|
||||
print("\n--- Checking all getter methods ---")
|
||||
|
||||
# A list of all getter methods to call.
|
||||
# Some methods require arguments, which are provided as tuples.
|
||||
# (method_name, args_tuple)
|
||||
getters_to_test = [
|
||||
#('get_timestamp', ()),
|
||||
#('get_distance', (0,)),
|
||||
#('get_distance', (1,)),
|
||||
#('get_chart_data', ()),
|
||||
#('get_attitude', (0,)), #!!OK
|
||||
#('get_attitude', (1,)),
|
||||
#('get_temperature', ()),
|
||||
#('get_dataset_config', (0,)),
|
||||
#('get_distance_setup', ()),
|
||||
#('get_chart_setup', ()),
|
||||
#('get_transceiver_settings', ()),
|
||||
#('get_sound_speed', ()),
|
||||
#('get_uart_config', (1, 0)), # uart_id=1, version=0
|
||||
#('get_uart_config', (1, 1)), # uart_id=1, version=1
|
||||
('get_version_info', ()), #!! OK
|
||||
#('get_mark_status', ()), #!! OK
|
||||
#('get_diagnostics', ()),
|
||||
#('get_navigation_data', ()),
|
||||
#('get_dvl_velocity_data', ()),
|
||||
#('get_signal_encoder_data', ()),
|
||||
#('get_signal_decoder_data', ()),
|
||||
#('get_auto_response_timeout', ()),
|
||||
#('get_auto_response_filter', ()),
|
||||
#('get_auto_response_payload', ()),
|
||||
#('get_usbl_solution', ()),
|
||||
]
|
||||
|
||||
for method_name, args in getters_to_test:
|
||||
try:
|
||||
method = getattr(antenna, method_name)
|
||||
print(f"Calling {method_name}{args}...")
|
||||
result = method(*args)
|
||||
print(f"Result: {result}")
|
||||
except Exception as e:
|
||||
print(f"An error occurred while calling {method_name}: {e}")
|
||||
print("-" * 20)
|
||||
time.sleep(0.01) # Give the device a moment between commands
|
||||
|
||||
print("--- Finished checking all getter methods ---\n")
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
Main function to connect to the Kogger antenna and listen for messages.
|
||||
"""
|
||||
# Determine the serial port to use
|
||||
slot_total = 0
|
||||
if len(sys.argv)>1:
|
||||
if sys.argv[1].startswith("/dev/"):
|
||||
DEFAULT_SERIAL_PORT = sys.argv[1]
|
||||
serial_port = sys.argv[1]
|
||||
slot_index = int(sys.argv[2])
|
||||
slot_total = int(sys.argv[3])
|
||||
slot_duration = 1
|
||||
else:
|
||||
serial_port = "/dev/ttyUSB"+str(sys.argv[1])
|
||||
slot_index = 0
|
||||
slot_total = 0
|
||||
slot_duration = 1
|
||||
else:
|
||||
serial_port = DEFAULT_SERIAL_PORT
|
||||
print(f"No serial port provided. Using default: {serial_port}")
|
||||
|
||||
# Instantiate the driver
|
||||
global antenna
|
||||
|
||||
antenna = kogger_protocol_driver.KoggerSBPDevice(serial_port, DEFAULT_SERIAL_SPEED, default_timeout=0.02, log_level=LOG_LEVEL, log_file=LOG_FILE)
|
||||
|
||||
try:
|
||||
# Connect to the antenna
|
||||
if not antenna.connect():
|
||||
print(f"Failed to connect to the antenna on port {serial_port}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
print(f"Successfully connected to the antenna on {serial_port}.")
|
||||
# Perform a one-time check of all getter functions
|
||||
check_all_getters(antenna)
|
||||
|
||||
result = antenna.set_auto_response_filter(0)
|
||||
logger.info("set_auto_response_filter(0)="+str(result))
|
||||
result = antenna.set_auto_response_timeout(0xffffffff)
|
||||
logger.info("set_auto_response_timeout(0xffffffff)="+str(result))
|
||||
result = antenna.set_auto_response_payload(0xff)
|
||||
logger.info("set_auto_response_payload(0xff)="+str(result))
|
||||
if slot_index!=0:
|
||||
result = antenna.set_echo_filter(True)
|
||||
logger.info("set_echo_filter(True)="+str(result))
|
||||
else:
|
||||
result = antenna.set_echo_filter(False)
|
||||
logger.info("set_echo_filter(False)="+str(result))
|
||||
|
||||
if slot_total!=0:
|
||||
if slot_index!=0:
|
||||
enable_delay=True
|
||||
else:
|
||||
enable_delay=False
|
||||
result = antenna.set_sync_mode(slot_total, slot_index, slot_duration, enable_delay)
|
||||
logger.info(f"set_sync_mode({slot_total},{slot_index},{slot_duration},{enable_delay})={result}")
|
||||
if slot_index !=0:
|
||||
antenna.register_precallback(kogger_protocol_driver.ID_USBL_SOLUTION, antenna.callback_usbl_solution)
|
||||
antenna.register_callback(kogger_protocol_driver.ID_USBL_SOLUTION, test_callback)
|
||||
antenna.register_default_callback(print_message)
|
||||
|
||||
print("\nPress a number (0-9) to send an acoustic ping. Press 'q' to quit.")
|
||||
|
||||
data_number = 255
|
||||
last_ping_time = 0
|
||||
str_star_moving_ping = ['|','/','−','\\']
|
||||
cnt_star_moving_ping = 0
|
||||
str_star = ""
|
||||
|
||||
while True:
|
||||
# Handle keyboard input
|
||||
if is_data():
|
||||
char = sys.stdin.read(1)
|
||||
if char.isdigit():
|
||||
data_number = int(char)
|
||||
print(f"\r\n--> Switched data number to: {data_number}")
|
||||
if slot_index==0:
|
||||
result = antenna.set_auto_response_payload(data_number)
|
||||
elif char.lower() == 'q':
|
||||
print("\r\nQuitting...")
|
||||
break
|
||||
elif ord(char) == 3: # Ctrl+C
|
||||
raise KeyboardInterrupt
|
||||
|
||||
# Send ping every 1 second
|
||||
current_time = time.time()
|
||||
if current_time - last_ping_time >= 1:
|
||||
last_ping_time = current_time
|
||||
if slot_index!=0:
|
||||
result = antenna.send_acoustic_ping(data_number)
|
||||
if result == True:
|
||||
cnt_star_moving_ping = (cnt_star_moving_ping+1)%4
|
||||
str_star = str_star_moving_ping[cnt_star_moving_ping]
|
||||
else:
|
||||
str_star = "!"
|
||||
|
||||
# Update the prompt on the same line
|
||||
prompt = f"\r"+str_star+"--> Pinging with "+str(data_number)+". Press 0-9+ENTER to change, 'q'+ENTER to quit."
|
||||
sys.stdout.write(prompt)
|
||||
sys.stdout.flush()
|
||||
|
||||
time.sleep(0.1)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\nExiting...")
|
||||
except Exception as e:
|
||||
print(f"An error occurred: {e}", file=sys.stderr)
|
||||
exc_type, exc_obj, exc_tb = sys.exc_info()
|
||||
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
|
||||
print(f"type={exc_type}, fname={fname}, line={exc_tb.tb_lineno}")
|
||||
finally:
|
||||
# Ensure the connection is closed gracefully
|
||||
print("Disconnecting from the antenna.")
|
||||
antenna.disconnect()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user