FOC Torque Control

Field	Value
Title	FOC Torque Control
Type	design
Status	draft
Version	0.1.0
Component	foc-torque
Date	2026-04-07

IMPORTANT — Implementation-blind document: This document describes behavior, structure, and responsibilities WITHOUT referencing code. No code blocks using programming languages (C++, C, Python, CMake, shell, etc.) are allowed. Use Mermaid diagrams to express behavior instead. Prose descriptions of algorithms are encouraged; source-level details are not.

Diagrams: All visuals must be either a Mermaid fenced code block (```mermaid) or ASCII art inline in the document. External image references using Markdown image syntax are not allowed.

Responsibilities

Is responsible for: - Executing the innermost FOC current control loop at 20 kHz in interrupt context - Converting three-phase stator currents to the rotor-synchronous dq frame via Clarke and Park transforms - Regulating the d-axis current (Id) to its setpoint using a dedicated PID controller - Regulating the q-axis current (Iq) to its setpoint using a dedicated PID controller - Converting the resulting voltage demands back to three-phase PWM duty cycles via inverse Park and SVM - Accepting a combined (Id, Iq) setpoint from an outer control loop or directly from an application - Providing Enable and Disable transitions that safely reset internal PID state

Is NOT responsible for: - Reading phase current values from the ADC hardware — currents are passed in as arguments - Committing duty cycles to the PWM hardware — duty cycles are returned to the Runner - Measuring or tracking rotor position — the mechanical angle is passed in as an argument - Outer-loop speed or position control - Flux weakening (Id ≠ 0 operation is structurally possible but not yet supported)

Component Details

Innermost Control Loop — 20 kHz ISR Execution

The torque control loop is the innermost layer of the FOC cascade. It is invoked once per PWM switching period (default 20 kHz) by the hardware interrupt handler that drives the three-phase inverter. The entire loop — from phase current input to duty cycle output — must complete within the interrupt budget.

The eleven-step sequence executed each cycle is:

Receive phase currents (Ia, Ib, Ic) as the PhaseCurrents argument.
Read the rotor mechanical angle θm from the Radians argument (supplied by the Runner from the encoder).
Compute the electrical angle: θe = θm × pole_pairs.
Evaluate cos(θe) and sin(θe) via the fast trigonometric LUT — one evaluation, results reused.
Clarke forward: (Ia, Ib) → (Iα, Iβ); Ic is derived from the balanced-phase constraint.
Park forward: (Iα, Iβ, θe) → (Id, Iq).
d-axis PID: error = Id_setpoint − Id → output = Vd.
q-axis PID: error = Iq_setpoint − Iq → output = Vq.
Inverse Park: (Vd, Vq, θe) → (Vα, Vβ).
Space vector modulation: (Vα, Vβ) → (duty_a, duty_b, duty_c).
Return PhasePwmDutyCycles to the Runner for immediate PWM register update.

d-axis and q-axis PID Controllers

Two independent incremental PID controllers regulate the direct (d) and quadrature (q) current components independently. Both PIDs share the same structural properties:

Output clamping: symmetric to [−1, +1], representing a normalised fraction of the DC bus voltage.
Anti-windup: the integrator is clamped whenever the output reaches the saturation boundary, preventing integrator wind-up during transients.
Gain derivation: the current regulator derives its own gains from the motor model (phase resistance, inductance) and the requested closed-loop bandwidth, normalised by the DC bus voltage. Tuning is therefore expressed as a single bandwidth in radians per second rather than raw controller gains.
Incrementalism: the PID uses the incremental (velocity) form, accumulating output from step to step, which naturally limits abrupt changes at Enable transitions.

The d-axis setpoint is normally 0 A for a surface PMSM (Id = 0 gives maximum torque per ampere). Setting Id_setpoint to a non-zero negative value for flux weakening is structurally possible but the operational consequences are outside the scope of this document.

Enable and Disable

Enable: arms both the d-axis and the q-axis PID controllers and resets their internal integrator state to zero. The most recently stored setpoints (Id_setpoint, Iq_setpoint) are re-applied immediately — there is no rollback to zero. This allows seamless re-engagement after a brief disable event without a step change in the setpoint.

Disable: disarms both PIDs. While disarmed, the Calculate() method returns zero duty cycles regardless of the current error. PID state is frozen and will be fully reset on the next Enable call.

The two states are the only run-time states of this component:

Current Envelope

The speed and position cascades bound the current reference their outer loop produces. Torque mode has no outer loop — the reference arrives from the operator, over CAN or the CLI — so the cascade applies the bound itself, at SetPoint, before the reference reaches the current loop.

The bound is on the magnitude of the current vector, not on either axis separately: the plant constraint is \(\sqrt{I_d^2 + I_q^2} \le I_{max}\), and a per-axis clamp would admit \(\sqrt 2\) times the limit on the diagonal. A setpoint outside the envelope is scaled onto it, preserving the commanded ratio between \(I_d\) and \(I_q\) rather than clipping one axis and rotating the vector.

The envelope comes from the inverter’s MaxCurrentSupported(), the same source the speed and position cascades take theirs from, so the three modes cannot disagree about the hardware limit.

The (Id, Iq) pair is written outside the interrupt and read by it. It is handed over through a lock-free triple buffer — the same mechanism that carries estimator windows the other way — so the interrupt always reads a pair published together, never a new Id with the previous Iq.

Pole-Pair Configuration

The pole-pair count is an integer property that translates the mechanical rotor angle supplied by the encoder into the electrical angle required by the Park transform. It must be configured before the first Calculate() call. Changing it while enabled produces undefined control behaviour and must be avoided.

Interfaces

Provided

Interface	Purpose	Contract
Configure	Supplies the motor model: resistance, inductance, flux linkage, bus voltage, sampling frequency and pole pairs.	Must be called before the first `Calculate()`. Must not be changed while Enabled.
Enable	Arms both PID controllers and resets their integrator state.	Safe to call repeatedly. PIDs start from a clean state each time. Last setpoints are preserved.
Disable	Disarms both PID controllers and forces zero duty cycle output.	Safe to call from any context. `Calculate()` returns zero while disabled.
SetCurrentTunings	Provides the current loop closed-loop bandwidth.	Gains are derived from the motor model and normalised internally. Takes effect on the next `Calculate()` call.
SetPoint	Sets the (Id, Iq) current setpoint in Ampere.	Scaled onto the current envelope on the way in. New setpoint is used on the next `Calculate()` invocation. Can be called while Enabled.
Calculate	Executes the full 11-step FOC torque loop for one control cycle.	Must be called at 20 kHz from ISR context. Returns `PhasePwmDutyCycles`. Must not block.

Required

Interface	Purpose	Contract
None	The inverter and encoder are managed externally by the Runner; they are not accessed directly by the torque control component.	—

Data Model

Entity	Field	Type / Unit	Range	Notes
Phase currents	Ia, Ib, Ic	Ampere (float)	± motor rated A	Ic derived; passed in from ADC callback
Synchronous frame	Id, Iq	Ampere (float)	± motor rated A	Regulated quantities
Current setpoint	Id_sp, Iq_sp	Ampere (float)	± motor rated A	Id_sp = 0 for SPMSM MTPA; Iq_sp from application
Voltage demands	Vd, Vq	Dimensionless (float)	[−1, +1]	PID output; normalised to DC bus
Duty cycles	Da, Db, Dc	Dimensionless (float)	[0.0, 1.0]	Returned to Runner for PWM register write
Mechanical angle	θm	Radians (float)	[0, 2π)	Supplied by Runner; pole-pairs multiply to θe
Electrical angle	θe	Radians (float)	[0, 2π)	θm × pole_pairs; used for Park and inverse Park
Pole pairs	P	Integer (unsigned)	≥ 1	Motor property; configured once at startup

Constraints & Limitations

Constraint	Value / Description
Control loop rate	20 kHz — must be called once per PWM switching period from the FOC interrupt.
Cycle budget	<= 4500 cycles (75% of the 6000-cycle control period at 120 MHz / 20 kHz) for the full `Calculate()` execution.
No virtual dispatch in hot path	`Calculate()` must not incur virtual function call overhead.
Output duty cycle format	Normalised floating-point [0.0, 1.0]; conversion to PWM timer counts is the Runner’s responsibility.
Electrical angle wrapping	Handled by the LUT normalisation within FastTrigonometry — no explicit modulo operation required in loop.
Gain derivation dependency	Gain derivation requires the motor model — including the DC bus voltage — to be supplied through `Configure()` first.
No flux weakening	Id_setpoint ≠ 0 is structurally accepted but operational flux-weakening strategy is not yet defined.
Current envelope	Vector magnitude bounded to the inverter’s `MaxCurrentSupported()` at `SetPoint`; it is not a regulator.
PID state at Enable	Integrators are always zeroed on Enable, regardless of previous state.

Controller Alternatives

The dual-PID current controller described above is one of several selectable strategies for the current loop. Alternative controllers — Decoupled PID, Deadbeat, and Sliding-mode — are described in documentation/theory/advanced-controllers.md (current-loop-decoupled-pid.md, current-loop-deadbeat.md, current-loop-sliding-mode.md). Plant model foundations are in documentation/theory/foc-plant-models.md.

The mechanism by which a controller strategy is selected at runtime, stored without heap allocation, dispatched in the ISR via type-aware visit, and persisted to NVM is described in documentation/design/controller-selection.md.