You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
251 lines
8.4 KiB
251 lines
8.4 KiB
#!/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")
|