From noisy sensor to torque command, on time.

An exponential moving average, a first-order Butterworth and a second-order Butterworth biquad in Direct Form I, chosen per caller by configuration. The Butterworth coefficients are the bilinear transform of the analog prototype ($Q = 1/\sqrt{2}$, pre-warped), which is what the library header points at: maximally flat through the passband, then 20 dB per decade per order. That is why a second-order output comes out straighter, and also why it arrives later.

The difference equations were the easy part. Most of my time went into how the filters start up and recover.

If a filter's delay line starts at zero, the first real sample looks like a step input, and the output shows a transient that has nothing to do with the car. To avoid that, every constructor primes the delay line with an initial value. For the biquad, that means setting all four state variables ($x[n-1], x[n-2], y[n-1], y[n-2]$) so a constant input gives a constant output from the first sample.

The same problem shows up after a sensor dropout or a fault: the filter keeps averaging old data that no longer reflects what the car is doing. So each filter has a reset() that re-primes the state from the most recent input, so stale samples don't leak into the output.

Reset is cheap because the biquad is Direct Form I. Its state is just the last two inputs and the last two outputs, so re-priming takes four assignments. In Direct Form II, the state is an intermediate signal, so a reset would mean solving for its steady-state value instead of copying the input. Direct Form I also let me check every line directly against the difference equation, which made review simpler for both my lead and I.

The constructors assert on configurations that have no valid answer: a cutoff or sample rate of zero or less, an EMA $\alpha$ outside $[0, 1]$, or a biquad cutoff at or above $f_s/2$. At that point the prewarp term $\tan(\pi f_c / f_s)$ blows up and the coefficients stop meaning anything.

That $f_s/2$ limit is the Nyquist–Shannon sampling theorem. A signal sampled at $f_s$ can only represent frequencies below $f_s/2$; anything above that folds back down and shows up as a lower frequency that was never there. A cutoff past that point asks the filter to separate frequencies the samples can't contain, so I check for it at construction instead of letting it build a filter that quietly misbehaves. The filter can't undo aliasing that already happened when the sensor was sampled, though. That's set by the sample rate each subsystem chooses.

Everything else is allowed, including settings that are legal but come with costs. A very low cutoff relative to the sample rate pushes the poles toward the unit circle, where float rounding starts to matter. Lower cutoffs also add delay. I documented both in the spec instead of banning them, because each subsystem knows its own task rate and latency budget and the library doesn't.

The filter doesn't clamp values or detect faults either. Some subsystems need exactly what the sensor reported, so deciding what counts as out of range or invalid is up to the caller. The filter only filters.

The filter modules are covered by GoogleTest unit and integration tests. I then ran the library on IMU data across 20 controlled bench runs. On average it removed 93% of the noise power, with group delay at 20 ms typical and 80 ms worst case.

I report those two numbers together because they're one tradeoff. Every choice of filter order and cutoff gives you smoothness at the cost of delay, so each team picks the balance that fits their subsystem and filters their own inputs.

This was my first task at UBC Formula Electric. I wrote it in C first, then ported it to C++ when the team's firmware moved over, and nothing broke in the port.

It's where I learned how an analog filter becomes a digital one (Laplace transform, transfer functions, the bilinear transform), and how the cutoff and sample rate together set the group delay a controller has to live with. I also learned to write GoogleTest suites, and to bench test a filter and measure its delay directly instead of just reading a plot.

The lesson that stuck was about the people using it. Every subsystem has its own constraints, so the design that worked was the one that rejected only what was impossible and documented everything else.

The FMAC is a fixed-function unit on the STM32H7 that runs FIR and IIR filters in hardware. It has its own local memory for input samples, coefficients and outputs, and it works in 16-bit fixed point. Once it's configured and fed data, it does the multiply-accumulates itself, so the CPU isn't spending instructions on the arithmetic. It was already on our board, and nothing was using it.

A filter running at 100 Hz is a small cost, but it's paid a hundred times a second for as long as the car is on, inside the control tasks themselves. A small, constant load like that is easy to ignore and hard to win back later, and the tasks paying for it are the safety-critical ones. Moving the filter into hardware lets each task do the same work in fewer cycles, and the cycles saved become margin between the task and its deadline.

The FMAC could run asynchronously: the task hands over a sample, blocks, and is woken by an interrupt or a DMA completion when the result is ready, while FreeRTOS runs something else in between. The driver deliberately doesn't do that.

Each call filters a very small amount of data. A first-order IIR needs the current sample and the previous output, and nothing else, and a short FIR window would be in the same position. Going asynchronous costs two context switches per sample: saving the waiting task's registers, choosing and running another task, then restoring the first one when the interrupt fires. For a filter this small, that overhead would cost more than the arithmetic it lets another task overlap with.

