12 KiB

Raw Blame History

HansonServo Protocol Migration Plan

Overview

The firmware has been updated from a simple XOR-checksum protocol to a more robust CRC16 tagged packet protocol. This document describes the changes needed in the desktop software.

Protocol Changes Summary

Aspect	Old Protocol	New Protocol
Sync bytes	`0xAA 0x55`	`0xA5 0x5A`
Checksum	XOR (1 byte)	CRC16-CCITT (2 bytes)
Command ID	1 byte numeric	4 byte ASCII tag
Sequence	None	2 byte counter
Baud rate	1,000,000	1,000,000 (unchanged)

New Packet Format

┌──────┬──────┬─────────┬─────────┬─────────┬───────────┬─────────┐
│ SYNC │ SYNC │   TAG   │ LENGTH  │   SEQ   │  PAYLOAD  │  CRC16  │
│ 0xA5 │ 0x5A │ 4 bytes │ 2 bytes │ 2 bytes │ N bytes   │ 2 bytes │
└──────┴──────┴─────────┴─────────┴─────────┴───────────┴─────────┘

Field Details

Field	Size	Description
SYNC0	1	Always `0xA5`
SYNC1	1	Always `0x5A`
TAG	4	ASCII identifier (e.g., "IDNT", "MSET")
LENGTH	2	Payload length, little-endian
SEQ	2	Sequence number, little-endian
PAYLOAD	N	Command-specific data
CRC16	2	CRC16-CCITT over TAG+LENGTH+SEQ+PAYLOAD, little-endian

CRC16-CCITT Implementation

def crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
    crc = init
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            if crc & 0x8000:
                crc = (crc << 1) ^ 0x1021
            else:
                crc <<= 1
            crc &= 0xFFFF
    return crc

// C# implementation
ushort Crc16Ccitt(byte[] data)
{
    ushort crc = 0xFFFF;
    foreach (byte b in data)
    {
        crc ^= (ushort)(b << 8);
        for (int i = 0; i < 8; i++)
        {
            if ((crc & 0x8000) != 0)
                crc = (ushort)((crc << 1) ^ 0x1021);
            else
                crc <<= 1;
        }
    }
    return crc;
}

Command Tag Mapping

Old → New Command Mapping

Old Command	Old ID	New Tag	Notes
CMD_ID_REQUEST	0x01	`IDNT`	Identity request
CMD_FILE_LIST	0x02	`FLST`	List files
CMD_LOAD_FILE	0x03	`FLOD`	Load file content
CMD_DELETE_FILE	0x04	`FDEL`	Delete file
CMD_SAVE_FILE	0x05	`FSAV`	Save animation
CMD_MESSAGE	0x06	`MSGE`	Log/debug message
CMD_SET_POSITION	0x07	`MSET`	Set motor positions
CMD_PLAY_FILE	0x08	`FPLY`	Play animation
CMD_STOP_FILE	0x09	`FSTP`	Stop animation
CMD_SCAN_CHANNEL	0x09	`MSCN`	Scan for motors
CMD_WRITE_DATA	0x10	`MWRT`	Write motor register
CMD_WRITE_CONFIG_UPDATE	0x12	`CONF`	Update config
CMD_START_POSITION_STREAM	0x14	`MSTM`	Motor stream control
POSITION_STREAM	0x15	`MPOS`	Motor position data

New Tags (not in old protocol)

Tag	Description
`IMU0`	IMU data (accel x,y,z)
`RDAR`	Radar target data
`BHVR`	Behavior control (enable/disable)
`BLST`	Behavior list (list all behaviors and states)
`STAT`	System state/heartbeat
`ACK!`	Acknowledge (success)
`NACK`	Negative acknowledge (failure)
`BOOT`	Enter bootloader

Detailed Command Reference

Identity & Configuration

`IDNT` - Get Robot Identity

Request: Empty payload
Response: Robot config serialized bytes (same format as before)

`CONF` - Update Configuration

Request: Same payload format as old CMD_WRITE_CONFIG_UPDATE
Response: ACK! on success, NACK with reason on failure

File Operations

`FLST` - List Files

Request: Empty payload
Response: Newline-separated filename list (UTF-8 string)

`FLOD` - Load File

Request: Filename as raw bytes (no length prefix)
Response: File contents as raw bytes, or NACK if not found

`FSAV` - Save Animation

Request: Same format as old CMD_SAVE_FILE:

[filename_len: 2 bytes LE]
[filename: N bytes]
[animation_header: 18 bytes]
[curve_segments: variable]
[node_graph: variable]

Response: ACK! on success, NACK on failure

`FDEL` - Delete File

Request:

[filename_len: 2 bytes LE]
[filename: N bytes]

Response: ACK! on success

`FPLY` - Play Animation

Request:

[filename_len: 2 bytes LE]
[filename: N bytes]
[play_mode: 1 byte]   // 0=idle, 1=once, 2=loop, 3=repeat
[repeat_count: 1 byte]
[start_frame: 2 bytes LE]  // Frame number to start playback from (0-based)

