realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
HansonServo/PROTOCOL_MIGRATION.md

20 KiB
Raw Permalink 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)
VLST List all visemes with names and motor positions
VADD Add a new viseme with name
VDEL Delete a viseme by ID
VSET Set motor positions for a viseme
VSME Trigger viseme (fire-and-forget)
STAT System state/heartbeat
FACE Face detection data (Radxa → host/robot, via WiFi)
ALIV Remote component alive/heartbeat (component ID + status)
SSET Settings set/dump (read/write individual settings by ID)
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

FACE - Face Detection Data (Radxa → host/robot via WiFi)

Payload:

[face_count: 1 byte]   // 0–N faces
For each face:
  [x: 2 bytes LE, signed]   // center-relative pixels (0,0 = image center)
  [y: 2 bytes LE, signed]
  [w: 2 bytes LE]           // bounding box width
  [h: 2 bytes LE]           // bounding box height
  [conf: 1 byte]            // confidence × 255 (0–255)

Notes:

  • Sent from Radxa over WebSocket when faces are detected
  • Radxa runs face_server.py (WebSocket server); robot/host connects to receive packets
  • x, y are offset from image center: positive = right/down, negative = left/up
  • Same packet format as serial (0xA5 0x5A + TAG + LENGTH + SEQ + PAYLOAD + CRC16)

ALIV - Remote Component Alive (Radxa/other → host/robot via WiFi)

Payload:

[component_id: 1 byte]   // Assigned ID for each remote component
[alive: 1 byte]         // 0 = not alive / stream disconnected, 1 = alive
// Future: extended payload for other devices

Component IDs:

  • 3 = Face detection (Radxa face_server.py – alive = MJPEG stream connected)

Notes:

  • Sent every 2 seconds by each remote component
  • Use alive to detect when a component has lost its connection and is retrying

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 (face tracking with eye motors 14 & 15)
  • 2 = Idle (perlin noise motion for all motors, ±500 range from center)
  • 3 = Viseme (mouth motor control for speech)

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

Visemes

VLST - List Visemes (host → device)

Request: Empty payload

Response:

[count: 1 byte]                    // Number of visemes
For each viseme:
  [viseme_id: 1 byte]
  [label: 3 bytes]                  // 3-character label (e.g., "AA ", "SIL")
  [motor_count: 1 byte]
  For each motor:
    [motor_id: 1 byte]
    [position_low: 1 byte]
    [position_high: 1 byte]

Position = position_low | (position_high << 8), range 0-4095

VADD - Add Viseme (host → device)

Request:

[label: 3 bytes]                    // 3-character label (e.g., "AA ", "SIL")

Response: ACK! with payload [new_viseme_id: 1 byte] on success, NACK on failure

VDEL - Delete Viseme (host → device)

Request:

[viseme_id: 1 byte]

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

VSET - Set Viseme Motor Positions (host → device)

Request:

[viseme_id: 1 byte]
[motor_count: 1 byte]
For each motor:
  [motor_id: 1 byte]
  [position_low: 1 byte]
  [position_high: 1 byte]

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

VSME - Trigger Viseme (host → device)

Request:

[viseme_id: 1 byte]

Response: None (fire-and-forget)

Notes:

  • Triggers the viseme behavior which controls the motors defined for that viseme
  • The behavior activates and holds the motor positions
  • After 3 seconds without a new viseme trigger, the behavior deactivates and releases motor control
  • Continuously sending viseme IDs will keep the mouth animated
  • Viseme behavior has higher priority than Idle behavior, lower than Focus

Default Viseme IDs (loaded at startup):

  • 0 = Neutral/rest (sil) - motors 40, 43, 44
  • 1 = AA (as in "father")
  • 2 = AE (as in "cat")
  • 3 = AH (as in "but")
  • 4 = AO (as in "bought")
  • 5 = EH (as in "bed")
  • 6 = IH (as in "bit")
  • 7 = IY (as in "beat")
  • 8 = OW (as in "boat")
  • 9 = UH (as in "book")
  • 10 = UW (as in "boot")

Settings

SSET - Settings Set/Dump (host ↔ device)

Write a single setting: Request:

[setting_id: 2 bytes LE]
[data: N bytes]          // 2 bytes for numeric, variable for strings

Response: ACK! on success, NACK if unknown setting ID