So the control task polls the FMAC and waits, and the wait is short. The trade is that the CPU does nothing else during that wait, which is the right call while the filter costs less than a context switch. That reasoning still has to be backed by numbers: profiling the filter in software, on the FMAC, and a FreeRTOS context switch on this chip is the next step, and if a larger filter ever moves onto the FMAC, those numbers are what decide whether it stays synchronous.

The FMAC uses Q1.15 format: every value is a 16-bit integer representing a number between −1 and just under +1. A gyro reading in degrees per second doesn't fit that range, so the driver scales each sample by the sensor's full scale ($\pm 250$ °/s), filters it, and scales it back.

The edge of that range is where it broke. At exactly full scale, $1.0 \times 32768$ is one past the largest int16, and the cast wraps it to $-32768$. The reading flips sign exactly when the sensor is maxed out. The driver now clamps to the largest value Q1.15 can hold before converting, and the FMAC's saturation mode is enabled so an overflow inside the filter clips instead of wrapping.

The coefficients come from the same Butterworth formula as my filter library, with one sign flipped. The library stores $a_1$ for $y[n] = \ldots - a_1 y[n-1]$, but the FMAC computes $+A_1 y[n-1]$, so the driver passes in $-a_1$. For a first-order filter at a 10 Hz cutoff and 100 Hz sample rate, every coefficient is inside $\pm 1$, so no gain shift is needed. That is why it's first-order: a second-order Butterworth at the same cutoff has $a_1 \approx {-1.14}$, which doesn't fit Q1.15 without a gain shift.

Getting it running on hardware was the hard part, because the boards were being developed at the same time as the firmware. A fault could be in my code, in the CubeMX configuration (an NVIC priority, for example), or in the hardware itself. The symptoms ranged from no IMU data to overflow errors to hard faults. I narrowed them down by reading registers in GDB and probing the board until only one explanation was left.

Three of the bugs were mine, and all three came from HAL functions not doing what their names suggest.

HAL_FMAC_AppendFilterData doesn't copy the sample. It keeps the pointer and reads it during the next poll. My sample was a local variable that was gone by then, so I moved the input and output buffers into the driver object, where they outlive the call.

HAL_FMAC_FilterPreload loads the filter's history, not its coefficients, and I had been passing it the coefficient arrays. It now preloads $P - 1$ zeros instead of $P$, because the extra slot was feeding a phantom zero into the pipeline.

The poll clears its output pointer on every call, and the first poll often only finishes writing the input. So the driver re-registers the output buffer on each pass and keeps polling within its own time budget, instead of treating "not ready yet" as a failure.

The first version waited on HAL_MAX_DELAY, which never times out. If the peripheral stopped responding, the calling task would hang. It's now a 100 ms timeout that returns an error the caller has to handle.

If a sample can't be appended, the handshake with the FMAC has fallen out of sync. The driver keeps its own copy of the filter configuration, because the caller's coefficient arrays are usually temporaries. That lets it stop, reconfigure and restart the filter, then retry the sample once before giving up.

On the IMU side, only valid readings go in. A failed SPI read is skipped instead of being pushed into the filter, where one bad value would stay in its history and skew every output after it.

This was my first time writing firmware below the application layer, in the hardware abstraction layer instead of app code. The hardest part was the driver itself: reading the reference manual, working out how the peripheral actually behaves, and turning that into a configuration that runs.

Debugging taught me the rest: stepping through registers in GDB, probing the board to rule hardware in or out, how an RTOS schedules tasks, how interrupts and their priorities interact, and how much of a firmware bug lives in configuration rather than code.

This project is what got me into firmware. The filter library taught me the maths. This one taught me that I like the part where the maths meets the silicon.

Without CubeMX, nothing runs before main unless I write it. I wrote startup.s by hand in ARM assembly. It holds the vector table, and its reset handler copies .data from flash to RAM, zeroes .bss, then calls main. The linker script places each section in memory and exports the symbols the startup code uses to find them.

The vector table is 98 words, and it checks its own length at assembly time. That check exists because of ST: the reference manual's vector table for this part skips position 66, so copying it row by row gives a 97-word table where every interrupt after 65 is shifted by one. It builds and boots fine, and only breaks once you enable an interrupt past that point and the wrong handler runs.

To prove the startup code works rather than assume it, I put a known value in .data (0xDEADBEEF) and one in .bss, then read both back at a breakpoint on main. I also found 0xDEADBEEF in the binary at the address the linker said it would be.

A printf call hard-faulted, and I decoded it end to end. HFSR said forced, CFSR said a precise BusFault with a valid fault address, and the stacked PC landed inside iprintf, loading through _impure_ptr. That pointer read back as garbage, and the fault address was exactly that garbage plus 8. Everything agreed.