Response: ACK! on success, NACK if file not found

Notes:

start_frame allows resuming playback from a specific frame
FRAME packets report actual frame numbers (i.e., if start_frame=163, FRAME packets will show 163, 164, 165...)

`FSTP` - Stop Animation

Request: Empty payload (0 bytes) Response: ACK! on success

Notes:

Immediately stops the currently playing animation regardless of play mode
Motors remain in their current positions (torque not disabled)
No FRAME completion packet is sent

Motor Control

`MSET` - Set Motor Positions

Request: Array of motor commands:

[motor_id: 1 byte][position: 2 bytes LE] × N motors

Response: ACK!

`MPOS` - Motor Position Stream (device → host)

Payload: Same format as MSET request

[motor_id: 1 byte][position: 2 bytes LE] × N motors

Sent automatically when streaming is enabled

`MSCN` - Scan for Motors

Request:

[channel: 1 byte]  // 0 or 1

Response: Multiple packets, one per found motor:

[channel: 1][motor_id: 1][model: 2][min_angle: 2][max_angle: 2]
[position: 2][cw_dead: 1][ccw_dead: 1][offset: 2][mode: 1]
[torque_enable: 1][acceleration: 1][goal_pos: 2][goal_time: 2]
[goal_speed: 2][lock: 1][speed: 2][load: 2][temp: 1][moving: 1]
[current: 2][voltage: 1]

Final packet has motor_id = 255 to signal scan complete.

`MWRT` - Write Motor Register

Request:

[channel: 1 byte]
[motor_id: 1 byte]
[register: 1 byte]
[data_len: 1 byte]   // 1 or 2
[data: 1-2 bytes]

Response: Register read-back value (1 or 2 bytes)

Special case: Register 5 with 1 byte changes the motor ID.

`MSTM` - Motor Stream Control

Request:

[enable: 1 byte]  // 0=disable, 1=enable

Response: ACK!

When enabled, device streams MPOS packets every 50ms.

Sensors

`IMU0` - IMU Data (device → host)

Payload:

[accelX: 2 bytes LE, signed]   // g-forces × 100
[accelY: 2 bytes LE, signed]   // g-forces × 100
[accelZ: 2 bytes LE, signed]   // g-forces × 100
[pitch: 2 bytes LE, signed]    // degrees × 100
[roll: 2 bytes LE, signed]     // degrees × 100

Sent automatically when IMU streaming is enabled

Coordinate System:

X: left/right axis (affects roll)
Y: front/back axis (affects pitch)
Z: up/down axis

Notes:

Acceleration values scaled by 100 (not 1000 as before)
Euler angles calculated from accelerometer data
Heading/yaw not available (accelerometer only)

`RDAR` - Radar Data (device → host)

Payload:

[target_count: 1 byte]
For each of 3 targets:
  [valid: 1 byte]      // 0 or 1
  [x: 2 bytes LE]      // cm × 10, signed
  [y: 2 bytes LE]      // cm × 10, signed
  [speed: 2 bytes LE]  // cm/s × 10, signed

Sent automatically when radar streaming is enabled

Behaviors

`BHVR` - Behavior Control (host → device)

Request:

[behaviorID: 1 byte]   // Behavior ID (1 = Focus)
[enable: 1 byte]       // 0 = disable, non-zero = enable

Response: ACK! on success, NACK on failure

Behavior IDs:

1 = Focus (radar tracking with eye motors 14 & 15)
2 = Idle (perlin noise motion for all motors, ±500 range from center)

`BLST` - Behavior List (host → device)

Request: Empty payload

Response:

[count: 1 byte]                    // Number of registered behaviors
[behaviorID1: 1 byte][enabled1: 1 byte]  // First behavior
[behaviorID2: 1 byte][enabled2: 1 byte]  // Second behavior
...

enabled: 1 = enabled, 0 = disabled

System

`STAT` - System State/Heartbeat (device → host)

Payload:

[uptime: 4 bytes LE]  // seconds since boot
[flags: 2 bytes LE]   // bit flags

Flags:

Bit 0: IMU ready
Bit 1: Animation playing
Bit 2: Motor streaming active
Bit 3: IMU streaming active
Bit 4: Radar streaming active

Sent automatically every 1 second

`MSGE` - Log Message (device → host)

Payload: UTF-8 string (no null terminator)

`ACK!` - Acknowledge

Payload:

[original_tag: 4 bytes]  // The tag being acknowledged

`NACK` - Negative Acknowledge

Payload:

[original_tag: 4 bytes]
[reason: N bytes, optional UTF-8 string]

`BOOT` - Enter Bootloader

Request: Empty payload
Response: MSGE "Entering bootloader...", then device resets

Implementation Checklist

1. Protocol Layer Changes

Update sync byte detection from 0xAA 0x55 to 0xA5 0x5A
Implement CRC16-CCITT calculation
Update packet parsing to handle new format:
- Read 4-byte tag instead of 1-byte command
- Read 2-byte sequence number (can ignore for now, or use for debugging)
- Verify CRC16 instead of XOR checksum
Update packet building:
- Use 4-byte tags
- Add sequence counter (increment per packet)
- Calculate and append CRC16

