Okon tcm client

Serial/jetson_serial_light.py

@@ -0,0 +1,208 @@
+''' Serial communication between Jetson and TCM module
+
+Implementation of serial communication between Jetson and TCM module.
+jetson_serial_light version has the thread responsible for transmission removed
+(not needed since it is executed on demand)
+
+Authors: Maks Cebula, Maciej Morawski
+'''
+import datetime
+import logging
+import threading
+import time
+from multiprocessing import Lock
+
+import serial
+
+DEVICE_PORT = "/dev/ttyACM0"
+
+# SET BUFFER DEBUG MODE
+# False - without printing anything
+# True - print information about buffer (worker)
+BUFFER_DEBUG_MODE = True
+
+# SET DATA DEBUG MODE
+# False - without printing anything
+# True - print information about data (readFromSerial method)
+DATA_DEBUG_MODE = True
+
+# SET LOCK DEBUG MODE
+# False - without printing anything
+# True - print information about acquiring/releasing lock
+LOCK_DEBUG_MODE = True
+
+# ADD PAUSE BETWEEN WRITING AND READING IN MAIN METHOD
+# False - without printing anything
+# True - print information about buffer (worker)
+SLEEP_BETWEEN_WRITE_AND_READ = False
+
+class JetsonSerial:
+    def __init__(self, serial_port=DEVICE_PORT):
+
+        self.serial_port = serial_port
+
+        self.crash_event = threading.Event()
+
+        self.InitSerial()
+
+    def InitSerial(self, baud_rate=115200, timeout=0.5, parity=serial.PARITY_NONE):
+
+        self.serial = serial.Serial()
+        self.serial.port = self.serial_port
+        self.serial.baudrate = baud_rate
+        self.serial.timeout = timeout
+        self.serial.write_timeout = timeout
+        self.serial.parity = parity
+
+        self.output_buffer = []
+        self.output_lock = Lock()
+
+        # Try open serial to check if it is available
+        try:
+            self.serial.open()
+            self.serial.reset_input_buffer()
+            self.serial.reset_output_buffer()
+
+        except:
+            logging.error("Couldn't open serial: {}".format(self.serial_port))
+            exit()
+
+        self.startWorkers()
+
+    def runWorker(self):
+
+        # Read from device and write to jetson
+
+        busy = datetime.datetime.utcnow()
+
+        while self.serial.isOpen() and not self.crash_event.is_set():
+
+            try:
+                buff_size = self.serial.inWaiting()
+            except:
+                logging.error("Cannot check if there is something in input queue!")
+                self.crash_event.set()
+                self.handleCrash()
+                break
+
+            if buff_size == 0:
+                not_busy = datetime.datetime.utcnow()
+
+                if ((not_busy - busy).total_seconds()) > 5:
+                    # Make decision what to do when serial is open but not responding
+                    logging.error("Serial port is not responding for more than 5 seconds...")
+                    break
+
+                continue
+
+            done = False
+            while not done:
+                acq = self.output_lock.acquire(block=False)
+                if acq:
+                    if LOCK_DEBUG_MODE:
+                        print("lock acquired by worker")
+                    try:
+                        buffer = self.serial.read(buff_size)
+                        self.output_buffer.append(buffer)
+                        if BUFFER_DEBUG_MODE:
+                            print(buffer)
+                    finally:
+                        self.output_lock.release()
+                        if LOCK_DEBUG_MODE:
+                            print("lock released by worker")
+                        done = True
+                    break
+
+            busy = datetime.datetime.utcnow()
+
+    def startWorkers(self):
+
+        self.tx_worker = threading.Thread(target=self.runWorker, daemon=True)
+        self.tx_worker.start()
+
+    def writeToSerial(self, data: bytes):
+
+        try:
+            self.serial.write(data)
+        except:
+            logging.error("Cannot write to serial, ending...")
+
+    def readFromSerial(self):
+        done = False
+        while not done:
+            acq = self.output_lock.acquire(block=False)
+            if acq:
+                if LOCK_DEBUG_MODE:
+                    print("lock acquired while readFromSerial method executed")
+                try:
+                    data = self.output_buffer
+                    if(DATA_DEBUG_MODE):
+                        print(data)
+                    self.output_buffer.clear()
+                finally:
+                    self.output_lock.release()
+                    if LOCK_DEBUG_MODE:
+                        print("lock released while readFromSerial method executed")
+                    done = True
+                break
+
+        return data
+
+    def handleCrash(self):
+
+        self.crash_event.clear()
+
+        # Close serial
+        self.serial.close()
+
+        # Try reopen
+        counter = 10
+
+        while counter > 0:
+            try:
+                self.serial.open()
+                break
+            except:
+                time.sleep(0.5)
+                counter = counter - 1
+                logging.error("Cannot open serial port!")
+
+        if not (self.serial.isOpen()):
+            exit()
+
+        self.startWorkers()
+
+    def __del__(self):
+
+        print("Im cleaning...")
+
+        self.crash_event.set()
+
+        self.tx_worker.join()
+
+        self.crash_event.clear()
+
+        if self.serial.is_open:
+            self.serial.close()
+
+
+if __name__ == "__main__":
+
+    format = "%(asctime)s [%(funcName)s():%(lineno)s] %(message)s"
+    logging.basicConfig(format=format, level=logging.INFO, datefmt="%H:%M:%S")
+
+    serial = JetsonSerial()
+
+    start = datetime.datetime.utcnow()
+    end = datetime.datetime.utcnow()
+
+    while (end - start).total_seconds() < 5:
+
+        serial.writeToSerial(b"\xAA\xFF\xCC")
+        if SLEEP_BETWEEN_WRITE_AND_READ:
+            time.sleep(0.1)
+        serial.readFromSerial()
+
+        end = datetime.datetime.utcnow()
+
+    serial.__del__()

