diff --git a/pybldc/pybldc.py b/pybldc/pybldc.py index a38ded8..19fe70f 100644 --- a/pybldc/pybldc.py +++ b/pybldc/pybldc.py @@ -24,6 +24,7 @@ import abc import argparse import logging +import math import queue import threading import time @@ -116,7 +117,7 @@ def __exit__(self, exc_type: Type[BaseException], exc_value: BaseException, trac self.shutdown() @abc.abstractmethod - def shutdown(self, timeout: float = 1.0) -> None: + def shutdown(self, timeout: Optional[float] = 1.0) -> None: pass @staticmethod @@ -317,7 +318,7 @@ def _wait_for_packet_response( expected_response: List[int], timeout: float, ) -> Optional[bool]: - if timeout == 0.0: + if math.isclose(timeout, 0.0): # We do not actually care about the answer, so return immediately return None @@ -332,7 +333,6 @@ def _wait_for_packet_response( self._logger.debug(f"PyBldcBase: Received packet response: {response}, expected: {expected_response}") # Make sure it replies with the command as the first byte and "OK" as the second byte - # Some commands repeat the data if response[0] == comm_packet_id: return response[1] == 1 and response[2:] == expected_response except queue.Empty: @@ -427,7 +427,9 @@ def __init__( def __enter__(self) -> "PyBldcCan": return self - def shutdown(self, timeout: float = 1.0) -> None: + def shutdown(self, timeout: Optional[float] = 1.0) -> None: + if timeout is None: + raise ValueError('A timeout of "None" is not supported') self._can_notifier.stop(timeout=timeout) self._can_bus.shutdown() @@ -520,9 +522,9 @@ def __init__( super().__init__(logger=logger) # Open the serial port, but read from it in a thread, so we are not blocking the main loop - self._serial = serial.Serial(port=port, baudrate=baudrate, timeout=0.5) + self._serial = serial.Serial(port=port, baudrate=baudrate, timeout=0.5, exclusive=True) self._shutdown_thread = threading.Event() - self._received_packet_queue: queue.Queue[List[int]] = queue.Queue() + self._packet_queue: queue.Queue[List[int]] = queue.Queue() self._thread = threading.Thread( target=self._serial_read_thread, name="_serial_read_thread", @@ -530,7 +532,7 @@ def __init__( self._serial, self._shutdown_thread, self._logger, - self._received_packet_queue, + self._packet_queue, ), ) self._thread.daemon = False # Make sure the application joins this before closing down @@ -539,14 +541,19 @@ def __init__( def __enter__(self) -> "PyBldcSerial": return self - def shutdown(self, timeout: float = 1.0) -> None: + def shutdown(self, timeout: Optional[float] = 1.0) -> None: self._shutdown_thread.set() self._thread.join(timeout=timeout) self._serial.close() def ping(self, timeout: float = 1.0) -> bool: # We assume that we can ping it if the serial port is open - return bool(self._serial.is_open) + self._logger.info("PyBldcSerial: Checking if serial port is open") + retval = bool(self._serial.is_open) + if not retval: + # Sleep if the port is not open or this will just fail immediately when called again + time.sleep(timeout) + return retval def _send_implementation(self, data: List[int], expected_response: List[int], timeout: float) -> Optional[bool]: if len(data) <= 255: @@ -569,12 +576,16 @@ def _send_implementation(self, data: List[int], expected_response: List[int], ti send_buffer.extend(PyBldcBase._pack_uint16(crc16_ccitt(data))) # Checksum send_buffer.append(3) # Stop byte + # Make sure the serial port is still open + if not self._serial.is_open: + return False + # Send the buffer on the serial interface self._serial.write(send_buffer) # Wait for the response return self._wait_for_packet_response( - packet_queue=self._received_packet_queue, + packet_queue=self._packet_queue, comm_packet_id=cast(CommPacketId, data[0]), expected_response=expected_response, timeout=timeout, @@ -585,80 +596,91 @@ def _serial_read_thread( ser: Any, shutdown_event: threading.Event, logger: logging.Logger, - received_packet_queue: queue.Queue[List[int]], + packet_queue: queue.Queue[List[int]], ) -> None: try: data_buffer = bytearray() - while not shutdown_event.is_set() and ser.is_open: - data = ser.read() - if data: - data_buffer += data - - while True: - # Based on "try_decode_packet" in "vesc_tool/packet.cpp" - if len(data_buffer) == 0: - break - - is_len_8b = data_buffer[0] == 2 - is_len_16b = data_buffer[0] == 3 - is_len_24b = data_buffer[0] == 4 - - if not is_len_8b and not is_len_16b and not is_len_24b: - logger.warning(f"_serial_read_thread: Discarding invalid data: {data_buffer[0]}") + + while not shutdown_event.is_set(): + if not ser.is_open: + try: + ser.open() + logger.debug("BlhostSerial: Serial port was re-opened") + except serial.serialutil.SerialException: + logger.debug("BlhostSerial: Failed to open serial port", exc_info=True) + time.sleep(1.0) + continue + + try: + data_buffer += ser.read() + except serial.serialutil.SerialException: + # This is triggered when the VESC jumps to the bootloader, as the serial port will be closed + ser.close() + logger.debug( + 'BlhostSerial: Caught SerialException exception in "_serial_read_thread"', exc_info=True + ) + time.sleep(1.0) + continue + + # Based on "try_decode_packet" in "vesc_tool/packet.cpp" + while len(data_buffer) > 0: + is_len_8b = data_buffer[0] == 2 + is_len_16b = data_buffer[0] == 3 + is_len_24b = data_buffer[0] == 4 + + if not is_len_8b and not is_len_16b and not is_len_24b: + logger.warning(f"_serial_read_thread: Discarding invalid data: {data_buffer[0]}") + data_buffer = data_buffer[1:] # Discard the fist byte + continue + + data_start = data_buffer[0] + if len(data_buffer) < data_start: + # We need more data before we can read the data + break + + if is_len_8b: + data_len = data_buffer[1] + if data_len < 1: + logger.warning(f"_serial_read_thread: Data len is not valid: {data_len} < 1") data_buffer = data_buffer[1:] # Discard the fist byte continue - - data_start = data_buffer[0] - if len(data_buffer) < data_start: - # We need more data before we can read the data - break - - if is_len_8b: - data_len = data_buffer[1] - if data_len < 1: - logger.warning(f"_serial_read_thread: Data len is not valid: {data_len} < 1") - data_buffer = data_buffer[1:] # Discard the fist byte - continue - elif is_len_16b: - data_len = data_buffer[1] << 8 | data_buffer[2] - if data_len < 255: - logger.warning(f"_serial_read_thread: Packet is too short: {data_len} < 255") - data_buffer = data_buffer[1:] # Discard the fist byte - continue - else: - data_len = data_buffer[1] << 16 | data_buffer[2] << 8 | data_buffer[3] - if data_len < 65535: - logger.warning(f"_serial_read_thread: Packet is too short: {data_len} < 65535") - data_buffer = data_buffer[1:] # Discard the fist byte - continue - - if len(data_buffer) < data_len + data_start + 3: - # Need more data to determine rest of packet - break - - parsed_data = data_buffer[data_start : data_start + data_len] - if ( - crc16_ccitt(parsed_data) - != data_buffer[data_start + data_len] << 8 | data_buffer[data_start + data_len + 1] - ): - logger.warning("_serial_read_thread: CRC failed") + elif is_len_16b: + data_len = data_buffer[1] << 8 | data_buffer[2] + if data_len < 255: + logger.warning(f"_serial_read_thread: Packet is too short: {data_len} < 255") data_buffer = data_buffer[1:] # Discard the fist byte continue - - stop_byte = data_buffer[data_start + data_len + 2] - if stop_byte != 3: - logger.warning(f"_serial_read_thread: Invalid stop byte: {stop_byte}") + else: + data_len = data_buffer[1] << 16 | data_buffer[2] << 8 | data_buffer[3] + if data_len < 65535: + logger.warning(f"_serial_read_thread: Packet is too short: {data_len} < 65535") data_buffer = data_buffer[1:] # Discard the fist byte continue - # The data is now parsed, so advance the buffer to the next message - data_buffer = data_buffer[data_start + data_len + 3 :] - received_packet_queue.put_nowait(list(parsed_data)) - - logger.debug(f"_serial_read_thread: Packet response: {list(parsed_data)})") - except serial.serialutil.SerialException: - # This is triggered when the VESC jumps to the bootloader, so just ignore it - logger.debug('BlhostSerial: Caught SerialException exception in "_serial_read_thread"', exc_info=True) + if len(data_buffer) < data_len + data_start + 3: + # Need more data to determine rest of packet + break + + parsed_data = data_buffer[data_start : data_start + data_len] + if ( + crc16_ccitt(parsed_data) + != data_buffer[data_start + data_len] << 8 | data_buffer[data_start + data_len + 1] + ): + logger.warning("_serial_read_thread: CRC failed") + data_buffer = data_buffer[1:] # Discard the fist byte + continue + + stop_byte = data_buffer[data_start + data_len + 2] + if stop_byte != 3: + logger.warning(f"_serial_read_thread: Invalid stop byte: {stop_byte}") + data_buffer = data_buffer[1:] # Discard the fist byte + continue + + # The data is now parsed, so advance the buffer to the next message + data_buffer = data_buffer[data_start + data_len + 3 :] + packet_queue.put_nowait(list(parsed_data)) + + logger.debug(f"_serial_read_thread: Packet response: {list(parsed_data)})") except Exception: logger.exception('BlhostSerial: Caught exception in "_serial_read_thread"')