diff --git a/Tools/scripts/imu_uart_decode.py b/Tools/scripts/imu_uart_decode.py new file mode 100755 index 0000000000..e9954620cf --- /dev/null +++ b/Tools/scripts/imu_uart_decode.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 +''' +script to decode IMU data on a UART +''' + +import socket, struct, serial, sys + +import argparse + +parser = argparse.ArgumentParser(description='decode IMU data from ArduPilot') + +parser.add_argument('--set-file', type=str, default=None, help='replace parameter defaults from a file') +parser.add_argument('--tcp', default=None, help='TCP endpoint as IP:port') +parser.add_argument('--device', default=None, help='serial port to read from') +parser.add_argument('--baudrate', default=921600, help='baudrate for serial port') + +args = parser.parse_args() + +if args.tcp is None and args.device is None: + print("Must specicy --tcp or --device") + sys.exit(1) + +if args.tcp is not None: + server_address = args.tcp.split(':') + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.connect((server_address[0], int(server_address[1]))) +else: + port = serial.Serial(args.device, baudrate=args.baudrate) + +def crc16_from_bytes(bytes, initial=0): + # CRC-16-CCITT + # Initial value: 0xFFFF + # Poly: 0x1021 + # Reverse: no + # Output xor: 0 + # Check string: '123456789' + # Check value: 0x29B1 + + try: + if isinstance(bytes, basestring): # Python 2.7 compatibility + bytes = map(ord, bytes) + except NameError: + if isinstance(bytes, str): # This branch will be taken on Python 3 + bytes = map(ord, bytes) + + crc = initial + for byte in bytes: + crc ^= byte << 8 + for bit in range(8): + if crc & 0x8000: + crc = ((crc << 1) ^ 0x1021) & 0xFFFF + else: + crc = (crc << 1) & 0xFFFF + return crc & 0xFFFF + +class Packet(object): + def __init__(self, pkt): + '''parse the packet''' + self.pkt = pkt + self.delta_velocity = [0.0]*3 + self.delta_angle = [0.0]*3 + (self.magic, + self.length, + self.timestamp_us, + self.delta_velocity[0], + self.delta_velocity[1], + self.delta_velocity[2], + self.delta_angle[0], + self.delta_angle[1], + self.delta_angle[2], + self.delta_velocity_dt, + self.delta_angle_dt, + self.counter, + self.crc) = struct.unpack("