wip

quad/scripts/uart_comm_listen.py

+#!/usr/local/bin/python3.6
+
+import sys
+import time
+
+import serial
+
+def read_packet(ser):
+    header = ser.read(7)
+    length = int.from_bytes(header[5:7], byteorder='little')
+    data = ser.read(length)
+    checksum = ser.read()
+    return data
+
+def query_received(ser):
+    # Send request
+    query_msg = create_msg(0, 3, 0, b'')
+    ser.write(query_msg)
+    ser.flush()
+    
+    resp = read_packet(ser)
+    received_str = resp[:-1].decode()
+
+    return tuple(map(int, received_str.split(',')))
+
+if __name__ == '__main__':
+    with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
+        i = 0
+        while True:
+            ser.reset_input_buffer()
+            time.sleep(0.05)
+            while ser.in_waiting != 0:
+                resp = read_packet(ser)
+                elapsed = int.from_bytes(resp, byteorder='little')
+                print("{} {}".format(i, elapsed))
+                i += 1
+            ser.flush()
+

quad/scripts/uart_comm_send.py

+#!/usr/local/bin/python3.6
+
+import sys
+import time
+
+import serial
+
+def create_msg(main_type, subtype, msg_id, data):
+    msg = bytes()
+    msg += b'\xBE'
+    msg += main_type.to_bytes(1, 'little')
+    msg += subtype.to_bytes(1, 'little')
+    msg += msg_id.to_bytes(2, 'little')
+    msg += len(data).to_bytes(2, 'little')
+    msg += data
+    
+    checksum = 0
+    for b in msg:
+        checksum ^= b
+    msg += checksum.to_bytes(1, 'little')
+    return msg
+
+def create_test_packet(size=8):
+    data = bytes((i % 256 for i in range(size)))
+    return create_msg(0, 2, 0, data)
+
+if __name__ == '__main__':
+    with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
+        for i in range(0, 100):
+            time.sleep(0.1)
+            ser.reset_input_buffer()
+            ser.write(create_test_packet(8))
+            ser.flush()