diff --git a/Serial/jetson_serial_light.py b/Serial/jetson_serial_light.py new file mode 100644 index 0000000..e3c75fd --- /dev/null +++ b/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__() diff --git a/Serial/tcm_parser.py b/Serial/tcm_parser.py new file mode 100644 index 0000000..6e4e1e5 --- /dev/null +++ b/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 diff --git a/Serial/tcm_protocol.py b/Serial/tcm_protocol.py new file mode 100644 index 0000000..3733985 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Serial/test.py b/Serial/test.py new file mode 100644 index 0000000..cc1c097 --- /dev/null +++ b/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]) diff --git a/environment.yml b/environment.yml index 2dd33ee..30a3174 100644 --- a/environment.yml +++ b/environment.yml @@ -1,6 +1,7 @@ name: okon-autonomy dependencies: - numpy + - pyserial - pip - pip: - py_trees