From 11c5d4c173cc5808e26bb197620f0f496878ee9c Mon Sep 17 00:00:00 2001 From: VESICOLOS Date: Mon, 23 Mar 2026 17:45:24 +0100 Subject: [PATCH 1/3] Fixed _sync_read. Addresses #6: `_sync_read` receives one big package from the serial bus, so we shouldn't call `read_response` more than once. This trusts that the buffer read by `read_response` is big enough so that all returned data fits into it. It's currently at 1024 bytes, which means that reading a 2-byte word from up to 128 servos should be OK. Leaving this open for a future patch. --- src/python_st3215/st3215.py | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/python_st3215/st3215.py b/src/python_st3215/st3215.py index 4d25a1d..5f052db 100644 --- a/src/python_st3215/st3215.py +++ b/src/python_st3215/st3215.py @@ -448,19 +448,22 @@ def _sync_read( parameters = [address, data_length, *servo_ids] packet = self.send_instruction(0xFE, Instruction.SYNC_READ, parameters) responses: dict[int, Optional[dict[str, object]]] = {} - for servo_id in servo_ids: - response = self.read_response(packet) - if response: - parsed = self.parse_response(response) - self.logger.debug( - f"Servo {servo_id}: received SYNC READ response {parsed}" - ) - responses[servo_id] = parsed - else: + rx = self.read_response(packet) + if rx is None: + return None + b = 0 + while b+3 < len(rx) and rx[b]==0xFF and rx[b+1]==0xFF: + servo_id = rx[b+2] + paramlen = rx[b+3] + pkglen = paramlen + 4 + if paramlen != data_length + 2: self.logger.warning( - f"Servo {servo_id}: no response received for SYNC READ" + f"Servo {servo_id}: no valid response for SYNC READ" ) responses[servo_id] = None + break + responses[servo_id] = self.parse_response(rx[b:b+pkglen]) + b += pkglen return responses def __enter__(self) -> "ST3215": From 2bffa43435552096ab8ff11f1459776a6ac7110c Mon Sep 17 00:00:00 2001 From: VESICOLOS Date: Mon, 23 Mar 2026 18:32:24 +0100 Subject: [PATCH 2/3] Fixed signed-word issues. The servos encode signed words with a dedicated sign bit, and not in 2s-complement signed words. Also, the running speed is a signed word, this was missing. --- src/python_st3215/decorators.py | 16 +++++++--------- src/python_st3215/registers.py | 4 ++-- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/python_st3215/decorators.py b/src/python_st3215/decorators.py index 863f91e..1b79395 100644 --- a/src/python_st3215/decorators.py +++ b/src/python_st3215/decorators.py @@ -44,19 +44,17 @@ def wrapper(self: Any, value: int, *args: Any, **kwargs: Any) -> Any: def encode_signed_word(value: int) -> tuple[int, int]: """ Encode a signed 16-bit value to low and high bytes. + Encoding uses most-significant bit as sign bit. Args: - value: Signed integer (-32768 to 32767) + value: Signed integer (-32767 to 32767) Returns: Tuple of (low_byte, high_byte) """ + low, high = abs(value) & 0xFF, (abs(value) >> 8) & 0x7F if value < 0: - raw = 65536 + value - else: - raw = value - low = raw & 0xFF - high = (raw >> 8) & 0xFF + high |= 0x80 return low, high @@ -65,13 +63,13 @@ def decode_signed_word(raw: int) -> int: Decode a 16-bit raw value to signed integer. Args: - raw: Raw 16-bit value (0-65535) + raw: Raw 16-bit value (0-65535) with dedicated sign bit. Returns: - Signed integer (-32768 to 32767) + Signed integer (-32767 to 32767) """ if raw & 0x8000: - return raw - 65536 + return -(raw & 0x7FFF) return raw diff --git a/src/python_st3215/registers.py b/src/python_st3215/registers.py index 7809676..f13d193 100644 --- a/src/python_st3215/registers.py +++ b/src/python_st3215/registers.py @@ -1268,7 +1268,7 @@ def read_current_speed(self) -> Optional[int]: Returns: int: Current speed in steps/s. Returns None if read fails. """ - return read_word(self.servo, 0x3A) + return read_word(self.servo, 0x3A, signed=True) def sync_read_current_speed(self, servo_ids: list[int]) -> dict[int, Optional[int]]: """ @@ -1295,7 +1295,7 @@ def sync_read_current_speed(self, servo_ids: list[int]) -> dict[int, Optional[in raw = data[0] | (data[1] << 8) else: raw = data[0] | (data[1] << 8) - results[servo_id] = raw + results[servo_id] = decode_signed_word(raw) else: results[servo_id] = None return results From 99d2c711ad5a900477e14212dc7caaf631a7ae2a Mon Sep 17 00:00:00 2001 From: Thomas Voigtmann Date: Fri, 27 Mar 2026 16:25:48 +0100 Subject: [PATCH 3/3] Fixed errors pointed out by Copilot. --- src/python_st3215/decorators.py | 2 ++ src/python_st3215/st3215.py | 9 ++++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/python_st3215/decorators.py b/src/python_st3215/decorators.py index 1b79395..925e30c 100644 --- a/src/python_st3215/decorators.py +++ b/src/python_st3215/decorators.py @@ -52,6 +52,8 @@ def encode_signed_word(value: int) -> tuple[int, int]: Returns: Tuple of (low_byte, high_byte) """ + if value < -32767 or value > 32767: + raise ValueError low, high = abs(value) & 0xFF, (abs(value) >> 8) & 0x7F if value < 0: high |= 0x80 diff --git a/src/python_st3215/st3215.py b/src/python_st3215/st3215.py index 5f052db..e29e358 100644 --- a/src/python_st3215/st3215.py +++ b/src/python_st3215/st3215.py @@ -447,10 +447,12 @@ def _sync_read( ) parameters = [address, data_length, *servo_ids] packet = self.send_instruction(0xFE, Instruction.SYNC_READ, parameters) - responses: dict[int, Optional[dict[str, object]]] = {} + responses: dict[int, Optional[dict[str, object]]] = { + servo_id: None for servo_id in servo_ids + } rx = self.read_response(packet) if rx is None: - return None + return responses b = 0 while b+3 < len(rx) and rx[b]==0xFF and rx[b+1]==0xFF: servo_id = rx[b+2] @@ -462,7 +464,8 @@ def _sync_read( ) responses[servo_id] = None break - responses[servo_id] = self.parse_response(rx[b:b+pkglen]) + if b+pkglen <= len(rx): + responses[servo_id] = self.parse_response(rx[b:b+pkglen]) b += pkglen return responses