Serial/tcm_parser.py

@@ -0,0 +1,127 @@
+''' File containing data parser class for TCM serial communication
+
+Authors: Maks Cebula
+'''
+
+import imp
+import struct
+from ast import Bytes
+from select import select
+from sys import byteorder
+
+
+class TCMParser:
+
+    # Dictionary for Okon TCM-board message types
+    # For reference see TCM/Src/helpers/CommunicationProtocol/communication_protocol.h
+    MSG_TYPE = {
+        "MSG_OKON_REQUEST": 0x00,
+        "MSG_OKON_SERVICE": 0x01,
+        "MSG_OKON_STICKS": 0x02,
+        "MSG_OKON_MODE": 0x03,
+        "MSG_OKON_CL_STATUS": 0x04,
+        "MSG_OKON_TYPE_COUNT": 0x05,
+    }
+
+    # Default constructor
+    def __init__(self):
+        print("Parser default constructor")
+
+    # Method calculating CRC16 with given offset and lenght of the data
+    # Returns checksum as an array of two bytes, firts is Most Significant Byte, second Last Significant Byte
+    def calculateCheckSum(self, data: bytearray, offset, length):
+
+        if data is None or offset < 0 or offset > len(data) - 1 and offset + length > len(data):
+            print("Invalid data, cannot calculate crc!")
+            return 0
+
+        crc16 = 0xFFFF
+
+        for i in range(0, length):
+            crc16 ^= data[offset + i] << 8
+
+            for j in range(0, 8):
+                if (crc16 & 0x8000) > 0:
+                    crc16 = (crc16 << 1) ^ 0x1021
+                else:
+                    crc16 = crc16 << 1
+
+        crc16 = crc16 & 0xFFFF
+
+        crc16_bytes = (crc16).to_bytes(2, byteorder="big")
+
+        # For test purposes only
+        # print(["0x%02x" % b for b in crc16_bytes])
+
+        return crc16_bytes
+
+    # Method parsing raw data to TCM compatible frame
+    # Returns array of bytes which are ready to send
+    # Input:
+    # float_data - an array of floats to be converted
+    # msg_type - string key for MSG_TYPE dictionary
+    def parseData(self, float_data, msg_type):
+
+        floats_to_bytes = self.convertFloatsToBytes(float_data)
+
+        if floats_to_bytes == 0:
+            print("Invalid bytes array, ending data parsing...")
+            return 0
+
+        data_length = (len(floats_to_bytes)).to_bytes(2, byteorder="big")
+
+        length_MSB = data_length[0]
+        length_LSB = data_length[1]
+
+        parsed_data = bytearray()
+
+        # Adding frame const header bytes
+        parsed_data += bytes.fromhex("69")  # Nice!
+        parsed_data += bytes.fromhex("68")
+
+        # Adding length bytes
+        parsed_data += length_MSB.to_bytes(1, byteorder="big")
+        parsed_data += length_LSB.to_bytes(1, byteorder="big")
+
+        # Adding message type byte
+        try:
+            parsed_data += self.MSG_TYPE[msg_type].to_bytes(1, byteorder="big")
+        except:
+            print("Invalid message type, no i co ja mam tu wstawić???")
+            return 0
+
+        # Adding converted floats
+        parsed_data += floats_to_bytes
+
+        crc = self.calculateCheckSum(floats_to_bytes, 0, len(floats_to_bytes))
+
+        # Adding checksum
+        parsed_data += crc[0].to_bytes(1, byteorder="big")
+        parsed_data += crc[1].to_bytes(1, byteorder="big")
+
+        # For tests purposes only
+        # print(["0x%02x" % b for b in parsed_data])
+
+        return parsed_data
+
+    # Method not implemented yet
+    def compareCheckSum(self, recv_data):
+        print("Parser method")
+        # calc_crc = self.calculateCheckSum(recv_data, 5, len(recv_data)-2)
+
+    def convertFloatsToBytes(self, floats_array):
+
+        converted_floats = bytearray()
+
+        if len(floats_array) == 0:
+            print("Empty array of floats, cannot convert to bytes!")
+            return 0
+
+        for float in floats_array:
+            float_as_bytes = bytearray(struct.pack("f", float))
+            converted_floats += float_as_bytes
+
+        # For tests purposes only
+        # print(["0x%02x" % b for b in converted_floats])
+
+        return converted_floats

