# Internals

How the firmware works inside. You don't need this unless you're modifying the firmware itself.

## Threads

Each thread runs at a fixed rate on a specific CPU core. Higher priority threads can interrupt lower priority ones.

| Thread | Frequency | Priority | CPU | Description | Status |
|--------|-----------|----------|-----|-------------|--------|
| `estop` | 100 Hz | 99 | 7 | Safety watchdog, heartbeat check | Enabled |
| `can_rx` | 1000 Hz | 95 | 1 | CAN RX drain for all buses | Enabled |
| `can0` | 100 Hz | 94 | 6 | CAN bus 0 motor I/O (L_Leg) | Enabled |
| `can1` | 100 Hz | 94 | 5 | CAN bus 1 motor I/O (R_Leg) | Enabled |
| `imu` | 500 Hz | 92 | 4 | BNO085 read (built-in fusion) | Enabled |
| `policy` | 50 Hz | 90 | 3 | ONNX neural network inference | Enabled |
| `can2` | 100 Hz | 86 | 5 | CAN bus 2 motor I/O (L_Arm) | Disabled |
| `can3` | 100 Hz | 85 | 5 | CAN bus 3 motor I/O (R_Arm) | Disabled |
| `can4` | 100 Hz | 84 | 6 | CAN bus 4 motor I/O (Torso) | Disabled |
| `can5` | 100 Hz | 83 | 6 | CAN bus 5 motor I/O | Disabled |
| `aggregator` | 100 Hz | 80 | 2 | Merge CAN data into state snapshot | Enabled |
| `netrx` | 100 Hz | 50 | 2 | UDP command reception (port 8888) | Enabled |
| `nettx` | 100 Hz | 45 | 2 | Protobuf telemetry streaming | Enabled |
| `power` | 10 Hz | 30 | 3 | Battery management | Disabled |

Priority is Linux `SCHED_FIFO` (1-99). Higher number = more important = can interrupt lower numbers.

### CPU Layout

```
CPU 1: can_rx                          (High-frequency CAN receive)
CPU 2: aggregator, netrx, nettx        (Data aggregation + Network)
CPU 3: policy, power                   (Control)
CPU 4: imu                             (IMU sensor)
CPU 5: can1, can2, can3                (CAN buses 1-3)
CPU 6: can0, can4, can5                (CAN buses 0, 4-5)
CPU 7: estop                           (Safety-critical)
```

Why this layout:
- **CPU 7**: E-stop gets its own core so nothing can starve safety
- **CPU 1**: CAN RX at 1kHz needs a dedicated core for consistent timing
- **CPU 4**: IMU at 500Hz has its own core for low-latency sensor reads
- **CPU 5-6**: CAN motor threads spread across two cores
- **CPU 2-3**: Network, aggregator, and policy share cores (lower frequency)

### What Each Thread Does

**estop** - Watches for problems. If it doesn't see commands for 100ms, or a motor overheats, or the IMU dies, it kills power to the motors.

**can_rx** - Runs at 1kHz to drain incoming CAN frames from all buses. This dedicated high-frequency thread ensures motor feedback is captured promptly, preventing packet loss in userspace. Without this, the 100Hz CAN threads might miss feedback frames that arrive between their polling intervals.

**imu** - Reads the BNO085 at 500Hz, writes orientation to `g_app.imu`.

**can0-5** - Each one talks to motors on one CAN bus. Sends commands, reads back position/velocity/current/temperature.

**aggregator** - Combines motor data from all CAN threads into one snapshot.

**policy** - Runs the ONNX neural network. Reads sensor state, outputs motor commands.

**netrx** - Listens on UDP 8888 for your commands. Parses the protobuf, puts it in a queue for the policy.

**nettx** - Sends telemetry back to you at 100Hz.

### Real-Time Stuff

At startup, the firmware locks all memory (`mlockall`) so there are no page faults during operation. Each thread is pinned to a specific CPU core so it doesn't get migrated around.

### Thread Lifecycle

```{mermaid}
flowchart TD
    init["SCHEDULER_init()"] --> create

    subgraph create["SCHEDULER_create_thread()"]
        c1["Allocate thread slot"]
        c2["Configure timer with frequency"]
        c3["Set priority"]
    end

    create --> start

    subgraph start["SCHEDULER_start_thread()"]
        s1["Start periodic timer"]
        s2["Set CPU affinity"]
        s3["Thread enters RUNNING state"]
    end

    start --> running
    running["Thread Running<br/>(periodic loop)"] --> callback["callback()"]
    callback --> running

    running --> stop["SCHEDULER_stop_thread()"]
    stop --> stopped["Thread Stopped"]

    style create fill:#e3f2fd
    style start fill:#e8f5e9
    style running fill:#fff3e0
```

---

## Shared State (g_app)

All threads share data through one struct called `g_app`. It has everything: joint positions, IMU data, commands, error flags.

```c
typedef struct {
    /* System Flags */
    volatile bool running;
    volatile bool estop_active;
    ControlMode_E current_mode;

    /* Joint State */
    struct { float pos[30], vel[30], current[30], temp[30]; } joints;

    /* IMU State (atomic) */
    struct { _Atomic uint32_t seq; float quat[4], ang_vel[3], gravity[3]; } imu;

    /* Gait & Control */
    float gait_phase;
    GaitMode_E gait_mode;

    /* Inter-Thread Communication */
    struct { ... } comms;

    /* Thread Management */
    struct { ... } threads;

} AppContext_S;

extern AppContext_S g_app;  /* Global instance */
```

