New Control Modes, New Radio, Bugfixes, Lots of Work

[nickwitten]

Merge Report: dev/nick/pose-control -> main

Overview

This merge rewrites the robot's motion control architecture from a single-mode
velocity controller into a multi-mode position/velocity/acceleration control
system. Along the way it replaces the motor control interface with closed-loop
current (torque) control, adds support for a new radio module, and reorganizes
shared libraries.

130 files changed, +15,426 / -3,133 lines.

---

1. Motion Control Rewrite

The body controller (robot_controller.rs) was replaced. The old
BodyVelocityController accepted a single body velocity setpoint, ran it
through a hand-tuned constant-gain Kalman filter (CGKF) estimating 3 body
velocity states from 4 encoder + 1 gyro measurements, applied a PID, clamped
velocities and accelerations, and output wheel velocity commands. It ran at
100 Hz. The control law, state estimator, and robot kinematics were all
implemented inline in the firmware.

The new BodyController delegates state estimation and kinematics to the
ateam-controls library (added as a submodule under controls/). The
controls library provides a RobotModel that contains a 6-state Kalman
filter tracking both position and velocity in the global frame, using 8
measurements (3 vision pose + 4 encoder + 1 gyro). The firmware creates a
RobotModel at initialization and calls kf_update and kf_predict each
tick. The control loop now runs at 1 kHz (derived from DEFAULT_CONTROL_DT
= 0.001s in the controls library).

The controller accepts a BasicControl packet from the radio and matches
on its SkillCommand variant to select a control policy:

• GlobalPosition: bang-bang trajectory planning to target pose, with PID
  feedback. Requires active vision (200ms timeout). Position error uses
  calculate_with_derivative (a new PID method that takes an external
  derivative signal -- here, the twist error between trajectory and
  estimate -- instead of numerically differentiating position error).
  Theta wrapping uses remainderf to stay in [-pi, pi].

• GlobalVelocity / LocalVelocity: bang-bang trajectory planning to
  target twist, with PID feedback on the twist error between the tracked
  trajectory state and the estimated twist. Local commands are rotated
  into the global frame via z_rotation_mat(theta).

• GlobalAcceleration / LocalAcceleration: direct passthrough -- applies
  the target acceleration through the dynamics model A*x + B*u to get
  the next state.

All policies output a (body_twist, body_accel) pair. The controller then
applies Coulomb friction compensation: when the commanded acceleration is
above a deadzone threshold, it uses the target twist direction to compute
the friction force (to overcome static friction during motion); when at rest,
it uses the deadzoned estimated twist (for a stable equilibrium). Friction
force is computed by the robot model and subtracted via the inverse inertia
matrix. The final body-level commands are converted to wheel velocities and
wheel torques through the robot model's kinematic transforms (which are
theta-dependent).

Both velocity and position control modes use trajectory planning to shape
the acceleration profile. The trajectory is recomputed when the target
changes or when the actual state strays too far from the tracked trajectory
state (thresholds configurable via TRAJ_RECOMPUTE_ERROR). Trajectory
parameters (max vel/accel) can be sent from the software stack via the
command packet, falling back to defaults when all zeros.

The old parameter files (body_vel_filter_params.rs with its hardcoded 5x3
Kalman gain matrix, body_vel_pid_params.rs, robot_physical_params.rs)
are deleted. A new controller_params.rs defines PID gains for pose and
twist feedback, pose control gains (ff/fb weighting), trajectory recompute
thresholds, and the friction compensation deadzone. The ParameterInterface
implementation now exposes the controls library's KalmanFilterParams,
RobotPhysicalParams, and individual PID/control parameters for live
tuning via the parameter command protocol.

The PID controller itself gains a reset() method and a
calculate_with_derivative() method. The CGKF gains a reset() method
but is otherwise unused by the new controller (it remains in the codebase).

2. Motor Current Control