Serial/tcm_protocol.py

@@ -0,0 +1,37 @@
+''' File containing protocol for TCM serial communication
+
+Authors: Kajetan Jonak
+'''
+
+class Protocol(object):
+     class MESSAGE_TYPE:
+        #MSGS with ID 0,1,2 are messages that are sended to okoń (there is no need to parse them)
+        REQUEST = 0x0
+        SERVICE = 0x1
+        CONTROL = 0x2
+        #MSGS below are sended from TCM to jetson
+        PID = 0x3
+        CONTROL_MATRIX = 0x4
+        HEART_BEAT = 0x5
+        SERVICE_CONFIRM = 0x6
+        TELEMETRY = 0x7
+        VARIABLE = 0x8
+
+        class REQUEST_MSG:
+            PID = 0x00
+            CONTROL_MATRIX = 0x1
+            DATA = 0x2
+
+        class SERVICE_MSG:
+            ENTER_SERVICE_MODE = 0x0
+            REBOOT = 0x01
+            ENABLE_DIRECT_MOTORS_CTRL =0x02      
+            DISABLE_DIRECT_MOTORS_CTRL = 0x03
+            DIRECT_MOTORS_CTRL = 0x4
+            DIRECT_MOTORS_CTRL_VIA_MATRIX = 0x5
+            SAVE_SETTINGS = 0x6
+            LOAD_SETTINGS_FROM_FLASH = 0x7
+            UPDATE_PID_VALUES = 0x8
+
+        class CONTROL_MSG:
+            STICK = 0x0

Serial/test.py

@@ -0,0 +1,15 @@
+''' Sample test on how to use TCMParser module
+
+Authors: Maks Cebula
+'''
+from __future__ import print_function
+
+from tcm_parser import TCMParser
+
+Parser = TCMParser()
+
+data = [10.5, 11.5, 5.0, 0.778]
+
+parsed_data = Parser.parseData(data, "MSG_OKON_STICKS")
+
+print(["0x%02x" % b for b in parsed_data])

environment.yml

@@ -1,6 +1,7 @@
 name: okon-autonomy
 dependencies:
   - numpy
+  - pyserial
   - pip
   - pip:
     - py_trees