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

        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.

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

        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