None of it was real. The board was still running the previous binary, and I was decoding it against the new ELF. After a clean flash, compare-sections matched and the fault never came back. The first clue was a watchpoint on _impure_ptr that never tripped, and GDB had been warning me the whole time: "Source file is more recent than executable."

The rule I took from it: check that flash matches the ELF before decoding anything. A decode against the wrong image isn't weak evidence. It's fabricated evidence, and it's convincing because it's internally consistent.

SysTick runs at 1 kHz at the lowest priority, because a tick handler should never delay a device interrupt. Getting an exact 1 kHz meant knowing which clock SysTick actually sees and setting the reload value from that.

delay_ticks() measures elapsed ticks by unsigned subtraction instead of comparing against a computed deadline, so it survives the counter wrapping. I tested this by forcing the counter to 0xFFFFFFF0 just before a wait: it passed through zero and still waited the full length. If SysTick isn't running, it traps on a breakpoint instead of returning early, so a hang shows up as a hang and not a subtle timing bug. Public timing APIs take ticks, not milliseconds, the same approach as pdMS_TO_TICKS, so the quantisation stays visible to the caller.

Task control blocks, per-priority ready lists, blocked and delayed states with tick-based wakeup, and a lowest-priority idle task so the core always has something valid to run.

The context switch comes first. On exception entry the hardware stacks eight registers, and the PendSV handler stacks the other eight, r4 to r11, in assembly. Each task's initial stack frame is built to look exactly like a preempted task, so the first launch is just a resume. That's one code path instead of a special case that only runs once.

Counting semaphores, and mutexes with priority inheritance. The plan is to reproduce unbounded priority inversion first, with three instrumented tasks on a scope, and only then fix it, so the fix is demonstrated rather than claimed.

With 16 KB of SRAM, there's no room to be careless with stacks. Each task stack is filled with a known pattern at creation. Scanning from the bottom, the first word that no longer holds the pattern marks the deepest the stack has ever reached, which gives that task's high-water mark. If the pattern at the very bottom is gone, the stack has overflowed, and the kernel reports it as a named fault for that task, instead of a HardFault whose stacked PC points at whichever task ran next.

Register-level work on this part means reading the right table. The F3 puts GPIO on AHB rather than APB2, its USART uses ISR with separate TDR and RDR registers, and its baud register holds the divider directly instead of the mantissa and fraction most tutorials shift into place. Nearly every STM32 example online targets the F1 or F4, so the habit that matters is reading registers back in GDB after configuring a peripheral, because the compiler can't tell which register I meant.

Writing GPIO and UART drivers from scratch without a HAL, and learning SPI and I2C alongside UART, so I understand how the three serial protocols differ instead of only the one I needed first.

Writing my own startup code in assembly and my own linker script: what the vector table is and how the core uses it on reset, what sections are, how .data and .bss get set up before main, and how to read and write a linker script.

How the chip is clocked: the internal RC oscillator versus an external quartz crystal and why you'd pick one over the other, how the PLL and the system clock mux choose and multiply a source, and how the AHB and APB prescalers divide it down for each bus. That's what turns a clock frequency into ticks and real time, and it's why a peripheral's timing depends on which bus clock it sits on. Some peripherals run off those prescaled bus clocks and some don't, so I learned to check which clock a peripheral actually uses before computing anything from it.

On this board that means the internal 8 MHz HSI with no PLL yet, so SYSCLK and the APB1 clock PCLK1 are both 8 MHz, and USART2, which this part wires straight to PCLK1, takes its baud divider from it: $8\,\text{MHz} / 115200 \approx 69$. The PLL comes later, and the limits are already worked out: from HSI it tops out at 64 MHz, because 72 MHz needs an external crystal the Nucleo doesn't fit, and APB1 is capped at 36 MHz, so it has to run at half of SYSCLK, 32 MHz, and every divider derived from PCLK1 moves with it. The clock tree is derived from three numbers in one header, so that change is a two-number edit.

Reading the reference manual and the core documentation until a peripheral makes sense, debugging at register level, and how PSP and MSP split the stack between tasks and the kernel. Exception handling is what I'm working on now.

Four states on distance thresholds: searching with the claw open, engaging when the object comes inside 30 cm, locked while it is carried, and disengaging back to searching once the reading clears 60 cm at the drop zone. The gap between the two thresholds is the hysteresis that keeps a borderline object from being picked up and released on alternate samples.

A first-order Butterworth low-pass in Direct Form I, $y[n] = b_0 x[n] + b_1 x[n-1] - a_1 y[n-1]$, cutting at 1 Hz against a 20 Hz sample rate. Separate feedforward and feedback delay lines, and the last valid output is held when a reading comes back invalid, so a dropout costs a sample of freshness instead of a spurious transition. Butterworth for the maximally flat passband: the point is to remove the spikes without dragging the legitimate approach.

