#! /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()