kogger-transpondeur-continu/driver/test/test_usv.py

#! /usr/bin/env python
# Test to run USV. Will ping AUV, and wait for response.

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__), '..')))
from kogger_protocol_driver import KoggerSBPDevice

# --- 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/ttyAMA4"
#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

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 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.
    """
    global DEFAULT_SERIAL_PORT
    global DEFAULT_SERIAL_SPEED
    global LOG_LEVEL
    global LOG_FILE

    # Determine the serial port to use
    if len(sys.argv)>1:
        if sys.argv[1].startswith("/dev/"):
            DEFAULT_SERIAL_PORT = sys.argv[1]
            serial_port = sys.argv[1]
        else:
            serial_port = "/dev/ttyUSB"+str(sys.argv[1])
    else:
        print(DEFAULT_SERIAL_PORT)
        serial_port = DEFAULT_SERIAL_PORT
        print(f"No serial port provided. Using default: {serial_port}")

    # Instantiate the driver
    antenna = 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))
        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}")
                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
                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)
    finally:
        # Ensure the connection is closed gracefully
        print("Disconnecting from the antenna.")
        antenna.disconnect()

if __name__ == "__main__":
    main()