2. Command Handler Updates

Replace command ID constants with tag strings
Update request builders for each command
Update response parsers for each command
Add handlers for new response types:
- ACK! - generic success
- NACK - generic failure with reason
- STAT - heartbeat (can use to detect connection)
- IMU0 - IMU data (if needed)
- RDAR - radar data (if needed)

3. UI/UX Improvements (Optional)

Show connection status based on STAT heartbeat
Display IMU orientation if sensor is available
Show radar targets if sensor is available

Example: Sending a Motor Position Command

Old Code (pseudocode)

def send_motor_positions(motors):
    payload = b''
    for motor_id, position in motors:
        payload += bytes([motor_id])
        payload += struct.pack('<H', position)
    
    length = len(payload)
    checksum = CMD_SET_POSITION ^ (length >> 8) ^ (length & 0xFF)
    for b in payload:
        checksum ^= b
    
    packet = bytes([0xAA, 0x55, CMD_SET_POSITION])
    packet += struct.pack('>H', length)  # big-endian length
    packet += payload
    packet += bytes([checksum])
    
    serial.write(packet)

New Code (pseudocode)

def send_motor_positions(motors):
    tag = b'MSET'
    payload = b''
    for motor_id, position in motors:
        payload += bytes([motor_id])
        payload += struct.pack('<H', position)
    
    length = len(payload)
    seq = get_next_sequence()
    
    # Build data for CRC: tag + length + seq + payload
    crc_data = tag
    crc_data += struct.pack('<H', length)
    crc_data += struct.pack('<H', seq)
    crc_data += payload
    crc = crc16_ccitt(crc_data)
    
    packet = bytes([0xA5, 0x5A])
    packet += tag
    packet += struct.pack('<H', length)
    packet += struct.pack('<H', seq)
    packet += payload
    packet += struct.pack('<H', crc)
    
    serial.write(packet)

Testing Strategy

Connection Test: Send empty IDNT request, expect IDNT response with config
File List Test: Send FLST, expect filename list
Motor Test: Send MSET with known positions, expect ACK!
Heartbeat: After connecting, should receive STAT packets every second

Backwards Compatibility

The new protocol uses different sync bytes (0xA5 0x5A vs 0xAA 0x55), so there's no ambiguity. If you need to support both old and new firmware:

Detect firmware version by sync bytes in received packets
Switch protocol handler based on detected version
Or: just update all firmware to new version

Questions?

The firmware source is at: C:\Users\jake\Documents\hansonProjects\HansonServo\

Key files:

protocol.h/cpp - Packet format and CRC implementation
commands.h/cpp - Command handlers
sensors.h/cpp - IMU and radar drivers

12 KiB Raw Blame History Unescape Escape

HansonServo Protocol Migration Plan

Overview

Protocol Changes Summary

New Packet Format

Field Details

CRC16-CCITT Implementation

Command Tag Mapping

Old → New Command Mapping

New Tags (not in old protocol)

Detailed Command Reference

Identity & Configuration

IDNT - Get Robot Identity

CONF - Update Configuration

File Operations

FLST - List Files

FLOD - Load File

FSAV - Save Animation

FDEL - Delete File

FPLY - Play Animation

FSTP - Stop Animation

Motor Control

MSET - Set Motor Positions

MPOS - Motor Position Stream (device → host)

MSCN - Scan for Motors

MWRT - Write Motor Register

MSTM - Motor Stream Control

Sensors

IMU0 - IMU Data (device → host)

RDAR - Radar Data (device → host)

Behaviors

BHVR - Behavior Control (host → device)

BLST - Behavior List (host → device)

System

STAT - System State/Heartbeat (device → host)

MSGE - Log Message (device → host)

ACK! - Acknowledge

NACK - Negative Acknowledge

BOOT - Enter Bootloader

Implementation Checklist

1. Protocol Layer Changes

2. Command Handler Updates

3. UI/UX Improvements (Optional)

Example: Sending a Motor Position Command

Old Code (pseudocode)

New Code (pseudocode)

Testing Strategy

Backwards Compatibility

Questions?

12 KiB

Raw Blame History

`IDNT` - Get Robot Identity

`CONF` - Update Configuration

`FLST` - List Files

`FLOD` - Load File

`FSAV` - Save Animation

`FDEL` - Delete File

`FPLY` - Play Animation

`FSTP` - Stop Animation

`MSET` - Set Motor Positions

`MPOS` - Motor Position Stream (device → host)

`MSCN` - Scan for Motors

`MWRT` - Write Motor Register

`MSTM` - Motor Stream Control

`IMU0` - IMU Data (device → host)

`RDAR` - Radar Data (device → host)

`BHVR` - Behavior Control (host → device)

`BLST` - Behavior List (host → device)

`STAT` - System State/Heartbeat (device → host)

`MSGE` - Log Message (device → host)

`ACK!` - Acknowledge

`NACK` - Negative Acknowledge

`BOOT` - Enter Bootloader