Quadrotor Flight Controller

Architecture

Control on rotor angular velocity.

Rather than commanding motor throttle directly, the controller resolves everything down to a per-rotor angular velocity setpoint, then closes a speed loop on real RPM fed back over bidirectional DShot.

position → velocity → acceleration → tilt
tilt → body rate → angular acceleration → torque
(thrust, torque) → RotorAllocator → per-rotor ω setpoint
ω setpoint → RotorSpeedLoop (closed on RPM) → DShot

Why rotor angular velocity, and not throttle

A conventional flight controller ends its cascade at a throttle value per motor — a duty cycle handed to an ESC in the hope that it produces the intended thrust. That hope is the problem. Thrust from a propeller goes roughly as the square of its angular velocity, and the mapping from throttle to angular velocity is not fixed: it sags as the battery drains, shifts with air density, and changes as the propeller loads up in fast descent. A controller tuned at full charge is a differently tuned controller at half charge.

Commanding angular velocity instead removes that uncertainty. The allocator converts the desired thrust and three-axis torque into four rotor speed setpoints using the airframe's geometry, and each rotor then runs its own closed loop against the RPM the ESC reports back over bidirectional DShot. Battery sag becomes a disturbance the inner loop rejects rather than a gain error the outer loops never learn about.

The practical payoff is that every quantity above the allocator stays in physical units. The attitude loop asks for a torque in newton-metres, the position loop asks for a thrust in newtons, and the allocator is the single place where physics turns into actuator commands. When yaw authority was wrong, that separation is what made the fault findable: the torque request was correct and the mapping beneath it was not.

Conventions live in one file

A single header defines the body frame — +X right, +Y forward, +Z up — declares every rotation right-handed, and derives the IMU transform from that. Anything downstream that needs a sign asks the header rather than deciding for itself.

The mixer is derived, not written down

The allocation matrix is built at run time from rotor positions and spin directions by summing r × F, so a sign error in the mixer would have to be a geometry error first.

Drag torque identified in flight

The propeller drag-torque coefficient was never characterised for this airframe, so the controller identifies it live using normalised LMS, gated behind a commissioning flag plus checks on RPM freshness, excitation, allocator saturation, and attitude. It is hard-clamped to a physical band, so a bad estimate degrades to slightly-off yaw authority rather than a sign flip.

Debugging

One wrong sign, five different symptoms.

Roll and pitch flew well. Yaw and horizontal position did not. The cause was a single variable holding a counter-clockwise-positive rate while its name said clockwise — the IMU is mounted upside down, and a gyro sign correction had quietly inverted it.

Four separate consumers each assumed clockwise and each broke differently: the yaw mixer turned into positive feedback and saturated, the magnetic-heading function returned the negative of the right-handed yaw it advertised and fed that to the state estimator, the two body/navigation frame transforms were written as each other's transpose — invisible at heading zero, fully cross-coupled at 90° — and the optical-flow lever-arm term injected fake translation that reinforced the error during rotation.

A fifth problem was found last and mattered most for the symptom: on the transmitter, the yaw command was a compile-time constant zero, read from no pin at all. The receiver was handed 0.0 on every packet. That alone would have prevented yaw control even with the other four fixed.

An earlier diagnosis had correctly identified gating, signal-quality, and gain problems and fixed them. It never found the sign layer — and no gain change could have.

A stick deflection is a velocity request, not a tilt

The sticks never command an angle. Roll and pitch map to a body-frame velocity setpoint of up to roughly ±0.5 m/s at full deflection, and the velocity loop decides what tilt is needed to produce it. Tilt is the means, not the command — which is why the velocity traces are smooth while the angle traces underneath them are busy. The attitude loop is continuously chasing whatever tilt the current velocity error demands.

Because the body frame is +X right, +Y forward, +Z up, the two channels split cleanly: the roll stick rolls the aircraft about its forward axis and moves it along ±X, right and left; the pitch stick pitches it about its right-hand axis and moves it along ±Y, forward and back.

The circle is hand-flown, not planned

There is no circular trajectory anywhere in the firmware. The loop in the ground track is what comes out of driving the two sticks in quadrature by hand — roll right while easing pitch forward, then pitch back as the roll comes off. A circle fitted to the flown path lands at 0.542 m radius with 0.076 m rms residual, which measures how steady the pilot was rather than how well a controller tracked anything.

The panel on the right is the part that says the vehicle works. Every stick step produces a body-velocity response of the right sign and roughly the right magnitude, on both axes simultaneously, and the two axes stay decoupled while the aircraft is rotating through the turn.

Altitude converges on what the rangefinder measures

The first four seconds are a real hold command: 0.5 m, commanded from the ground, and the aircraft climbs onto it and settles. After that the throttle stick is in the pilot's hand and the dashed commanded trace vanishes underneath the solid measured one — with throttle active the setpoint is continuously re-anchored to the estimated height, so letting go of the stick leaves the aircraft holding whatever altitude it had reached. Commanded and measured differ by 0.085 m rms across the whole run, and nearly all of that is the opening climb.