The motors now run wheel-torque.bin instead of wheel.bin. On the
control board, the WheelMotor driver is replaced by CurrentControlledMotor
(motor.rs), which communicates with the motor controller using a new
CcmCommand/CcmResponse packet protocol (replacing the old
MotionCommand/MotorTelemetry protocol). The motor type
CcmMotionControlType has modes for velocity, current, velocity+current,
and motor-off. The control task converts the body controller's wheel torque
output to wheel currents via robot_model.torques_to_currents(), converts
to milliamps, applies a 1500 mA safety clamp, and sends current setpoints
to each motor.

On the STM32F031 motor controllers, 6-step commutation (6step.c) was
rewritten as 6step_current.c. The old code did open-loop voltage/duty-cycle
commutation. The new code adds a closed-loop current PI controller running in
the commutation ISR. The PI controller operates entirely in fixed-point
arithmetic (new fixedarith.c library) because the F031 has no FPU. The PI
uses S7.10 / S5.13 / S12.0 fixed-point formats with explicit bit-width
tracking through each arithmetic operation. An anti-jitter threshold
suppresses output oscillation near zero error.

Current sensing (current_sensing.c/.h) was overhauled. The old code had
three ADC modes (polling, DMA, timer-DMA); the new code uses two
(PWM-triggered DMA and software). The ADC result struct was simplified to
three channels: motor current, bus voltage, and STSPIN temperature. Proper
op-amp gain calculations are documented inline (gain = 5.5, sense resistor
= 0.05 ohm, settling time = 0.247us). A zero-current calibration routine
samples the ADC at startup and stores the bias. Calibration data can be
persisted to the last page of flash (address 0x08007C00) via the STM32
bootloader interface, which gained read_device_memory and
write_device_memory implementations (the old read_device_memory was
a panic!).

New motor controller binaries: wheel-torque (production current-controlled
image), wheel-torque-test (test harness). New control board binaries:
profile-wheel-curr and profile-wheel-vel capture step response data
over USB for offline tuning, and hwtest-torque for interactive motor
testing.

3. Nora W36x Radio

A driver stack was added for the u-blox Nora W36x WiFi module, which
replaces the Odin W26x module on newer board revisions. The existing Odin
driver files were reorganized under lib-stm32/src/drivers/radio/w26x/.

The Nora driver operates in pure AT command mode (no EDM framing, unlike the
Odin driver). nora_w36x.rs implements the UART transport, command
send/response, and high-level operations: WiFi configuration (SSID, WPA
passphrase), UART baud rate negotiation (115200 startup -> 921600 runtime),
socket creation (TCP/UDP, with multicast support via AT+USOCM), data
transfer (buffered mode and direct binary mode via +UESODB events), and
connection lifecycle management.

at_protocol.rs parses AT responses and unsolicited result codes (URCs)
from raw UART buffers. It handles OK, ERROR, + prefixed events, and
inline socket data events with binary payloads.

radio_robot_nora.rs wraps the Nora driver in the robot's packet-level
protocol: hardware reset sequencing, UART connect with retry, WiFi
association, multicast group join, the HelloRequest/HelloResponse
handshake with the software bridge, and runtime packet send/receive
(encoding RadioPacket structs to/from raw bytes using RadioHeader and
RadioData unions). radio_nora_task.rs is the Embassy async task that
runs the radio lifecycle with error recovery and publishes/subscribes
on the inter-task channels.

The radio stress test infrastructure includes coms_reliability.py (a
Python script that floods the radio with packets and measures loss/latency)
and hwtest-radio-w36 (a firmware binary for isolated radio testing).

4. Control Task Changes

Beyond switching to the new BodyController and CurrentControlledMotor,
the control task has significant structural changes:

• Loop timing is derived from DEFAULT_CONTROL_DT (1ms), with all
  frequencies (basic telemetry at 100 Hz, extended telemetry at 100 Hz,
  trace logging at 10 Hz, packet timeout at 200ms) expressed as tick
  counts relative to the control frequency.

