From e9f94ac9f13130a83bd7720d227839fa4fc88204 Mon Sep 17 00:00:00 2001 From: mpfmorawski Date: Sat, 13 Aug 2022 20:11:36 +0200 Subject: [PATCH 1/6] Adding code prepared for serial communication --- Serial/TCMParser.py | 129 +++++++++++++++++++++ Serial/jetson_serial.py | 209 ++++++++++++++++++++++++++++++++++ Serial/jetson_serial_light.py | 175 ++++++++++++++++++++++++++++ Serial/readme | 7 ++ 4 files changed, 520 insertions(+) create mode 100644 Serial/TCMParser.py create mode 100644 Serial/jetson_serial.py create mode 100644 Serial/jetson_serial_light.py create mode 100644 Serial/readme diff --git a/Serial/TCMParser.py b/Serial/TCMParser.py new file mode 100644 index 0000000..7d634d6 --- /dev/null +++ b/Serial/TCMParser.py @@ -0,0 +1,129 @@ +# File containing data parser class for TCM serial communication + +from ast import Bytes +import imp +from select import select +from sys import byteorder +import struct + +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 \ No newline at end of file diff --git a/Serial/jetson_serial.py b/Serial/jetson_serial.py new file mode 100644 index 0000000..0d6f044 --- /dev/null +++ b/Serial/jetson_serial.py @@ -0,0 +1,209 @@ + +from multiprocessing import Lock +import threading +import time +import logging +import serial +import datetime + +class JetsonSerial: + + + def __init__(self, serial = '/dev/ttyACM0'): + + self.serial_port = serial + + 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.input_buffer = None + self.output_buffer = [] + self.input_lock = Lock() + 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 runReceiverWorker(self): + + # Read from jetson and write to device + + while ( self.serial.isOpen() and not self.crash_event.is_set() ): + + self.input_lock.acquire() + data = self.input_buffer + self.input_buffer = None + self.input_lock.release() + + if ( data == None ): + continue + + try: + self.serial.write(data) + except: + self.crash_event.set() + self.handleCrash() + logging.error("Cannot write to serial, ending...") + break + + + def runTransceiverWorker(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 + + self.output_lock.acquire() + self.output_buffer.append( self.serial.read(buff_size) ) + self.output_lock.release() + + busy = datetime.datetime.utcnow() + + + def startWorkers(self): + + self.rx_worker = threading.Thread(target=self.runReceiverWorker, daemon=True) + self.tx_worker = threading.Thread(target=self.runTransceiverWorker, daemon=True) + + self.rx_worker.start() + self.tx_worker.start() + + + def writeToSerial(self, data : bytes): + + self.input_lock.acquire() + self.input_buffer = data + self.input_lock.release() + + + def readFromSerial(self): + + self.output_lock.acquire() + + data = self.output_buffer + + if (len(data) > 0): + self.output_buffer = [] + + self.output_lock.release() + + return data + + + def handleCrash(self): + + start_time = datetime.datetime.utcnow() + + while (self.rx_worker.isAlive() or self.tx_worker.isAlive() ): + # logging.info("One thread is still alive!") + curr_time = datetime.datetime.utcnow() + if ( ( curr_time - start_time ).total_seconds() > 10 ): + break + + 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.rx_worker.join() + 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() < 2): + + # if (serial.rx_worker.is_alive() == False): + # serial.rx_worker.join() + # serial.rx_worker = threading.Thread(target=serial.runReceiverWorker, daemon=True) + # serial.rx_worker.start() + + # if (serial.tx_worker.is_alive() == False): + # serial.tx_worker.join() + # serial.tx_worker = threading.Thread(target=serial.runTransceiverWorker, daemon=True) + # serial.tx_worker.start() + + serial.writeToSerial(b'\xAA\xFF\xCC') + print(serial.readFromSerial()) + end = datetime.datetime.utcnow() + + serial.__del__() diff --git a/Serial/jetson_serial_light.py b/Serial/jetson_serial_light.py new file mode 100644 index 0000000..cc99792 --- /dev/null +++ b/Serial/jetson_serial_light.py @@ -0,0 +1,175 @@ + +from multiprocessing import Lock +import threading +import time +import logging +import serial +import datetime + +class JetsonSerial: + + + def __init__(self, serial = '/dev/ttyACM0'): + + self.serial_port = serial + + 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 runTransceiverWorker(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 + + self.output_lock.acquire() + self.output_buffer.append( self.serial.read(buff_size) ) + self.output_lock.release() + + busy = datetime.datetime.utcnow() + + + def startWorkers(self): + + self.tx_worker = threading.Thread(target=self.runTransceiverWorker, 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): + + self.output_lock.acquire() + + data = self.output_buffer + + if (len(data) > 0): + self.output_buffer = [] + + self.output_lock.release() + + 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): + + # if (serial.rx_worker.is_alive() == False): + # serial.rx_worker.join() + # serial.rx_worker = threading.Thread(target=serial.runReceiverWorker, daemon=True) + # serial.rx_worker.start() + + # if (serial.tx_worker.is_alive() == False): + # serial.tx_worker.join() + # serial.tx_worker = threading.Thread(target=serial.runTransceiverWorker, daemon=True) + # serial.tx_worker.start() + + serial.writeToSerial(b'\xAA\xFF\xCC') + print(serial.readFromSerial()) + + end = datetime.datetime.utcnow() + + serial.__del__() diff --git a/Serial/readme b/Serial/readme new file mode 100644 index 0000000..a9007b7 --- /dev/null +++ b/Serial/readme @@ -0,0 +1,7 @@ + +Co trzeba zmienić: +- nazwe wątków, w poprzednim założeniu ich nazwy miały sens, teraz należy je odwrócić (tx -> rx, rx -> tx) +- obsługa bufora odbieranych danych, obecnie część bajtów jest gubiona w trakcie czytania + +Alternatywa: + jetson_serial_light ma usunięty wątek odpowiedzialny za transmisję (nie jest potrzebny skoro jest wykonywana na żądanie), problemy te same co zwykły \ No newline at end of file From 73085f9d54f7e64ca8b6d2c532de178acbfe2145 Mon Sep 17 00:00:00 2001 From: mpfmorawski Date: Sat, 13 Aug 2022 20:12:37 +0200 Subject: [PATCH 2/6] Updateing the environment for serial communication --- environment.yml | 1 + 1 file changed, 1 insertion(+) 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 From 5f88855ddd33736d09c22ddb060e70d355c90479 Mon Sep 17 00:00:00 2001 From: Maciej Morawski <62252131+mpfmorawski@users.noreply.github.com> Date: Tue, 16 Aug 2022 16:41:14 +0200 Subject: [PATCH 3/6] Adding TCM Protocol file made by Kajtek Jonak --- Serial/TCM_proto.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 Serial/TCM_proto.py diff --git a/Serial/TCM_proto.py b/Serial/TCM_proto.py new file mode 100644 index 0000000..1498883 --- /dev/null +++ b/Serial/TCM_proto.py @@ -0,0 +1,32 @@ +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 From adee59222d197fbde05a3543b52533d526fc1295 Mon Sep 17 00:00:00 2001 From: mpfmorawski Date: Tue, 16 Aug 2022 19:08:19 +0200 Subject: [PATCH 4/6] Maks Cebula's update of jetson_serial code --- Serial/jetson_serial.py | 5 +++-- Serial/jetson_serial_light.py | 17 +++++++++++------ Serial/test.py | 11 +++++++++++ 3 files changed, 25 insertions(+), 8 deletions(-) create mode 100644 Serial/test.py diff --git a/Serial/jetson_serial.py b/Serial/jetson_serial.py index 0d6f044..43e373f 100644 --- a/Serial/jetson_serial.py +++ b/Serial/jetson_serial.py @@ -9,7 +9,7 @@ class JetsonSerial: - def __init__(self, serial = '/dev/ttyACM0'): + def __init__(self, serial = '/dev/ttyUSB0'): self.serial_port = serial @@ -190,7 +190,7 @@ def __del__(self): start = datetime.datetime.utcnow() end = datetime.datetime.utcnow() - while((end - start).total_seconds() < 2): + while((end - start).total_seconds() < 5): # if (serial.rx_worker.is_alive() == False): # serial.rx_worker.join() @@ -204,6 +204,7 @@ def __del__(self): serial.writeToSerial(b'\xAA\xFF\xCC') print(serial.readFromSerial()) + end = datetime.datetime.utcnow() serial.__del__() diff --git a/Serial/jetson_serial_light.py b/Serial/jetson_serial_light.py index cc99792..8bc6347 100644 --- a/Serial/jetson_serial_light.py +++ b/Serial/jetson_serial_light.py @@ -9,7 +9,7 @@ class JetsonSerial: - def __init__(self, serial = '/dev/ttyACM0'): + def __init__(self, serial = '/dev/ttyUSB0'): self.serial_port = serial @@ -69,6 +69,11 @@ def runTransceiverWorker(self): continue + + # Sensitive part, beware death locks + while (self.output_lock.locked()): + pass + self.output_lock.acquire() self.output_buffer.append( self.serial.read(buff_size) ) self.output_lock.release() @@ -93,13 +98,13 @@ def writeToSerial(self, data : bytes): def readFromSerial(self): - self.output_lock.acquire() + # Sensitive part, beware death locks + while (self.output_lock.locked()): + pass + self.output_lock.acquire() data = self.output_buffer - - if (len(data) > 0): - self.output_buffer = [] - + self.output_buffer.clear() self.output_lock.release() return data diff --git a/Serial/test.py b/Serial/test.py new file mode 100644 index 0000000..6a2a18c --- /dev/null +++ b/Serial/test.py @@ -0,0 +1,11 @@ +from __future__ import print_function +from TCMParser 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]) + From d4d7418c583e3d5a772ab42937565743bf36f5e0 Mon Sep 17 00:00:00 2001 From: mpfmorawski Date: Tue, 16 Aug 2022 20:24:24 +0200 Subject: [PATCH 5/6] Adding print in proper place to check if code works --- Serial/jetson_serial_light.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Serial/jetson_serial_light.py b/Serial/jetson_serial_light.py index 8bc6347..6742863 100644 --- a/Serial/jetson_serial_light.py +++ b/Serial/jetson_serial_light.py @@ -1,5 +1,3 @@ - -from multiprocessing import Lock import threading import time import logging @@ -9,7 +7,7 @@ class JetsonSerial: - def __init__(self, serial = '/dev/ttyUSB0'): + def __init__(self, serial = '/dev/ttyACM0'): self.serial_port = serial @@ -28,7 +26,7 @@ def InitSerial(self, baud_rate = 115200, timeout = 0.5, parity = serial.PARITY_N self.serial.parity = parity self.output_buffer = [] - self.output_lock = Lock() + self.output_lock = threading.Lock() # Try open serial to check if it is available try: @@ -43,7 +41,7 @@ def InitSerial(self, baud_rate = 115200, timeout = 0.5, parity = serial.PARITY_N self.startWorkers() - def runTransceiverWorker(self): + def runWorker(self): # Read from device and write to jetson @@ -75,7 +73,9 @@ def runTransceiverWorker(self): pass self.output_lock.acquire() - self.output_buffer.append( self.serial.read(buff_size) ) + buffer = self.serial.read(buff_size) + self.output_buffer.append(buffer) + print(buffer) self.output_lock.release() busy = datetime.datetime.utcnow() @@ -83,7 +83,7 @@ def runTransceiverWorker(self): def startWorkers(self): - self.tx_worker = threading.Thread(target=self.runTransceiverWorker, daemon=True) + self.tx_worker = threading.Thread(target=self.runWorker, daemon=True) self.tx_worker.start() @@ -106,7 +106,7 @@ def readFromSerial(self): data = self.output_buffer self.output_buffer.clear() self.output_lock.release() - + return data @@ -169,11 +169,11 @@ def __del__(self): # if (serial.tx_worker.is_alive() == False): # serial.tx_worker.join() - # serial.tx_worker = threading.Thread(target=serial.runTransceiverWorker, daemon=True) + # serial.tx_worker = threading.Thread(target=serial.runWorker, daemon=True) # serial.tx_worker.start() serial.writeToSerial(b'\xAA\xFF\xCC') - print(serial.readFromSerial()) + serial.readFromSerial() end = datetime.datetime.utcnow() From 4eebacebea489f91f5c22879aab6edae60d54f63 Mon Sep 17 00:00:00 2001 From: mpfmorawski Date: Wed, 17 Aug 2022 20:16:00 +0200 Subject: [PATCH 6/6] Changing Lock in jetson_serial_light to multiprocessing.Lock --- Serial/jetson_serial.py | 210 ----------------------- Serial/jetson_serial_light.py | 162 +++++++++-------- Serial/readme | 7 - Serial/{TCMParser.py => tcm_parser.py} | 64 ++++--- Serial/{TCM_proto.py => tcm_protocol.py} | 7 +- Serial/test.py | 8 +- 6 files changed, 138 insertions(+), 320 deletions(-) delete mode 100644 Serial/jetson_serial.py delete mode 100644 Serial/readme rename Serial/{TCMParser.py => tcm_parser.py} (69%) rename Serial/{TCM_proto.py => tcm_protocol.py} (86%) diff --git a/Serial/jetson_serial.py b/Serial/jetson_serial.py deleted file mode 100644 index 43e373f..0000000 --- a/Serial/jetson_serial.py +++ /dev/null @@ -1,210 +0,0 @@ - -from multiprocessing import Lock -import threading -import time -import logging -import serial -import datetime - -class JetsonSerial: - - - def __init__(self, serial = '/dev/ttyUSB0'): - - self.serial_port = serial - - 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.input_buffer = None - self.output_buffer = [] - self.input_lock = Lock() - 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 runReceiverWorker(self): - - # Read from jetson and write to device - - while ( self.serial.isOpen() and not self.crash_event.is_set() ): - - self.input_lock.acquire() - data = self.input_buffer - self.input_buffer = None - self.input_lock.release() - - if ( data == None ): - continue - - try: - self.serial.write(data) - except: - self.crash_event.set() - self.handleCrash() - logging.error("Cannot write to serial, ending...") - break - - - def runTransceiverWorker(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 - - self.output_lock.acquire() - self.output_buffer.append( self.serial.read(buff_size) ) - self.output_lock.release() - - busy = datetime.datetime.utcnow() - - - def startWorkers(self): - - self.rx_worker = threading.Thread(target=self.runReceiverWorker, daemon=True) - self.tx_worker = threading.Thread(target=self.runTransceiverWorker, daemon=True) - - self.rx_worker.start() - self.tx_worker.start() - - - def writeToSerial(self, data : bytes): - - self.input_lock.acquire() - self.input_buffer = data - self.input_lock.release() - - - def readFromSerial(self): - - self.output_lock.acquire() - - data = self.output_buffer - - if (len(data) > 0): - self.output_buffer = [] - - self.output_lock.release() - - return data - - - def handleCrash(self): - - start_time = datetime.datetime.utcnow() - - while (self.rx_worker.isAlive() or self.tx_worker.isAlive() ): - # logging.info("One thread is still alive!") - curr_time = datetime.datetime.utcnow() - if ( ( curr_time - start_time ).total_seconds() > 10 ): - break - - 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.rx_worker.join() - 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): - - # if (serial.rx_worker.is_alive() == False): - # serial.rx_worker.join() - # serial.rx_worker = threading.Thread(target=serial.runReceiverWorker, daemon=True) - # serial.rx_worker.start() - - # if (serial.tx_worker.is_alive() == False): - # serial.tx_worker.join() - # serial.tx_worker = threading.Thread(target=serial.runTransceiverWorker, daemon=True) - # serial.tx_worker.start() - - serial.writeToSerial(b'\xAA\xFF\xCC') - print(serial.readFromSerial()) - - end = datetime.datetime.utcnow() - - serial.__del__() diff --git a/Serial/jetson_serial_light.py b/Serial/jetson_serial_light.py index 6742863..e3c75fd 100644 --- a/Serial/jetson_serial_light.py +++ b/Serial/jetson_serial_light.py @@ -1,23 +1,52 @@ +''' 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 -import logging +from multiprocessing import Lock + import serial -import datetime -class JetsonSerial: +DEVICE_PORT = "/dev/ttyACM0" +# SET BUFFER DEBUG MODE +# False - without printing anything +# True - print information about buffer (worker) +BUFFER_DEBUG_MODE = True - def __init__(self, serial = '/dev/ttyACM0'): +# SET DATA DEBUG MODE +# False - without printing anything +# True - print information about data (readFromSerial method) +DATA_DEBUG_MODE = True - self.serial_port = serial +# 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): - 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 @@ -26,7 +55,7 @@ def InitSerial(self, baud_rate = 115200, timeout = 0.5, parity = serial.PARITY_N self.serial.parity = parity self.output_buffer = [] - self.output_lock = threading.Lock() + self.output_lock = Lock() # Try open serial to check if it is available try: @@ -38,16 +67,15 @@ def InitSerial(self, baud_rate = 115200, timeout = 0.5, parity = serial.PARITY_N logging.error("Couldn't open serial: {}".format(self.serial_port)) exit() - self.startWorkers() - + 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() ): + while self.serial.isOpen() and not self.crash_event.is_set(): try: buff_size = self.serial.inWaiting() @@ -56,87 +84,96 @@ def runWorker(self): self.crash_event.set() self.handleCrash() break - - if ( buff_size == 0 ): + + if buff_size == 0: not_busy = datetime.datetime.utcnow() - if ( ( (not_busy - busy).total_seconds() ) > 5 ): + 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 - # Sensitive part, beware death locks - while (self.output_lock.locked()): - pass - - self.output_lock.acquire() - buffer = self.serial.read(buff_size) - self.output_buffer.append(buffer) - print(buffer) - self.output_lock.release() - - busy = datetime.datetime.utcnow() - + 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): - def writeToSerial(self, data : bytes): - try: self.serial.write(data) except: logging.error("Cannot write to serial, ending...") - - def readFromSerial(self): - - # Sensitive part, beware death locks - while (self.output_lock.locked()): - pass - - self.output_lock.acquire() - data = self.output_buffer - self.output_buffer.clear() - self.output_lock.release() - - return data + 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 + # Close serial self.serial.close() # Try reopen counter = 10 - while counter > 0 : + while counter > 0: try: self.serial.open() break except: time.sleep(0.5) - counter = counter-1 + counter = counter - 1 logging.error("Cannot open serial port!") - - if not ( self.serial.isOpen() ): + + if not (self.serial.isOpen()): exit() - - self.startWorkers() + self.startWorkers() def __del__(self): - + print("Im cleaning...") self.crash_event.set() @@ -147,32 +184,23 @@ def __del__(self): 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") + 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): - - # if (serial.rx_worker.is_alive() == False): - # serial.rx_worker.join() - # serial.rx_worker = threading.Thread(target=serial.runReceiverWorker, daemon=True) - # serial.rx_worker.start() - - # if (serial.tx_worker.is_alive() == False): - # serial.tx_worker.join() - # serial.tx_worker = threading.Thread(target=serial.runWorker, daemon=True) - # serial.tx_worker.start() + while (end - start).total_seconds() < 5: - serial.writeToSerial(b'\xAA\xFF\xCC') + serial.writeToSerial(b"\xAA\xFF\xCC") + if SLEEP_BETWEEN_WRITE_AND_READ: + time.sleep(0.1) serial.readFromSerial() end = datetime.datetime.utcnow() diff --git a/Serial/readme b/Serial/readme deleted file mode 100644 index a9007b7..0000000 --- a/Serial/readme +++ /dev/null @@ -1,7 +0,0 @@ - -Co trzeba zmienić: -- nazwe wątków, w poprzednim założeniu ich nazwy miały sens, teraz należy je odwrócić (tx -> rx, rx -> tx) -- obsługa bufora odbieranych danych, obecnie część bajtów jest gubiona w trakcie czytania - -Alternatywa: - jetson_serial_light ma usunięty wątek odpowiedzialny za transmisję (nie jest potrzebny skoro jest wykonywana na żądanie), problemy te same co zwykły \ No newline at end of file diff --git a/Serial/TCMParser.py b/Serial/tcm_parser.py similarity index 69% rename from Serial/TCMParser.py rename to Serial/tcm_parser.py index 7d634d6..6e4e1e5 100644 --- a/Serial/TCMParser.py +++ b/Serial/tcm_parser.py @@ -1,12 +1,16 @@ -# File containing data parser class for TCM serial communication +''' File containing data parser class for TCM serial communication + +Authors: Maks Cebula +''' -from ast import Bytes import imp +import struct +from ast import Bytes from select import select from sys import byteorder -import struct -class TCMParser : + +class TCMParser: # Dictionary for Okon TCM-board message types # For reference see TCM/Src/helpers/CommunicationProtocol/communication_protocol.h @@ -16,20 +20,18 @@ class TCMParser : "MSG_OKON_STICKS": 0x02, "MSG_OKON_MODE": 0x03, "MSG_OKON_CL_STATUS": 0x04, - "MSG_OKON_TYPE_COUNT": 0x05 + "MSG_OKON_TYPE_COUNT": 0x05, } - # Default constructor - def __init__(self) : + 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) : + def calculateCheckSum(self, data: bytearray, offset, length): - if data is None or offset < 0 or offset > len(data)- 1 and offset+length > len(data) : + 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 @@ -38,36 +40,35 @@ def calculateCheckSum(self, data : bytearray, offset, length) : for i in range(0, length): crc16 ^= data[offset + i] << 8 - for j in range(0,8): + for j in range(0, 8): if (crc16 & 0x8000) > 0: - crc16 =(crc16 << 1) ^ 0x1021 + crc16 = (crc16 << 1) ^ 0x1021 else: crc16 = crc16 << 1 crc16 = crc16 & 0xFFFF - crc16_bytes = (crc16).to_bytes(2, byteorder='big') + 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) : + def parseData(self, float_data, msg_type): floats_to_bytes = self.convertFloatsToBytes(float_data) - if (floats_to_bytes == 0): + 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') + data_length = (len(floats_to_bytes)).to_bytes(2, byteorder="big") length_MSB = data_length[0] length_LSB = data_length[1] @@ -75,16 +76,16 @@ def parseData(self, float_data, msg_type) : parsed_data = bytearray() # Adding frame const header bytes - parsed_data += bytes.fromhex('69') # Nice! - parsed_data += bytes.fromhex('68') + 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') + 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') + 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 @@ -95,35 +96,32 @@ def parseData(self, float_data, msg_type) : 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') + 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) : + def compareCheckSum(self, recv_data): print("Parser method") # calc_crc = self.calculateCheckSum(recv_data, 5, len(recv_data)-2) + def convertFloatsToBytes(self, floats_array): - - def convertFloatsToBytes(self, floats_array) : - converted_floats = bytearray() - if len(floats_array) == 0 : + 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)) + 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 \ No newline at end of file + return converted_floats diff --git a/Serial/TCM_proto.py b/Serial/tcm_protocol.py similarity index 86% rename from Serial/TCM_proto.py rename to Serial/tcm_protocol.py index 1498883..3733985 100644 --- a/Serial/TCM_proto.py +++ b/Serial/tcm_protocol.py @@ -1,3 +1,8 @@ +''' 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) @@ -28,5 +33,5 @@ class SERVICE_MSG: LOAD_SETTINGS_FROM_FLASH = 0x7 UPDATE_PID_VALUES = 0x8 - class CONTROL_MSG + class CONTROL_MSG: STICK = 0x0 \ No newline at end of file diff --git a/Serial/test.py b/Serial/test.py index 6a2a18c..cc1c097 100644 --- a/Serial/test.py +++ b/Serial/test.py @@ -1,5 +1,10 @@ +''' Sample test on how to use TCMParser module + +Authors: Maks Cebula +''' from __future__ import print_function -from TCMParser import TCMParser + +from tcm_parser import TCMParser Parser = TCMParser() @@ -8,4 +13,3 @@ parsed_data = Parser.parseData(data, "MSG_OKON_STICKS") print(["0x%02x" % b for b in parsed_data]) -