# 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

```python
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
```

```csharp
// 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 motors 14 & 15)

#### `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)
```python
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)
```python
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