### How Threads Share Data

**Seqlocks** - When one thread writes a snapshot and multiple threads read it. The reader retries if it catches the writer mid-update.

| Structure | Writer | Reader(s) | Data |
|-----------|--------|-----------|------|
| `cmd_snapshot` | policy_thread | CAN threads | `IoCmd_S` motor commands |
| `state_snapshot` | aggregator_thread | policy, nettx | `IoState_S` sensor state |

**Queues** - When one thread produces data and another consumes it. Lock-free, no blocking.

| Queue | Producer | Consumer | Data |
|-------|----------|----------|------|
| `netcmd_to_policy` | netrx_thread | policy_thread | `NetCmd_S` |
| `can_to_aggregator[6]` | CAN threads | aggregator | `IoState_S` |

### Performance

| Operation | Time |
|-----------|------|
| Seqlock read | ~50 ns |
| Queue push/pop | ~100 ns |
| Atomic load | ~10 ns |

No locks, no blocking, no surprises.

---

## Data Flow

```{mermaid}
flowchart TD
    subgraph inputs["Input Threads"]
        imu["IMU thread<br/>500 Hz<br/>Reads sensor"]
        netrx["netrx_thread<br/>100 Hz<br/>Receives UDP"]
        can_in["CAN threads<br/>100 Hz<br/>Talks to motors"]
    end

    imu --> imu_data["g_app.imu<br/>(atomic seqlock)"]
    netrx --> netcmd_queue["netcmd_to_policy<br/>(SPSC queue)"]
    can_in --> can_queue["can_to_aggregator<br/>(SPSC queues)"]

    imu_data --> policy
    netcmd_queue --> policy["policy_thread<br/>50 Hz<br/>Runs ONNX model"]

    policy --> cmd_snap["cmd_snapshot<br/>(seqlock)<br/>IoCmd_S: motor targets"]

    cmd_snap --> can_out["CAN threads<br/>(read cmds, write state)"]
    can_queue --> aggregator

    can_out --> aggregator["aggregator_thread<br/>Combines state"]

    aggregator --> state_snap["state_snapshot<br/>(seqlock)<br/>IoState_S: joints, IMU, errors"]

    state_snap --> nettx["nettx_thread<br/>100 Hz<br/>Sends telemetry"]

    style inputs fill:#e3f2fd
    style policy fill:#fff3e0
    style aggregator fill:#e8f5e9
```

---

## Internal Structs

### IoState_S

Sensor data going to the policy. Written by aggregator, read by policy.

| Field | Type | Unit | Description |
|-------|------|------|-------------|
| `joint_pos[30]` | float | rad | Joint positions |
| `joint_vel[30]` | float | rad/s | Joint velocities |
| `joint_current[30]` | float | A | Motor phase currents |
| `joint_temp[30]` | float | °C | Motor temperatures |
| `base_quat[4]` | float | - | Orientation quaternion [w,x,y,z] |
| `base_omega[3]` | float | rad/s | Angular velocity in body frame |
| `base_gravity[3]` | float | - | Gravity vector (normalized) |
| `gait_phase` | float | - | Locomotion phase [0-1] |
| `battery_voltage` | float | V | Pack voltage |
| `motors_enabled` | bool | - | Motor power state |
| `emergency_stop` | bool | - | E-stop active |
| `timestamp_us` | uint64 | μs | Capture timestamp |

### IoCmd_S

Motor commands from policy to CAN threads.

| Field | Type | Unit | Description |
|-------|------|------|-------------|
| `target_pos[30]` | float | rad | Target positions |
| `target_vel[30]` | float | rad/s | Target velocities (feedforward) |
| `target_torque[30]` | float | Nm | Target torques (feedforward) |
| `kp[30]` | float | - | Position gains |
| `kd[30]` | float | - | Velocity gains |
| `enable_motors` | bool | - | Enable motor power |
| `emergency_stop` | bool | - | Trigger E-stop |

### NetCmd_S

Commands from network to policy (parsed from your RobotCommand).

| Field | Type | Unit | Description |
|-------|------|------|-------------|
| `body_vel_cmd[3]` | float | m/s, rad/s | Velocity command [vx, vy, vyaw] |
| `control_mode` | ControlMode_E | enum | Requested control mode |
| `enable` | bool | - | Enable motion |
| `emergency_stop` | bool | - | Remote E-stop |
| `timestamp_us` | uint64 | μs | Command timestamp |

---

## Important Constants

| Name | Value | What it means |
|------|-------|---------------|
| `DEADMAN_TIMEOUT_US` | 100000 | 100ms without commands = E-stop |
| `NETWORK_RX_PORT` | 8888 | Where to send commands |
| `NUM_CAN_BUSES` | 6 | Number of CAN interfaces |

## Source Files

| File | What it does |
|------|--------------|
| `source/main.c` | Thread setup in `init_thread_config()` |
| `source/app_context/app_context.h` | Global state struct |
| `source/lib/io_types.h` | IoState_S, IoCmd_S, NetCmd_S |