config, sonar, filter and claw split into separate modules, with every pin, threshold and filter coefficient in one header. Teammates who were not writing the firmware could change how the machine behaved and test it, which on a first-year team is the difference between one person owning the code and five people making progress.

The module takes the IMU's health from the main torque-vectoring checks and picks a level every cycle. INACTIVE while the IMU is healthy, still computing its torque in the background so the handover is bumpless. ACTIVE_DIFF_GNSS when the IMU is lost but GNSS is good. ACTIVE_DIFF_WHEEL when GNSS is gone too. OPEN_DIFF, equal torque on all four wheels, when steering or more than one wheel speed is lost.

Moving between levels is deliberately slow. A downgrade waits 50 ms, so one bad sample does nothing. An upgrade waits a full second and only happens near straight, never mid-corner. Any downgrade then holds until the car stops, so the driver never gets a car that changes behaviour lap to lap, and every switch crossfades the torque over 0.25 s.

Draou's electronic differential gives two in-wheel motors speed setpoints from Ackermann geometry, $\Delta\omega = \frac{d_w \tan\delta}{L_w}\,\omega_v$, and assumes low speed with no tyre slip. Our car is torque-controlled and corners past 1.5 g, so I kept the kinematics and changed their role.

The wheel-speed targets become ceilings, not setpoints. Torque comes from the driver's request split by grip, and a wheel only loses torque if it spins past its ceiling, which is why no yaw feedback is needed. All four targets come from one pair, the speed $v$ and a reference yaw rate $r_{\text{ref}}$ at the rear-axle centre, so they always agree with each other. The rear equations are exact whatever the car is doing laterally, because the rear wheels aren't steered.

The reference yaw rate adds what the paper leaves out: an understeer gradient $K$, and a limit at what the tyres can actually hold. $r_{\text{ref}} = \operatorname{clamp}\left(\frac{v\tan\delta_{\text{eff}}}{L + K v^2},\ \pm\frac{k_\mu \mu g}{\max(|v|, 1)}\right)$, where $\delta_{\text{eff}}$ is a load-weighted average of the two front wheels. The car runs anti-Ackermann, and under load transfer the loaded outer tyre does most of the work, so it gets most of the say.

All four wheels are driven, so every wheel turns slightly faster than the ground and none of them is a free-rolling speed reference. Speed comes from a small predict-correct filter instead. It predicts from the motor torques and a drag and rolling-resistance model, then corrects with GNSS and a weighted least-squares fit of the four slip-corrected wheel speeds.

The GNSS correction uses the receiver's raw velocity, not the INS solution, because that solution leans on the IMU this module exists to do without. Without GNSS, traction-control logic bounds the estimate: while accelerating the car can't be faster than its slowest wheel, and while braking it can't be slower than its fastest.

A loaded tyre is about 3% smaller than its nominal radius, which is as big as the whole slip budget. So each wheel's rolling radius is learned from GNSS whenever the car is coasting in a straight line.

Torque is split in proportion to each wheel's load, which keeps every tyre at the same fraction of its grip. That puts more torque on the outer wheels and makes a turn-in yaw moment, and with no IMU nothing supervises it, so the moment is capped: 300 N·m with GNSS, and zero without, where left and right get equal torque.

A PI controller per wheel cuts torque only from a wheel that crosses its ceiling, and gives it back as grip returns. The cut isn't moved to the other wheels, trading some acceleration for predictability. A wheel with no speed feedback can't be slip-limited at all, so it and its axle partner go to zero, and losing one corner can't turn into an uncommanded yaw moment.

Lateral tyre modelling isn't attempted. It needs the body slip angle, which can't be observed without an IMU: a single-antenna GNSS gives the direction the car is travelling, not the direction it is pointing. Everything above is built from quantities that survive the IMU going away.

The main controller does want lateral state, so I also brought up the car's SBG Ellipse-D GNSS/INS and verified its CAN output end to end, confirming the same messages reach telemetry logging and the downstream vehicle-dynamics control inputs. A sensor that logs but does not reach the controller is the failure this was checking for. It is also where this module's GNSS velocity comes from.

The research and design are complete, and the proof of concept passes 23 of 23 host tests in GoogleTest. The kinematics match rigid-body rotation to $10^{-4}$ m/s. A wheel spinning 25% fast is cut to under 10% of its torque while the others are untouched. The handover from the main controller never steps more than 0.34 N·m in a cycle. With no GNSS and a slip model 50% wrong, the speed estimate stays within 4.3%.

Those tests run on synthetic kinematic data on the host, not on a vehicle model or the car's microcontroller. Next is my team lead's review, then the firmware implementation, closed-loop simulation and calibration: the steering lookup table, the understeer gradient, $\mu$ and the slip-limiter gains.