Now we have access to the input from the RC receiver, and the current orientation from the gyro and accelerometer, the next big hurdle is to actually process these into motor outputs.

Now the algorithm for the job is called a proportional integral derivative controller, or a PID loop. There is plenty of stuff out there on this, but basically it takes the desired value (setpoint), and the actual value (input), calculates the error value and then puts this through 3 different terms to calculate an output.

Here’s a diagram explaining this:

Within the PID controller, the calculated error value has three things done to it, which are then added up at the end.

**Proportional**– this just multiplies the error by a constant (kp)**Integral**– this finds the integral of the error, and multiplies it by a constant (ki)**Derivative**– this finds the change over time of the error, and multiplies it by a constant (kd)

The end value can be written as the following:

This algorithm is used in industrial control everywhere, and is a robust and reliable way of altering an output based on an input.

So I need to integrate this into my program. That isn’t as hard as it may look, but I am most likely going to need to actually build the quadcopter first, so that I can properly test it and tune the constants: it would be a bit of a shot in the dark to do everything without testing, and then test it at the end.

Meanwhile, I can start to think about what is actually going to control what.

This is what I’m planning on doing:

As you can see, there are two main PID controllers for each axis: one calculates the desired rate of rotation based on the difference between the actual angle and the desired angle, and the next takes that desired rotation rate along with the actual rotation rate and turns it into an output that can be sent to the motors.

Now for this I need access to a current rotational velocity in each of the three axes. This is, of course, what the gyro measures directly, so I could just use it, but the implementation would be complicated alongside the RTIMULib library that I’ve already used, and so I have opted just to work with the known current angle.

I just need to derive the angle to find the change in angle (i.e. rotational velocity). That means a simple function that takes the current angle and the last angle, and the time since the last angle was measured, and finds the rotational rate from it.

Here it is:

void getRotVel() { unsigned long now = micros(); double timeChange = now - lastRotVelSample; double timeChangeSeconds = timeChange*1e-6; currentRotVel.x = (currentPosition.x - lastPosition.x) / timeChangeSeconds; currentRotVel.y = (currentPosition.y - lastPosition.y) / timeChangeSeconds; currentRotVel.z = (currentPosition.z - lastPosition.z) / timeChangeSeconds; lastPosition.x = currentPosition.x; lastPosition.y = currentPosition.y; lastPosition.z = currentPosition.z; lastRotVelSample = now; }

This relies upon having got a new sample for current position and the right moment, and so I’ve opted to call it at the same time as the main function that finds the current position. This should mean it is only activated when there is a brand new position to work from.

Here’s my `refreshPosition()` function that updates both `currentPosition` and `currentRotVel`:

void refreshPosition(int frequency) { unsigned long now = millis(); if (now - lastPositionSample >= frequency) { getPosition(); getRotVel(); } lastPositionSample = now; }

It calls both every `frequency` milliseconds, which is supplied as a parameter to the function.

This may not be ideal, and I can’t help but think that it is still better to read the data directly from the gyros. However, I’m going to press on with building for now, as there’s not much I can do anyway without a working quadcopter.