• Telemetry is rate-limited rather than sent every loop iteration. Basic
  telemetry is sent every 10 ticks, extended telemetry every 10 ticks or
  immediately on a vision update. The BasicTelemetry packet now includes
  inline BodyControlTelemetry (body mode, wheel velocities, body
  velocity estimate). The ExtendedTelemetry packet now carries a
  BodyControlExtendedTelemetry struct with full state information:
  IMU readings, vision pose, trajectory state, KF prediction/estimate,
  body commands, and per-skill telemetry.

• Timing instrumentation tracks motor packet processing, command
  processing, control update, and telemetry publishing durations. Warnings
  and error telemetry are sent when the loop exceeds 400us.

• controls_err is a new atomic flag on SharedRobotState. If the
  controls library returns an error (e.g. from kf_update or trajectory
  planning), the flag is set, motors are locked out, and an error telemetry
  packet is sent. reset_controller in the command packet clears the
  controller state and re-enables motion.

• BCM_OFF body control mode now correctly results in zero motor commands,
  and the wheel motion type is set to CCM_MCT_MOTOR_OFF when neither
  velocity nor torque control is enabled.

• Vision pose measurements and the vision update flag are extracted from
  BasicControl and passed into BodyController::control_update(),
  which manages the Kalman filter's vision measurement gating internally.

5. lib-crossarch

A new lib-crossarch crate was created for no_std code that is not
STM32-specific. The queue, filter, math, and power modules were moved from
lib-stm32 into lib-crossarch, and lib-stm32 re-exports them.

The queue (queue.rs) is a fixed-size SPSC ring buffer with async waker
support. The move included several changes to eviction and cancellation
semantics: EnqueueRef now tracks whether an eviction occurred, and
cancel() properly restores the write index and size when an eviction is
rolled back (important for DMA operations where a cancel means the DMA
engine has already overwritten the buffer contents). A comprehensive test
suite (459 lines) was added covering basic operations, overflow, eviction,
cancel-after-eviction, and concurrent access patterns.

The filter module provides an IirFilter (single-pole exponential) and a
WindowAveragingFilter (generic over window size, with optional soft
initialization that fills the window with the first sample). The math
module provides lerp, linear_map, and range utilities with a
Number trait alias for generic numeric operations. The power module
provides battery percentage estimation from LiPo voltage curves.

6. Python Tooling

A pyproject.toml with uv.lock was added at the repo root, and Python
dependencies are integrated into the nix flake so they are available in
nix develop.

torque_data_writer.py decodes C struct telemetry from USB serial by
parsing the C header files in software-communication/ and unpacking
raw bytes into named fields. It can print to console or save to JSON.

packet_decoder.py is a general-purpose decoder for the radio packet
format (RadioHeader + payload), printing decoded BasicTelemetry,
ExtendedTelemetry, and ErrorTelemetry packets.

plot_telemetry.py plots decoded telemetry data (state estimates,
commands, errors) for offline analysis.

coms_reliability.py is a radio stress test tool that connects to the
robot's multicast group, floods it with command packets at configurable
rates, and measures packet loss, round-trip latency, and throughput.

7. Build and Clock Configuration

PLL3 was reconfigured: PLL3Q changed from 124 MHz to 48 MHz (required
for USB), PLL3P from 186 MHz to 192 MHz. HSI48 now syncs from USB SOF.
This fixes USB connectivity on the control board.

The Makefile adds CMakeLists.txt as a dependency for motor controller
targets. The VS Code nix extension recommendation was updated from the
abandoned extension to the community-maintained one. Packet buffer sizes
were increased from 60 to 80 bytes to accommodate the larger telemetry
structs. CLAUDE.md was added documenting repository structure and build
commands.

The inline float safety validation functions (is_float_safe,
is_command_packet_safe) were removed from lib.rs -- packet validation
is now handled at the controls library and packet decoding layers.