CBF Safety Filter on STM32 Clamps 48 kW ML Commands to 180 W

During a simulated fault phase, the ML controller on an autonomous surface vessel emitted a thrust command equivalent to about 48 kW against motors rated 180 W. That is 48,000 / 180 ≈ 267 times rated power. On the water, the ESCs would fail within seconds and the wiring would carry the excess as I²R heat. The working fix is a three-stage pipeline: a distilled PD equation in one C function, a deterministic control-barrier-function (CBF) filter between that function and the ESC, and a hardware limit behind both.

Overcommand mechanism: why the fault phase produced 267x rated power

A learned controller has no built-in notion of the actuator's power rating. Its output layer extrapolates when a sensor fault pushes inputs outside the training range, and nothing in the network bounds the result. A PD equation has the same property: gain times error plus gain times gyro rate grows without limit as the error term grows. Three conditions produce an unsafe command:

• Out-of-range inputs: a failed gyro or error signal drives the model or equation far outside its training distribution.
• NaN or Inf inputs: a NaN propagates through float arithmetic to the output. Ordered comparisons against NaN evaluate false, so a plain if (u > max) clamp lets NaN through.
• No mapping from command to power: the controller output is a unitless thrust command. The 180 W motor limit only means something once you convert it to a command bound using the motor and propeller data.

Runtime protection options compared

Four approaches cover the runtime-safety question for ML on a peripheral. They differ in where they act and how they behave at the limit.

Recommended stack: equation, CBF filter, hardware backstop

Use all three protective layers in series, with the hardware limit independent of the firmware. The filter is not AI: it is fixed arithmetic with no learning, no inference, and no uncertainty. The controller generates the command; the filter guarantees the command is inside the physical envelope. The two do different jobs.

1. Sensors (gyro, error, thrust) feed the distilled PD equation.
2. The equation output passes through the CBF filter, which attenuates or zeroes it.
3. The filtered value goes to the ESC.
4. The fuse or driver current limit stays in place as the last line of defense. It is binary, so it protects against faults the filter cannot see, but it should never be the layer that does routine limiting.

The filter's job is to act proportionally before the hardware limit is reached. With that ordering the motor sees no sudden stop and the fuse does not trip.

Distilling the controller into one MISRA-C function

Replace the ML model with a closed-form equation so the deployed code is a single small function. The pipeline uses PySR symbolic regression on logged sensor data.

1. Log gyro, error, and thrust from real or simulated runs. Confirm the log spans the operating range you expect, including the disturbance cases, before fitting.
2. Run PySR on the log. The result is a pure PD controller equation derived from the data: one equation, not a model. Confirm the recovered form is a proportional term on error plus a derivative term on gyro rate, with constants you can read.
3. Compile the equation into a single MISRA-C function. Confirm the linked function is under 1 KB of flash, uses no heap allocation, and pulls in no ML runtime.
4. Compare the equation output against the original ML output on held-out data. Confirm the tracking error is acceptable before this function replaces the model on the vessel.

The distilled equation is still unbounded. Distillation makes the controller auditable and small; it does not make it safe, which is why the filter follows it.

Filter logic: NaN gate, bound, and attenuation

The filter has two rules: any non-finite input forces zero thrust, and the command is projected onto the allowed set. For a scalar thrust command, the minimum-modification projection onto a symmetric bound is a clamp. The proportional behavior comes from a bound that scales with remaining margin.

#include <math.h>

/* Sketch. u_max comes from the motor/propeller power-to-command map (180 W motor).
   scale in [0,1] shrinks the bound as margin shrinks (for example, measured
   current or temperature headroom). Symmetric bound assumes a bidirectional ESC. */
float safe_thrust(float u_cmd, float u_max, float scale)
{
    float u_lim;
    if (!isfinite(u_cmd) || !isfinite(scale)) { return 0.0f; }
    if (scale < 0.0f) { scale = 0.0f; }
    if (scale > 1.0f) { scale = 1.0f; }
    u_lim = u_max * scale;
    if (u_cmd >  u_lim) { return  u_lim; }
    if (u_cmd < -u_lim) { return -u_lim; }
    return u_cmd;
}

1. Derive u_max from the 180 W motor rating using the motor and propeller datasheet curves. Power rises nonlinearly with command for a propeller load, so a linear scaling of the command is wrong; read the power-versus-command relationship from the datasheet. Confirm by applying u_max on a bench supply with a current reading and checking that power stays at or below 180 W.
2. Place the NaN test before any comparison. Confirm by injecting a NaN into the input and reading zero at the ESC output.
3. Choose how scale is computed from a measured margin. Confirm that the applied command falls smoothly, with no step, as margin approaches zero.

Timing budget on the STM32F401

The full cycle of equation output, CBF filter, and actuation measured about 9 µs on an STM32F401. Verify that number on your build rather than trusting it, because compiler flags, clock configuration, and FPU use all move it.

1. Read your control-loop period from the timer configuration. Confirm the 9 µs figure is a small fraction of it.
2. Toggle a spare GPIO pin high before the equation call and low after the ESC write. Confirm the pulse width on a scope.
3. Force the NaN branch and the clamp branch separately and measure each. Confirm the worst-case path, not only the nominal path, fits the budget.

What the filter does not cover

• Same-MCU failure: if the MCU hangs or the ESC output register freezes, the filter never runs. The driver-level or fuse limit is the independent layer for this case, and a hardware watchdog that drops the ESC command on timeout covers the firmware-hang case.
• Plausible wrong values: a stuck sensor that returns a finite, in-range number passes the NaN gate. The bound still limits power, but the controller acts on bad data. Add plausibility or rate checks on each sensor if that failure is in scope.
• Wrong bound: a filter with an incorrect u_max protects nothing. The bound is only as good as the command-to-power map behind it.

Fault-injection test cases and pass criteria

Run these with the propeller unloaded or the motors on a current-limited bench supply, not in the water.

It is a simulation result.

FAQ