Notes: Writing a WiFi/WebSocket setting triggers an automatic reconnect.

Dump all settings: Request: Empty payload (0 bytes) Response: SSET packet:

[count: 2 bytes LE]
For each setting:
  [setting_id: 2 bytes LE]
  [data_len: 2 bytes LE]
  [data: data_len bytes]

Value encoding:

  • uint8/uint16/bool: data_len=2, stored as uint16 LE
  • float (0.0–65.535): data_len=2, stored as value × 1000 (e.g., 0.15 → 150)
  • int16 (signed): data_len=2, stored as uint16 reinterpret (e.g., -140 → 0xFF74)
  • string: data_len=N, raw UTF-8 bytes (no null terminator)

Focus Behavior Setting IDs (0x0500–0x0512):

ID Name Type Default Description
0x0500 FOCUS_EYE_MOTOR_1 uint8 14 Eye motor 1 ID
0x0501 FOCUS_EYE_MOTOR_2 uint8 15 Eye motor 2 ID
0x0502 FOCUS_NECK_MOTOR uint8 27 Neck yaw motor ID
0x0503 FOCUS_EYE_CENTER uint16 2200 Eye servo center position
0x0504 FOCUS_EYE_MIN uint16 1700 Eye servo min position
0x0505 FOCUS_EYE_MAX uint16 2500 Eye servo max position
0x0506 FOCUS_NECK_CENTER uint16 2000 Neck servo center position
0x0507 FOCUS_NECK_MIN uint16 1000 Neck servo min position
0x0508 FOCUS_NECK_MAX uint16 3000 Neck servo max position
0x0509 FOCUS_FACE_X_MIN int16 -140 Face detection X min (pixels)
0x050A FOCUS_FACE_X_MAX int16 140 Face detection X max (pixels)
0x050B FOCUS_EYE_SPEED float×1000 150 Eye dart speed (0–1)
0x050C FOCUS_NECK_SPEED float×1000 20 Neck follow speed (0–1)
0x050D FOCUS_EYE_RETURN_SPEED float×1000 50 Eye return-to-center speed
0x050E FOCUS_NECK_DELAY_MS uint16 500 Neck start delay (ms)
0x050F FOCUS_NECK_CONTRIBUTION float×1000 700 Neck offset coverage (0–1)
0x0510 FOCUS_NECK_INVERT bool 1 Invert neck motor direction
0x0511 FOCUS_EYE_CENTERING float×1000 30 Eye centering speed (no face)
0x0512 FOCUS_NECK_CENTERING float×1000 20 Neck centering speed (no face)

WiFi / WebSocket Setting IDs (0x0600–0x0604):

ID Name Type Default Description
0x0600 WIFI_SSID string "Police Surveillance Van" WiFi network name (max 32 chars)
0x0601 WIFI_PASSWORD string "ourpassword" WiFi password (max 64 chars)
0x0602 WIFI_HOST string "192.168.1.206" WebSocket server IP/hostname (max 63 chars)
0x0603 WIFI_PORT uint16 5001 WebSocket server port
0x0604 WIFI_PATH string "/" WebSocket URL path (max 31 chars)

Notes:

  • All settings are persisted to flash on write
  • The dump includes all registered settings with a length prefix per entry
  • Setting ranges are extensible: 0x0500 = Focus, 0x0600 = WiFi, future = 0x0700+

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
  • Bit 5: Face streaming active
  • Bit 6: Face detection remote alive

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

  1. Connection Test: Send empty IDNT request, expect IDNT response with config
  2. File List Test: Send FLST, expect filename list
  3. Motor Test: Send MSET with known positions, expect ACK!
  4. 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:

  1. Detect firmware version by sync bytes in received packets
  2. Switch protocol handler based on detected version
  3. 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
  • websocket_client.h/cpp - WiFi, WebSocket client + server

WebSocket Server

The firmware runs a WebSocket server on port 81 alongside the outbound client. Any device on the same WiFi network can connect and send protocol-framed packets.

URL: ws://<robot-ip>:81/

All commands from the serial protocol are supported over WebSocket: VSME, MSET, BHVR, SSET, FPLY, FSTP, etc.

Packets must use the same binary framing as serial: 0xA5 0x5A + TAG(4) + LEN(2) + SEQ(2) + PAYLOAD(N) + CRC16(2)

Up to 2 simultaneous WebSocket client connections are supported.