While waiting for the new components to arrive I’ve worked a bit on the stability pids.

The stability pids are responsible of controlling the orientation of the quad. So if I tell the quad to lean forward 30 degrees, the stab pids have to figure out how to control the motors lean forward with exactly 30 degrees. Same for leaning left/right.

Silkopter uses quaternions to represent rotations so it’s not as simple as just subtracting the pitch and roll from my target pitch and roll and use the delta. First of all I want to avoid as much as possible using euler angles because of the dreaded gimbal lock. Second of all there’s the wrapping problem of euler angles – going from 0 to 360 or from -180 to 180 which I always get wrong.

So quaternion it is.

My approach is this:

First compute the target quaternion from the input. I treat the input as euler angles and build a quaternion fro them. This presents no issue as the input is limited by design to +- 90 degrees. Let’s call this target_q

Get the current quad quaternion. This represents the orientation of the quad relative to the world frame of reference (the Earth). Let’s call this uav_q

Now compute their difference – or how much apart is target_q from uav_q

diff_q = inverse(uav_q) * target_q

where inverse(q) = quat(-q.x, -q.y, -q.z, q.w)

This step is the key – by doing the math with quaternions instead of euler angles I avoid the gimbal lock. Also, the quat difference give me the shortest path between the 2 rotations

I convert this q_diff to euler and this is how much I have to rotate in each axis to get to my desired orientation. These represent the stability pid inputs – how much the uav deviates from the desired pose – and the stab pids targets is 0.

I process the pids and the error is send as the target of the rate pids.

The rate pid outputs is sent to the motor mixer which in turn decides – based on the geometry of the uav – what motors need more or less thrust.

Repeat this 200 times per second.

Here’s a video of the quad simulation trying to match a pose. I control the pose (the 3 axis system to the right) with a ps3 controller and the quad is matching the pose using the algorithm above. It’s rock solid even if I move the joysticks randomly and it always follows the minimal path to the target rotation. Thank you, quaternions!