diff --git a/src/python_st3215/decorators.py b/src/python_st3215/decorators.py index 863f91e..925e30c 100644 --- a/src/python_st3215/decorators.py +++ b/src/python_st3215/decorators.py @@ -44,19 +44,19 @@ 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) """ + if value < -32767 or value > 32767: + raise ValueError + 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 +65,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 diff --git a/src/python_st3215/st3215.py b/src/python_st3215/st3215.py index 4d25a1d..e29e358 100644 --- a/src/python_st3215/st3215.py +++ b/src/python_st3215/st3215.py @@ -447,20 +447,26 @@ 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: + 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 responses + 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 + if b+pkglen <= len(rx): + responses[servo_id] = self.parse_response(rx[b:b+pkglen]) + b += pkglen return responses def __enter__(self) -> "ST3215":