#!/usr/bin/env python3 """ Decoder Module - vzug-e-hinge ============================== Decodes raw UART and I2C data into telemetry fields. UART Packet Format (14 bytes, Little Endian / LSB first): Byte 0-1: Start marker EF FE Byte 2-3: MotorCurrent (signed 16-bit, LSB first) Byte 4-5: EncoderValue (signed 16-bit, LSB first) Byte 6-7: RelativeEncoderValue (signed 16-bit, LSB first) Byte 8-9: V24PecDiff (signed 16-bit, LSB first) Byte 10-11: Reserved0 (signed 16-bit, LSB first) Byte 12: PWM (unsigned 8-bit, 0-100) Byte 13: End marker EE I2C Format (2 bytes, 14-bit angle from AS5600/AS5048A): Byte 0: High byte (bits 13-6) Byte 1: Low byte (bits 5-0) Reference: eHinge Encoder.pdf, Datalogger files Author: Kynsight Version: 2.0.0 (full decoding) Date: 2025-11-22 """ from typing import Dict, Any, Optional # UART Packet Constants FRAME_LEN = 14 START0 = 0xEF START1 = 0xFE END = 0xEE def _le_s16(b0: int, b1: int) -> int: """Decode Little Endian signed 16-bit value (LSB first).""" u = (b1 << 8) | b0 # b1 is high byte, b0 is low byte return u - 0x10000 if (u & 0x8000) else u def decode_uart_packet(packet_bytes: bytes) -> Dict[str, Any]: """ Decode UART packet bytes into telemetry fields. Packet format: EF FE [data] EE (14 bytes total, Little Endian / LSB first) Args: packet_bytes: Raw packet bytes (14 bytes including start/end markers) Returns: Dictionary with decoded fields or None values if invalid Example: packet_bytes = b'\\xEF\\xFE\\x04\\x00\\xED\\xFF\\x00\\x00\\x20\\x08\\x00\\x00\\x00\\xEE' decoded = decode_uart_packet(packet_bytes) # Returns: {'motor_current': 4, 'encoder_value': -19, 'v24_pec_diff': 2080, ...} """ # Validate packet if packet_bytes is None or len(packet_bytes) != FRAME_LEN: return { 'motor_current': None, 'encoder_value': None, 'relative_encoder_value': None, 'v24_pec_diff': None, 'pwm': None, 'reserved0': None, 'error': f'Invalid packet length: {len(packet_bytes) if packet_bytes else 0}, expected {FRAME_LEN}' } # Verify markers if not (packet_bytes[0] == START0 and packet_bytes[1] == START1 and packet_bytes[13] == END): return { 'motor_current': None, 'encoder_value': None, 'relative_encoder_value': None, 'v24_pec_diff': None, 'pwm': None, 'reserved0': None, 'error': f'Invalid markers: start={packet_bytes[0]:02X} {packet_bytes[1]:02X}, end={packet_bytes[13]:02X}' } # Decode fields (all Little Endian signed 16-bit except PWM) motor_current = _le_s16(packet_bytes[2], packet_bytes[3]) encoder_value = _le_s16(packet_bytes[4], packet_bytes[5]) relative_encoder_value = _le_s16(packet_bytes[6], packet_bytes[7]) v24_pec_diff = _le_s16(packet_bytes[8], packet_bytes[9]) reserved0 = _le_s16(packet_bytes[10], packet_bytes[11]) pwm = int(packet_bytes[12]) # Unsigned 8-bit (0-100) # Optional sanity check (adjust bounds as needed) if not (-40000 <= motor_current <= 40000 and 0 <= pwm <= 100): return { 'motor_current': motor_current, 'encoder_value': encoder_value, 'relative_encoder_value': relative_encoder_value, 'v24_pec_diff': v24_pec_diff, 'pwm': pwm, 'reserved0': reserved0, 'error': f'Out of range: motor_current={motor_current}, pwm={pwm}' } return { 'motor_current': motor_current, 'encoder_value': encoder_value, 'relative_encoder_value': relative_encoder_value, 'v24_pec_diff': v24_pec_diff, 'pwm': pwm, 'reserved0': reserved0, 'error': None } def _wrap14_delta(current: int, zero: int) -> int: """ Calculate signed wrap-around delta in 14-bit space. Maps (current - zero) to range [-8192..+8191] handling wrap-around. Args: current: Current 14-bit value (0-16383) zero: Zero reference 14-bit value (0-16383) Returns: Signed delta in range [-8192, +8191] Example: _wrap14_delta(100, 16300) = 184 # Wrapped forward _wrap14_delta(16300, 100) = -184 # Wrapped backward """ return ((current - zero + 8192) % 16384) - 8192 def decode_i2c_sample(i2c_bytes: Optional[bytes], zero_ref: int = 0) -> Dict[str, Any]: """ Decode I2C sample bytes into angle telemetry with zero reference. I2C format: 2 bytes (14-bit angle from AS5600/AS5048A sensor) Byte 0: High byte (bits 13-6 of angle) Byte 1: Low byte (bits 5-0 of angle) Args: i2c_bytes: Raw I2C bytes (2 bytes) zero_ref: Zero reference value (0-16383), default 0 Returns: Dictionary with decoded I2C angle fields Example: i2c_bytes = b'\\xEC\\x36' # Sample from sensor decoded = decode_i2c_sample(i2c_bytes, zero_ref=11318) # Returns: { # 'i2c_raw14': 11318, # 'i2c_zero_raw14': 11318, # 'i2c_delta_raw14': 0, # 'i2c_angle_deg': 0.0, # 'i2c_zero_angle_deg': 248.69 # } """ # Validate input if i2c_bytes is None or len(i2c_bytes) != 2: return { 'i2c_raw14': None, 'i2c_zero_raw14': zero_ref if zero_ref != 0 else None, 'i2c_delta_raw14': None, 'i2c_angle_deg': None, 'i2c_zero_angle_deg': (zero_ref * 360.0 / 16384.0) if zero_ref != 0 else None, 'error': f'Invalid I2C length: {len(i2c_bytes) if i2c_bytes else 0}, expected 2' } # Decode 14-bit value # Format: MSB contains bits [13:6], LSB contains bits [5:0] msb, lsb = i2c_bytes[0], i2c_bytes[1] raw14 = ((msb << 6) | (lsb & 0x3F)) & 0x3FFF # Calculate delta from zero with wrap-around delta14 = _wrap14_delta(raw14, zero_ref) if zero_ref != 0 else None # Convert to degrees # NOTE: Negative sign to match coordinate system (clockwise positive) angle_deg = -(delta14 * 360.0 / 16384.0) if delta14 is not None else None zero_angle_deg = (zero_ref * 360.0 / 16384.0) if zero_ref != 0 else None return { 'i2c_raw14': raw14, 'i2c_zero_raw14': zero_ref if zero_ref != 0 else None, 'i2c_delta_raw14': delta14, 'i2c_angle_deg': angle_deg, 'i2c_zero_angle_deg': zero_angle_deg, 'error': None } # ============================================================================= # Test Code # ============================================================================= if __name__ == "__main__": print("Decoder Module - Full Implementation") print("=" * 70) print() # Test UART packet decoding (from PDF example) # EF FE | 04 00 | ED FF | 00 00 | 20 08 | 00 00 | 00 | EE # Expected: MotorCurrent=4, EncoderValue=-19, Relative=0, V24=2080, PWM=0 test_uart = bytes([ 0xEF, 0xFE, # Start 0x04, 0x00, # MotorCurrent = 4 (Little Endian: LSB first) 0xED, 0xFF, # EncoderValue = -19 (Little Endian: LSB first) 0x00, 0x00, # RelativeEncoderValue = 0 0x20, 0x08, # V24PecDiff = 2080 (Little Endian: LSB first) 0x00, 0x00, # Reserved0 = 0 0x00, # PWM = 0 0xEE # End ]) decoded_uart = decode_uart_packet(test_uart) print("UART Packet Test:") print(f" Hex: {test_uart.hex(' ').upper()}") print(f" Decoded:") for key, val in decoded_uart.items(): print(f" {key:25s} = {val}") print() # Test I2C angle decoding (from your log) # EC 36 = 11318 raw14 = 248.69° test_i2c = bytes([0xEC, 0x36]) decoded_i2c = decode_i2c_sample(test_i2c, zero_ref=0) print("I2C Angle Test (no zero):") print(f" Hex: {test_i2c.hex(' ').upper()}") print(f" Decoded:") for key, val in decoded_i2c.items(): if val is not None: print(f" {key:25s} = {val}") print() # Test I2C with zero reference decoded_i2c_zero = decode_i2c_sample(test_i2c, zero_ref=11318) print("I2C Angle Test (with zero=11318):") print(f" Hex: {test_i2c.hex(' ').upper()}") print(f" Decoded:") for key, val in decoded_i2c_zero.items(): if val is not None: print(f" {key:25s} = {val}") print() print("✓ Decoder ready (full implementation)") print("✓ UART: Little Endian (LSB first), signed values") print("✓ I2C: 14-bit angle with zero reference and wrap-around")