Field-oriented motor control, encoder fusion, and fault recovery
for a two-axis brushless camera gimbal using SimpleFOC v2.4.0
This paper describes the design and implementation of a dual-axis brushless gimbal controller for the MicroRobo Systems GPS beacon tracking system. A Teensy 4.1 runs SimpleFOC v2.4.0 at a 2 kHz field-oriented control loop rate, driving two iPower GBM5208 gimbal motors (11 pole pairs, 12 V) through L6234-based SimpleFOC shields. Two AS5048A 14-bit magnetic encoders provide shaft angle feedback on separate SPI buses. The elevation axis uses cascaded angle-velocity-voltage control; the azimuth axis uses direct-voltage angle control, selected for its superior stability on a low-friction bearing. Key design decisions are discussed: differential control mode selection, PWM frequency choice, the exclusion of center-aligned 3PWM mode due to FlexPWM register conflicts, the rationale for zeroing the velocity derivative term, the 2 kHz loop rate cap, second-order IIR notch filtering for structural resonance suppression, and a three-tier runaway fault detection and recovery state machine. Measured steady-state position hold accuracy is 0.46° RMS on elevation and 0.47° RMS on azimuth under full camera payload at 12 V.
A camera gimbal for optical target tracking has requirements that differ materially from an aerial photography gimbal. The tracking application demands fast, accurate step response to angle commands arriving at 200 Hz from the ground station, while the camera-stabilization application demands smooth output free of high-frequency jitter. These requirements pull in opposite directions: bandwidth and smoothness are in tension, and the resolution of that tension defines the core tuning challenge.
The controller described here addresses this through two mechanisms: differential control modes per axis (cascaded on elevation, direct-voltage on azimuth) selected to match the mechanical load profile of each axis, and second-order IIR notch filtering applied post-loopFOC() to suppress a 4.9 Hz structural resonance without constraining the closed-loop bandwidth at tracking frequencies.
| Component | Part | Notes |
|---|---|---|
| Flight computer | PJRC Teensy 4.1 | 600 MHz Cortex-M7, hardware FPU |
| Motor driver ×2 | SimpleFOC Shield v2.0.4 (L6234) | Stacked on Teensy headers, one per axis |
| Gimbal motor ×2 | iPower GBM5208 | 11 pole pairs, 12 V, R ≈ 14 Ω |
| Encoder (elevation) | AMS AS5048A | 14-bit SPI, SPI1: MOSI=26, MISO=1, SCK=27, CS=0 |
| Encoder (azimuth) | AMS AS5048A | 14-bit SPI, SPI0: MOSI=11, MISO=12, SCK=13, CS=10 |
| Power supply | 12 V DC, ≥ 3 A | Shared between both shields |
The two AS5048A encoders are placed on separate SPI buses — SPI1 for elevation (CS=0), SPI0 for azimuth (CS=10) — rather than sharing a single bus with separate chip selects. While the AS5048A supports multi-device SPI with shared MOSI/MISO/SCK, the 2 kHz FOC loop requires both encoders to be read on every tick. A shared bus would serialize these reads, adding latency and reducing effective loop rate headroom. On separate buses they execute concurrently.
The elevation and azimuth axes use different SimpleFOC control modes, chosen to match the distinct mechanical load profiles of each axis.
Elevation — MotionControlType::angle (cascaded). The cascaded architecture routes the angle error through an outer angle PID to produce a velocity demand, which passes through an inner velocity PID to produce a voltage command. The velocity integrator (PID_velocity.I = 8.0) winds up against the static camera weight load, providing stable position hold without a steady-state angle error offset. Without the integrator the camera would sag below the commanded elevation under gravity load, requiring a constant commanded offset that varies with zoom lens position and payload balance.
Azimuth — MotionControlType::angle_nocascade (direct voltage). Added in SimpleFOC v2.3.x, this mode routes the angle PID output directly to motor voltage, bypassing the inner velocity loop entirely. The azimuth bearing has low, uniform Coulomb friction. The inner velocity loop added phase lag without stability benefit for this load profile, and the direct-voltage mode produced superior tracking step response. The full PID on P_angle (P for proportional tracking, I for steady-state offset correction, D for arrival damping) compensates for all bearing imperfections.
Both drivers are configured to 25 kHz via d.pwm_frequency = 25000 in the axis initialisation function. The SimpleFOC default for Teensy is significantly lower and produces an audible whine through the gimbal structure at frequencies in the range 1–5 kHz. At 25 kHz the switching frequency is above the audible range and above the structural resonance frequencies of concern.
The SIMPLEFOC_TEENSY4_FORCE_CENTER_ALIGNED_3PWM define is intentionally absent. Both drivers share FlexPWM2: the elevation driver occupies submodules 1 and 2, the azimuth driver occupies submodules 0 and 3. The SimpleFOC center-aligned initialisation sequence writes to module-level MCTRL and SYNC registers that are shared across all submodules of a FlexPWM instance. The second driver initialisation overwrites these registers, corrupting the phase alignment already established on the first driver. Edge-aligned fast PWM writes only submodule-level registers and avoids this collision.
The velocity PID derivative term is zeroed on both axes (PID_velocity.D = 0). The AS5048A produces 14-bit CORDIC output with approximately 0.02° quantization (≈ 0.35 mrad). At the 2 kHz FOC loop rate, consecutive angle samples frequently differ by exactly one LSB — a rectangular velocity noise signal of amplitude one LSB per sample interval. The D term differentiates this signal, producing high-frequency torque commands that cause audible torque reversals and visible motor jitter. The velocity LPF (LPF_velocity.Tf = 0.01 s) smooths the velocity estimate and is the correct tool for this noise.
The main loop gates FOC execution to one call per 500 µs. The AS5048A's internal CORDIC pipeline refreshes at approximately 1 kHz. Running the FOC loop faster causes repeated identical angle readings; the velocity estimator interprets a run of identical readings followed by a large step as a burst of zero velocity and then a velocity spike, producing oscillatory torque. The 2 kHz cap ensures at most one repeated reading per two ticks, which the velocity LPF handles correctly.
motor.sensor_offset is assigned after motor.initFOC(), not before. initFOC() uses the raw sensor reading to align the electrical zero of the motor to the encoder reference position. Applying sensor_offset before initFOC() shifts the angle reference used for electrical zero determination, producing incorrect commutation and unreliable torque production. The correct initialisation sequence is:
| Step | Call | Purpose |
|---|---|---|
| 1 | sensor.init() | Establish SPI communication, read initial angle |
| 2 | motor.linkSensor() | Associate encoder with motor instance |
| 3 | driver.init() | Configure PWM channels and enable gate driver |
| 4 | motor.init() | Apply PID parameters and limits |
| 5 | motor.initFOC() | Calibrate electrical zero using raw sensor angle |
| 6 | motor.sensor_offset = x | Apply mechanical offset after electrical calibration |
Plant characterization of the elevation axis using a swept-sine (chirp) input identified a structural resonance at 4.9 Hz with a 34 dB amplitude peak — large enough to drive into nonlinear operation if left unaddressed. The resonance appears in the Bode gain plot as a sharp peak above the 0 dB reference, indicating that the closed-loop system amplifies rather than attenuates motion at this frequency. A second-order IIR notch filter at 4.9 Hz, 1.0 Hz bandwidth, tuned to the 2 kHz sample rate suppresses this peak.
ω₀ = 2π × 4.9 / 2000 = 0.01539 rad/sample
r = 1 − (2π × 1.0 / 2000) / 2 = 0.99843
b₀ = b₂ = 1.0 b₁ = −2cos(ω₀) = −1.99976
a₁ = −2r·cos(ω₀) = −1.99951 a₂ = r² = 0.99687
The notch filter writes directly to motor.shaft_angle and motor.shaft_velocity after loopFOC() returns. This is an intentional deviation from standard SimpleFOC usage, where these fields are treated as read-only outputs of the sensor update. It is valid here because SimpleFOC re-reads the physical sensor at the start of the next loopFOC() call, overwriting the filtered values. The PID controllers therefore see notch-filtered feedback while the physical sensor state is unaffected. Three filter instances are maintained: one for elevation shaft angle (notchE, f₀ = 4.8 Hz), one for elevation shaft velocity (notchE_vel, f₀ = 4.9 Hz), and one for azimuth shaft angle (notchA, f₀ = 4.9 Hz).
The ground station sends angle commands at up to 200 Hz over USB Serial at 115200 baud. The command format is ASCII line-terminated: E{rad:.3f}\n for elevation, A{rad:.3f}\n for azimuth. At 10 bytes per command pair and 200 Hz, the link is loaded to approximately 2 kbyte/s — well within the 11.5 kbyte/s capacity at 115200 baud.
The parser is non-blocking. It accumulates bytes into a 12-byte buffer on each handleSerial() call and extracts the angle value only when a newline is received. The parsed angle is written to the axis state structure; the FOC loop picks it up on the next tick via wrapNearest().
SimpleFOC accumulates shaft_angle without wrapping to [0, 2π] — the unwrapped angle can be any real number. Ground station commands are in the range [−π, +π]. The wrapNearest() function converts a ground-station absolute angle to SimpleFOC's unwrapped space by taking the shortest angular path from the current shaft position:
delta = commanded − shaft_angle
while delta > π: delta −= 2π
while delta < −π: delta += 2π
target = shaft_angle + delta
Without this conversion, a command of 0.1 rad sent to a motor at shaft_angle = 100.0 rad would produce a full rotation rather than a small correction.
The controller streams 17-byte binary feedback frames to the ground station at 200 Hz (every 10 FOC ticks). The frame contains Teensy timestamp, elevation and azimuth shaft angles, and the quadrature voltage Vq for each axis. Vq is the torque-producing component of the FOC voltage vector — it is proportional to motor torque and useful for diagnosing load, saturation, and bearing friction in post-flight analysis.
| Offset | Size | Type | Field |
|---|---|---|---|
| 0 | 1 | uint8 | Header byte: 0x46 ('F') |
| 1 | 4 | uint32 | timestamp_us — Teensy micros() |
| 5 | 4 | float32 | angle_E — elevation shaft_angle (rad) |
| 9 | 4 | float32 | angle_A — azimuth shaft_angle (rad) |
| 13 | 4 | float32 | vq_E — elevation quadrature voltage (V) |
| 17 | 4 | float32 | vq_A — azimuth quadrature voltage (V) |
An uncontrolled brushless motor in FOC mode can accelerate rapidly if a sensor fault produces a corrupted angle reading. The fault state machine distinguishes between three scenarios — transient encoder glitches, sustained sensor contamination, and genuine motor runaway — and applies an appropriate response to each.
Tier 1 — Instant clamp (|ω| > 40 rad/s). A velocity exceeding 40 rad/s on any axis is physically implausible given the GBM5208's rated slew and the inertia of the camera payload. This reading is an encoder fault — typically a SPI noise spike producing a single corrupted angle sample. The motor is disabled immediately and enters glitch hold. The hold clears when the velocity LPF has decayed below 3 rad/s, confirming the noise event is over.
Tier 2 — Runaway watchdog (|ω| > 15 rad/s for 500 ms, travel > 1 rad). A sustained velocity above 15 rad/s is ambiguous: it could be an LPF contaminated by a large but finite noise event, or genuine motor runaway. The travel criterion resolves the ambiguity — if the motor has moved more than 1 radian (≈ 57°) during the high-velocity period it is genuinely running away. Less than 1 radian of actual travel during a high-velocity reading is classified as LPF contamination (a glitch, not a runaway) and enters the glitch hold path. Genuine runaway disables the motor, waits 2 seconds for the mechanics to settle, and resumes at the current shaft angle.
Tier 3 — Repeated fault halt. More than 3 runaway events or more than 10 spike events since power-on indicate a hardware problem — degraded SPI connection, encoder supply noise, or mechanical damage. Further recovery attempts are unsafe. The firmware halts via while(1), stopping both axes regardless of which faulted. A power cycle is required to resume. The pre-fault telemetry circular buffer is printed to Serial before halt to assist diagnosis.
The following per-axis calibration must be performed on initial installation and repeated after any motor or encoder replacement.
| Step | Action | Result |
|---|---|---|
| 1 | Set zero_electric_angle = 0, sensor_offset = 0 | Baseline — raw calibration |
| 2 | Upload, open Serial Monitor at 115200 baud | SimpleFOC prints detected electrical zero during initFOC() |
| 3 | Record the printed electrical zero angle | This is the correct zero_electric_angle value |
| 4 | Update zero_electric_angle with recorded value, upload | Motor now commutates correctly |
| 5 | Command the motor to the desired home position; read shaft_angle from Serial | Record the angle at the desired home position |
| 6 | Set sensor_offset = −shaft_angle at home, upload | Motor homes to the correct position on power-up |
Both GBM5208 motors are datasheet-specified at 11 pole pairs and confirmed at bench. If a substitute motor is used, run SimpleFOC's find_pole_pairs_number() utility before setting the BLDCMotor(N) constructor argument. An incorrect pole pair count produces a 180° position error at certain commanded angles and prevents stable position hold.
Plant characterization was conducted with the BirdDog MAKI Ultra camera payload installed at 12 V supply. The elevation and azimuth axes were characterized independently using swept-sine (chirp) and step response tests. Data was recorded via the binary feedback telemetry at 200 Hz and analyzed in Python.
| Metric | Elevation | Azimuth |
|---|---|---|
| Control mode | Cascaded angle | Direct voltage (nocascade) |
| Rise time (10–90%) | 0.120 s | 0.160 s |
| Overshoot | 5.6% | 0.0% |
| Damping ratio ζ | 0.655 | ~0.757 |
| Natural frequency ωn | 3.81 rad/s | ~10.64 rad/s |
| Steady-state RMS error | 0.46° | 0.47° |
| 95th percentile error | 1.03° | 0.91° |
| Closed-loop bandwidth (−3 dB) | 0.53 Hz | 1.65 Hz |
| Structural resonance | 4.9 Hz, 34 dB peak | Not observed |
| Phase crossover frequency | 0.40 Hz | 0.05 Hz |
| Mean Vq in steady state | 0.46 V | −0.01 V |
| FOC loop rate | 2 kHz (500 µs gate) | |
| Feedback telemetry rate | 200 Hz (every 10 FOC ticks) | |
The 0.46 V mean Vq on the elevation axis confirms the velocity integrator is actively holding the camera weight against gravity. The near-zero azimuth mean Vq confirms the direct-voltage mode is not compensating against a constant torque disturbance, validating the mode selection rationale. The 0.46°/0.47° RMS steady-state error reflects residual low-frequency oscillation at frequencies below the notch filter. The 34 dB elevation resonance is the dominant candidate for improvement — either through mechanical damping of the structural mode or a broader-bandwidth notch.
The zero_electric_angle and sensor_offset values in setup() are specific to the current motor and encoder assembly. Any motor or encoder swap requires full recalibration of the affected axis. There is no runtime self-calibration mechanism; this is a known operational dependency.
The while(1) in the fault state machine halts both axes regardless of which one faulted. For applications requiring independent per-axis fault isolation, the halt should be replaced with a per-axis m.disable() and a permanent disabled flag checked in loop(). The current behaviour is deliberate — a runaway on one axis typically indicates a systemic problem (power supply sag, SPI noise) that could affect the other axis as well.
The SimpleFOC Shield v2.0.4 does not expose current sense shunts in this configuration. Motor protection relies entirely on voltage limiting. Overcurrent protection depends on the power supply's current limiting behaviour. Adding current feedback would enable true torque control and thermal protection.
The 0.46°–0.47° RMS error is larger than the pointing accuracy a narrow-field 20× zoom camera requires for reliable target retention at long range. The 20× zoom camera has a horizontal field of view of approximately 3.4° — the target must be within roughly half this angle of the pointing center to remain in frame. At the measured error distribution (95th percentile ≈ 1.0°), the target will occasionally leave the frame during high-dynamics tracking. Mechanical damping of the 4.9 Hz resonance is the highest-leverage improvement path.
The gimbal controller demonstrates that SimpleFOC v2.4.0 on a Teensy 4.1 provides sufficient compute margin for 2 kHz dual-axis FOC, 200 Hz feedback telemetry, and a full fault state machine within a single-core embedded application. The differential control mode selection — cascaded on the gravity-loaded elevation axis, direct-voltage on the low-friction azimuth axis — produces distinct dynamic characteristics optimized for each axis's load profile, as confirmed by the measured damping ratios and frequency responses.
The notch filter approach to structural resonance suppression is effective for the identified 4.9 Hz mode but is a narrow-band solution. A mechanical fix — identifying and damping the compliant joint responsible for the resonance — would be more robust and would potentially reduce the steady-state error floor.
Source code, schematics, and plant characterization data are available at github.com/lcumbridge/mrs-gimbal-controller. Commercial licensing inquiries should be directed to microrobosys.com.