Skip to content

Commit

Permalink
Merge pull request #1 from Lauszus/feature/serial_open
Browse files Browse the repository at this point in the history 
Some improvements when uploading via serial/USB
  • Loading branch information
@Lauszus
Lauszus authored Jan 26, 2024
2 parents 7a7a5b9 + 73b8c5c commit f94e37e
Showing 1 changed file with 96 additions and 74 deletions.
170 changes: 96 additions & 74 deletions pybldc/pybldc.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
import abc
import argparse
import logging
import math
import queue
import threading
import time
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand All @@ -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:
Expand Down Expand Up @@ -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()

Expand Down Expand Up @@ -520,17 +522,17 @@ 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",
args=(
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
Expand All @@ -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:
Expand All @@ -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,
Expand All @@ -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"')

Expand Down

0 comments on commit f94e37e

Please sign in to comment.