Build your own Quadcopter Flight Controller - autopilot

Posted at: FRIday - 02/12/2016 09:36 - post name: SuperG
Build your own Quadcopter Flight Controller

Build your own Quadcopter Flight Controller

How to program your own AutoPilot/Flight Controller from scratch using an arduino/ArduPilot/RaspberryPi. The tutorial will walk you through everything from reading the RC radio and controlling the motors through to how the PIDs are stacked on top of each other to get stable flight.

 Information on how to do this is quite hard to find on the Internet, so I hope this article will make the world of flight controllers more accessible and encourage more people to contribute to ArduCopter.

The first shortcut is your choice of hardware. I chose to build my own from scratch at a stage when I knew nothing of RC or how to fly – this was a mistake. So do yourself a favour and buy the ArduPilot 2.5 control board, wire up your copter, learn RC, and how to fly, and then come back here. The board is essentially just an Arduino with some sensors connected which we will program in this article with our own software – by using it you have everything connected you’ll need to get flying – you’ll also be able to play with the excellent ArduCopter software.


The ArduPilot project is sponsored by 3D Robotics – this means that they build the hardware and sell it for a small profit, and then feed some of this profit back to the community. The hardware and software is entirely open source and anyone is free to copy it. You can buy the original from them direct, or identical copies from Hobbyking (named HKPilot) and RCTimer (named ArduFlyer).

In this article, I am going to assume you have the ArduPilot hardware which is essentially an Arduino with attached sensors. If you choose to ignore my advice and build your own hardware, or use the arduino board, then you’ll need to replace the lower level code (the HAL library). I’m also going to assume you have a quadcopter in X configuration – although not a lot of work is required (just different motor mixing) to switch between +/X and octa/hexacopters, they won’t be given it any substantial attention in the article. Ideally, you’ve already flown your quad with the ArduCopter code loaded and hence you should have your motors connected as follows and spinning in the direction shown.

Propellor Configuration

I’m also going to assume you have some experience with the arduino – or atleast with C/C++. The arduino libraries are not particularly brilliant or well suited, so we’ll be using some of the ArduPilot libraries which are superior. However, we’ll be keeping their use to a minimum in favour of the DIY approach (which is why you’re here after all). The first and main library that we’re going to use is the ArduPilot Hardware Abstraction Layer (HAL) library. This library tries to hide some of the low level details about how you read and write to pins and some other things – the advantage is that the software can then be ported to new hardware by only changing the hardware abstraction layer. In the case of ArduPilot, there are two hardware platforms, APM and PX4, each of which have their own HAL library which allows the ArduPilot code to remain the same across both. If you later decide to run your code on the Raspberry Pi, you’ll only need to change the HAL.

The HAL library is made up from several components:

  • RCInput – for reading the RC Radio.
  • RCOutput – for controlling the motors and other outputs.
  • Scheduler – for running particular tasks at regular time intervals.
  • Console – essentially provides access to the serial port.
  • I2C, SPI – bus drivers (small circuit board networks for connecting to sensors)
  • GPIO – Generial Purpose Input/Output – allows raw access to the arduino pins, but in our case, mainly the LEDs

WHAT TO DOWNLOAD: You’ll need to download the ArduPilot version of the Arduino IDE. Also grab the libraries which should be placed in your sketches folder. Also make sure you select your board type from the Arduino menu like so:

ArduPilot Arduino IDE Setup

Our flight controller is going to have to read in the radio inputs (pilot commands), measure our current attitude (yaw/pitch/roll), change the motor speeds to orientate the quad in the desired way. So let’s start out by reading the radio.


Reading the Radio Inputs

RC Radios have several outputs, one for each channel/stick/switch/knob. Each radio output transmits a pulse at 50Hz with the width of the pulse determining where the stick is on the RC transmitter. Typically, the pulse is between 1000us and 2000us long with a 18000us to 19000us pause before the next – so a throttle of 0 would produce a pulse of 1000us and full throttle would be 2000us. Sadly, most radios are not this precise so we normally have to measure the min/max pulse widths for each stick (which we’ll do in a minute).

Radio Signals

The ArduPilot HAL library does the dirty work of measuring these pulse widths for us. If you were coding this yourself, you’d have to use pin interrupts and the timer to measure them – arduino’s AnalogRead isn’t suitable because it holds (blocks) the processor whilst it is measuring which stops us from doing anything else. It’s not hard to implement an interrupt measurer, it can be programmed in an hour or so but as it’s fairly mundane we won’t.

Here’s some sample code for measuring the channel ‘values’ using the APM HAL library. The channel values are just a measure in microseconds of the pulse width.

#include <AP_Common.h>#include <AP_Math.h>#include <AP_Param.h>#include <AP_Progmem.h>#include <AP_ADC.h>#include <AP_InertialSensor.h>#include <AP_HAL.h>#include <AP_HAL_AVR.h>const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;  // Hardware abstraction layervoid setup() {}void loop() {  uint16_t channels[8];  // array for raw channel values    // Read RC channels and store in channels array  hal.rcin->read(channels, 8);    // Copy from channels array to something human readable - array entry 0 = input 1, etc.  uint16_t rcthr, rcyaw, rcpit, rcroll;   // Variables to store rc input  rcthr = channels[2];    rcyaw = channels[3];  rcpit = channels[1];  rcroll = channels[0];  hal.console->printf_P(            PSTR("individual read THR %d YAW %d PIT %d ROLL %d\r\n"),            rcthr, rcyaw, rcpit, rcroll);  hal.scheduler->delay(50);  //Wait 50ms }AP_HAL_MAIN();    // special macro that replace's one of Arduino's to setup the code (e.g. ensure loop() is called in a loop).

Create a new sketch and upload the code to the ardupilot hardware. Use the serial monitor and write down the minimum and maximum values for each channel (whilst moving the sticks to their extremes).

Now let’s scale the stick values so that they represent something meaningful. We’re going to use a function called map, which takes a number between one range and places it in another – e.g., if we had a value of 50, which was between 0-100, and we wanted to scale it to be between 0 and 500, the map function would return 250.

The map function (copied from Arduino library) should be pasted into your code after the #include and defines:

long map(long x, long in_min, long in_max, long out_min, long out_max){  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;}

It is used as:


It makes sense for the throttle to remain untouched, no doubt you’ve calibrated your ESCs with the existing throttle values (if you followed my advice about flying first) so let’s not play with it. Pitch and roll should be scaled to be between -45 degrees and +45 degrees, whilst yaw might scale to +-150 degrees.

My code in loop() now looks like follows after I’ve substituted in the map function with the min/max values for each stick. We’ll also change the variable types to long to support negative numbers.

long rcthr, rcyaw, rcpit, rcroll;   // Variables to store rc inputrcthr = channels[2];rcyaw = map(channels[3], 1068, 1915, -150, 150);rcpit = map(channels[1], 1077, 1915, -45, 45);rcroll = map(channels[0], 1090, 1913, -45, 45);

Pitch should be negative when the stick is forward and roll/yaw should be negative when the stick is left. If this isn’t the case then reverse them until they are correct.

You should now print the new values out and monitor them on the serial monitor. Ideally, they should be zero or very close when the sticks (except thr) are centred. Play with the min/max values until they are. There will be some jitter (waving about the true value) because the sticks on your transmitter are analog but it should be of the order +-1 or +-2 degrees. Once you’ve got your quad flying, you might consider returning here to introduce an averaging filter.

Ensure that pitch forward, roll left, and yaw left are negative numbers – if they’re not, put a minus sign before the map. Also ensure that the throttle increases in value as you raise the throttle.


Controlling the motors

Motors are controlled through the Electronic Speed Controllers (ESCs). They work on pulse widths between approximately 1000us and 2000us like the RC radio receiver – sending a pulse of 1000us typically means off, and a pulse of 2000us means fully on. The ESCs expect to receive the pulse at 50Hz normally, but most off the shelf ESCs average the last 5-10 values and then send the average to the motors. Whilst this can work on a quad, it behaves much better if we minimise the effect of this averaging filter to give near instantaneous response. Hence, the APM HAL library sends the pulse at 490Hz, meaning that the 5-10 pulses which are averaged occur very quickly largely negating the filter’s effect.

In setup(), let’s enable the outputs:

hal.rcout->set_freq(0xF, 490);hal.rcout->enable_mask(0xFF);

After your includes, let’s define a mapping of output number to motor name – this mapping is the same as the ArduCopter uses but the numbering starts from zero rather than one.

#define MOTOR_FL   2    // Front left    #define MOTOR_FR   0    // Front right#define MOTOR_BL   1    // back left#define MOTOR_BR   3    // back right

In your loop, after reading the radio inputs, let’s send the radio throttle straight to one of the motors:

hal.rcout->write(MOTOR_FR, rcthr);

You can now program your quad and try it, WITHOUT propellors. Slowly raise the throttle and the front right motor should spin up. By repeating the last line for the remaining three motors, all your motors would spin up although the quad will just crash if you have propellors on because we have to do stablisation – slight differences between the motors, props, ESCs, etc mean that slightly unequal force is applied at each motor so it’ll never remain level.

** Comment out the write line before proceeding for safety reasons **


Determining Orientation

Next, we need to determine which orientation, or attitude as it’s known, the quad copter is in. We can then use this, along with the pilot’s commands to vary the motor speed. There are two sensors used for determining orientation, accelerometers and gyroscopes. Accelerometers measure acceleration in each direction (gravity is an acceleration force so it gives us a direction to ground) and gyroscopes measure angular velocity (e.g. rotation speed around each axis); however, accelerometers are very sensitive to vibrations and aren’t particularly quick whilst gyroscopes are quick and vibration resistant but tend do drift (e.g. show constant rotation of 1/2 degrees/sec when stationary). So, we use a sensor fusion algorithm to fuse the two together and get the best of both worlds – the scope of such an algorithm is outside the scope of this article, typically a Kalman filter is used, or in the case of ArduPilot, a Direct Cosine Matrix (DCM). I’ve provided the DCM link for interest if you have a maths background – for the rest of us we don’t need to know the details.

Thankfully, the MPU6050 sensor chip containing the accelerometer and gyroscopes has a built in Digital Motion Processing unit (aka sensor fusion) that we can use. It will fuse the values together and present us with the result in quaternions. quaternions are a different way of representing orientation (as opposed to euler angles: yaw pitch roll) that has some advantages – if you’ve programmed 3d graphics you’ll already be familiar with them. To make things easier, we tend to convert quaternions into Euler angles and work with them instead.

Here’s the code to use the MPU6050 sensor with sensor fusion.

In setup():

// Disable barometer to stop it corrupting bushal.gpio->pinMode(40, GPIO_OUTPUT);hal.gpio->write(40, 1);// Initialise MPU6050 sensorins.init(AP_InertialSensor::COLD_START,    AP_InertialSensor::RATE_100HZ,  NULL);// Initialise MPU6050's internal sensor fusion (aka DigitalMotionProcessing)hal.scheduler->suspend_timer_procs();  // stop bus collisionsins.dmp_init();                        ins.push_gyro_offsets_to_dmp();hal.scheduler->resume_timer_procs();

Now let’s read the sensor. At the beginning of loop() add this line, which will force a wait until there is new sensor data. There’s no point in changing motor speeds unless we know something new.

while (ins.num_samples_available() == 0);

** Remove the 50ms delay from the loop, no longer needed **Now let’s get the yaw/pitch/roll from the sensor and convert them from radians to degrees:

ins.update();ins.quaternion.to_euler(&roll, &pitch, &yaw);roll = ToDeg(roll) ;pitch = ToDeg(pitch) ;yaw = ToDeg(yaw) ;

Now let’s print it out to the serial console:

hal.console->printf_P(   PSTR("P:%4.1f  R:%4.1f Y:%4.1f\n"),   pitch,   roll,   yaw);

You need to put a rate throttle on this print statement, e.g. ensure it’s only printed once every 20 times around the loop (hint: use a counter). Otherwise the serial line will get flooded.

Move your copter around and ensure the right values are changing!


Acrobatic / Rate mode control

Acrobatic/rate mode is where the sticks on your transmitter tell the quad to rotate at a particular rate (e.g. 50deg/sec), and when you return the sticks to center the quad stops rotating. This is as opposed to stablise mode where returning the sticks to center will level the quadcopter. It’s a mode that takes practice to learn how to fly in but we are required to implement this mode first because the stablise controllers operate on top of the rate controllers.

So, our aim is for each of the pilot’s sticks to dictate a rate of rotation and for the quad to try to achieve that rate of rotation. So if the pilot is saying rotate 50deg/sec forward on the pitch axis, and we’re currently not rotating, then we need to speed up the rear motors and slow down the front ones. The question is, by how much do we speed them up/slow them down? To decide this, you need to understand Proportional Integral Derivative (PID) controllers which we are going to make extensive use of. Whilst somewhat of a dark art, the principles are fairly straight forward. Let’s assume our quadcopter is not rotating on the pitch axis at the moment, so actual = 0, and let’s further assume the pilot wants the quad to rotate at 15deg/sec, so desired = 15. Now we can say that the error between what we want, and what we’ve got is:

error = desired - actual = 15 - 0 = 15

Now given our error, we multiply it by a constant, Kp, to produce the number which we will use to slow down or speed up the motors. So, we can say the motors change as follows:

frontMotors = throttle - error*KprearMotors = throttle + error*Kp

As the motors speed up the quad will start to rotate, and the error will decrease, causing the difference between the back/rear motor speeds to decrease. This is desirable, as having a difference in motor speeds will accelerate the quad, and having no difference will cause it to hold level (in a perfect world). Believe it or not, this is all we really need for rate mode, to apply this principle to each of the axes (yaw, pitch, roll) and using the gyros to tell us what rate we’re rotating at (actual). The question you’re probably asking is, what should I set Kp to? Well, that’s a matter for experimentation – I’ve set some values that work well with my 450mm quadcopter – stick with these until you’ve got this coded.

Rate only PID

If you’ve been studying PIDs before, you’ll know there are actually two other parts to a PID: integral and derivative. Integral (Ki is the tuning parameter) essentially compensates for a constant error, sometimes the Kp term might not provide enough response to get all the way if the quad is unbalanced, or there’s some wind. Derivative we’re going to ignore for now.

Let’s get started, define the following PID array and constants globally:

PID pids[6];#define PID_PITCH_RATE 0#define PID_ROLL_RATE 1#define PID_PITCH_STAB 2#define PID_ROLL_STAB 3#define PID_YAW_RATE 4#define PID_YAW_STAB 5

Now initialise the PIDs with sensible values (you might need to come back and adjust these later) in the setup() function.

pids[PID_PITCH_RATE].kP(0.7);//  pids[PID_PITCH_RATE].kI(1);pids[PID_PITCH_RATE].imax(50);pids[PID_ROLL_RATE].kP(0.7);//  pids[PID_ROLL_RATE].kI(1);pids[PID_ROLL_RATE].imax(50);pids[PID_YAW_RATE].kP(2.5);//  pids[PID_YAW_RATE].kI(1);pids[PID_YAW_RATE].imax(50);pids[PID_PITCH_STAB].kP(4.5);pids[PID_ROLL_STAB].kP(4.5);pids[PID_YAW_STAB].kP(10);

Leave the I-terms uncommented for now until we can get it flying OK, as they may make it difficult to identify problems in the code.

Ask the gyros for rotational velocity data for each axis.

Vector3f gyro = ins.get_gyro();

Gyro data is in radians/sec, gyro.x = roll, gyro.y = pitch, gyro.z = yaw. So let’s convert these to degrees and store them:

float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);

Next, we’re going to perform the ACRO stablisation. We’re only going to do this if the throttle is above the minimum point (approx 100pts above, mine is at 1170, where minimum is 1070) otherwise the propellors will spin when the throttle is zero and the quad is not-level.

if(rcthr > 1170) {   // *** MINIMUM THROTTLE TO DO CORRECTIONS MAKE THIS 20pts ABOVE YOUR MIN THR STICK ***/ long pitch_output =   pids[PID_PITCH_RATE].get_pid(gyroPitch - rcpit, 1);   long roll_output =   pids[PID_ROLL_RATE].get_pid(gyroRoll - rcroll, 1);   long yaw_output =   pids[PID_YAW_RATE].get_pid(gyroYaw - rcyaw, 1);   hal.rcout->write(MOTOR_FL, rcthr - roll_output - pitch_output); hal.rcout->write(MOTOR_BL, rcthr - roll_output + pitch_output); hal.rcout->write(MOTOR_FR, rcthr + roll_output - pitch_output); hal.rcout->write(MOTOR_BR, rcthr + roll_output + pitch_output);} else {  // MOTORS OFF hal.rcout->write(MOTOR_FL, 1000); hal.rcout->write(MOTOR_BL, 1000); hal.rcout->write(MOTOR_FR, 1000); hal.rcout->write(MOTOR_BR, 1000);  for(int i=0; i<6; i++) // reset PID integrals whilst on the ground pids[i].reset_I();}

Now raise the throttle about 20% and rotate your quad forward/back, and left/right in your hands and make sure the correct propellors speed up/slow down – if the quad is tilted forward, then the forward propellors should speed up and the rears slow down. If not, change the signs around on the motor outputs (e.g. if the pitch is wrong, swap the signs before the pitch, likewise with the roll).

You can test this fully if you choose, and tune your rate PIDs by fixing the quad on one axis with a piece of string and testing each of the axes in turn. It’s a useful experience to get a better understanding of how the rate PID is working but not strictly necessary. Here’s an example of mine with rate only PIDs – I command it to rotate at 50deg/second:

Video: Rate PIDs only with quad fixed on one axis:

Now we need to add yaw support in. As you know, two motors spin in different directions to give us yaw control. So we need to speed up / slow down the two pairs to keep our yaw constant.

hal.rcout->write(MOTOR_FL, rcthr - roll_output - pitch_output - yaw_output);hal.rcout->write(MOTOR_BL, rcthr - roll_output + pitch_output + yaw_output);hal.rcout->write(MOTOR_FR, rcthr + roll_output - pitch_output + yaw_output);hal.rcout->write(MOTOR_BR, rcthr + roll_output + pitch_output - yaw_output);

This is a bit more difficult to test. You need to raise the throttle so that it hovers a little. If the yaw signs are wrong then the quad will spin.

You should now be able to get your quad off the ground for a few seconds. If you’re comfortable flying acro mode you will even be able to fly it – although bear in mind that this is pure acro mode, not that on ArduCopter where it performs auto-levelling for you.

If your quad flies floppy, or oscillates, then you need to adjust your rate Kp. Up if floppy or down if oscillating. If it’s just going nuts, then you have the signs around the wrong way – try printing out PID outputs, and motor commands to debug whilst moving the quad around (without the battery connected).


Stablilised Control

Stabilised mode works similar to rate mode, except our code sits on top of the rate code as follows:

Cascaded PID structure

Now, the pilot’s sticks dictate the angle that the quad should hold, not the rotational rate. So we can say, if the pilot’s sticks are centred, and the quad is currently pitched at 20 degrees, then:

error = desiredAngle - actualAngle = 0 - 20 = -20

Now in this case, we’re going to multiply error by a Kp such that the output is the angular rate to achieve. You’ll notice from earlier, Kp for the stab controllers is set at 4.5. So, if we have an error of -20, then the output from the pid is -20*4.5 = -90 (the negative just indicates direction). This means the quad should try to achieve a rate of -90degrees per second to return it to level – we then just feed this into the rate controllers from earlier. As the quad starts to level, the error will decrease, the outputted target rate will decrease and so the quadcopter will initially return to level quickly and then slow down as it reaches level – this is what we want!

// our new stab pidsfloat pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250); float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid((float)rcyaw - yaw, 1), -360, 360);// rate pids from earlierlong pitch_output =  (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);  long roll_output =  (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);  long yaw_output =  (long) constrain(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);

Now your quad should be able to hover, although it might be wobbly / oscillating. So, if it’s not flying too great – now is the time to tune those PIDs, concentrating mainly on the rate ones (Kp in particular) – the stab ones _should_ be okay. Also turn on the rate I terms, and set them to ~1.0 for pitch/roll and nothing for yaw.

Notice that yaw isn’t behaving as expected, the yaw is locked to your yaw stick – so when your yaw stick goes left 45degrees the quad rotates 45 degrees, when you return your stick to centre, the quad returns its yaw. This is how we’ve coded it at present, we could remove the yaw stablise controller and just let the yaw stick control yaw rate – but whilst it will work the yaw may drift and won’t return to normal if a gust of wind catches the quad. So, when the pilot uses the yaw stick we feed this directly into the rate controller, when he lets go, we use the stab controller to lock the yaw where he left it.

As the yaw value goes from -180 to +180, we need a macro that will perform a wrap around when the yaw reaches -181, or +181. So define this near the top of your code:

#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))

If you examine it carefully, if x is < -180, it adds +360, if it’s > 180 then we add -360, otherwise we leave it alone.

Define this global or static variable:

float yaw_target = 0;

Now in the main loop, we need to feed the yaw stick to the rate controller if the pilot is using it, otherwise we use the stab controller to lock the yaw.

float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);if(abs(rcyaw) > 5) {  // if pilot commanding yaw yaw_stab_output = rcyaw;  // feed to rate controller (overwriting stab controller output) yaw_target = yaw;         // update yaw target}

You’ll also what to set your yaw target to be the direction that the quad is on the ground / throttle off – you can do this in the else part of the if.

That’s it, now yaw should behave normally. Although if you pay attention, you might notice that the yaw drifts slowly over several tens of seconds. This may not bother you, the reason it is happening is because although your yaw stick is centred, the radio jitter means the quad doesn’t always receive 0 – it hovers around that value causing the yaw to change. Additionally, the MPU6050’s yaw sensor drifts over time (1-2deg/sec) – you’ll need to use the compass to compensate for this drift (if you really care enough to fix it – most people don’t notice).


Final Product – video and full code

Congratulations – you’ve built your first flight controller for a multi-copter! You’ll notice it’s a lot more aggresive than the standard ArduCopter code – this is because ArduCopter has a lot of processing on pilot’s inputs to make it easier to fly. Raise your throttle to ~80% and your quad will _rocket_ into the sky far faster than you could achieve on ArduCopter. Be warned – don’t raise your throttle too close to 100% as that won’t leave any room for the controller to change the motor speeds to get it level and it’ll flip (you can implement an automatic throttle lowerer fairly easily).

Download the Completed Code – this should run straight away if your regular arducopter flies (after you’ve adjusted the radio max/mins) – you might also need to adjust the PIDs to get it stable.

Video of the final product in action:

Other ideas: Safety

  • add a mechanism to arm/disarm the quadcopter.
  • Ensure you’ve thought about what happens when there are bugs in your code – you don’t want the throttle getting stuck on full! Investigate the watchdog timer.


Optional: Raspberry Pi

Your best bet here is to use the ArduPilot hardware as a sensor/control expansion board by connecting it to the Pi over the USB. You need to be very careful because the Pi runs Linux and as a result of this it is very difficult to do finely grained timing like controlling ESCs/reading radios. I learnt a hard lesson after choosing to do the low level control loop (PIDs) on the Pi – trying to be clever I decided to put a log write in the middle of the loop for debugging – the quad initially flied fine but then Linux decided to take 2seconds to write one log entry and the quad almost crashed into my car! Therefore, your best bet is to offload the time critical stuff to the ardupilot hardware and then run highlevel control on the Pi (e.g. navigation). You’re then free to use a language like Python because millisecond precision isn’t needed. The example I will give here is exactly that scenario.

Raspberry Pi Quad Diagram

Connect the ArduPilot to your Raspberry Pi over USB and modify the code in this article to accept THR, YAW, PIT, ROL over the serial port (sample provided below). You can then set your raspberry Pi up as a wifi access point and send your stick inputs over wireless from your phone (beware that Wifi has very short range ~30m).

Sample code

Android App: Download app – sends thr, yaw, pitch, roll from pilot out on UDP port 7000 to – you can change this in the app

Raspberry PiDownload server – On the Pi, we run a python script that listens for the control packets from the Android app, and then sends them to the ArduPilot. Here I’m just implementing a simple relay, but you could easily do something more complex like navigation, control over 3G, etc.

ArduPilotDownload code – accepts thr, yaw, pitch and roll over the serial port rather than over the RC radio. A simple checksum is used to discard bad packets.


Optional: Autonomous Flight

Autonomous flight should now be fairly straightforward to implement. Some tips:

GPS Navigation: The ArduPilot provides libraries for parsing GPS data into latitude and longitude, you’ll just need a PID to convert desired speed into pitch/roll, and another PID for converting distance to waypoint into desired speed. You can use the compass to work out direction to your waypoint, and then just translate that into the right amount of pitch and yaw.

Altitude Hold: You can sense altitude with the barometer that is build onto the ArduPilot board. You’ll need two PIDs, one to calculated throttle alterations from desired ascent/descent rate and a second to calculate desired ascent/descent from distance to desired altitude.


More article on​:

Control Method: Proportional Integral Derrivative Control

Control Method: Proportional Integral Derrivative Control
When our autopilot is running, it will sense the angular attitude of the plane and the desired attitude. The difference between the desired and the actual is called the error. We will then change our servo output angles, to change our control surface angles (ailerons and elevator)  to correct the error. The problem becomes, how do we relate our sensor input to our servo outputs?
The solution: A PID controller.

A PID controller uses three terms to calculate an output that should correct the error.

output = (kp*error)+(ki*errorSum)+(kd*dError);

The first term (the proportional term) looks at our current inputs (pitch & roll in our case) and compares it to the desired position aka setpoint (0 degrees for level flight). The calculated error (difference) is multiplied by a constant (KP) to contain the value within our output range (0 to 180 deg is servo range, but it is typically smaller once installed in an RC aircraft). Basically, the greater the error, the greater the necessary change.

The second term is the integral term. It sums all previous error (as captured by the proportional term) over time. If the error remains uncorrected for too long, the integral term grows larger thus increasing the magnitude of our output until the correction is made. Again, multiply it by you constant (KI) to contain the term within the output range *WARNING* It is possible that if you collect errors for too long the integral term will exceed your output capability. This is called INTEGRAL WINDUP. This took me a long time to fix. I was able to minimize the term by limiting the scope of time through which I viewed my error. In order to keep the first two terms from overcorrecting our error we must look to the third term.

The derivative term (dError) looks at the rate that your sensor inputs are approaching your setpoint in order to project when the error will be corrected. The derivative term can be used to slow down the output if it is correcting too fast and is likely to overshoot the setpoint. Multiply it by the constant KD to keep the output within the possible range.

The PID is a very powerful feedback controller (i.e. it uses past outputs and inputs to calculate the new output).  It will greatly benefit you to do some outside reading if you have not used them before. Actually, the wikipedia page does a very good job of explaining the concepts involved. If you know calculus, the PID can become very intuitive tool.
Once you understand how it works, please look at the structure section on the feedforward control loop.

Demo PID Controller

Demo PID Controller
Before I threw my arduino into an airplane, I decided it would be a good idea to build a platform to test the PID on. My PID controller used the following parts:
   Eflite Outrunner Motor
   35 Amp ESC
   11.1 V LiPo battery
   1/2" PVC
   An old saw horse for a stand

The controller looks at the angle the arm is hanging at using our accelerometer and gyroscope, and uses that data to determine how much current to throw the motor in order to make the arm level.

Because the ESC is used to plug your motor into a radio reciever bus designed for servos, it is relatively easy to control a brushless AC motor with an ESC on an Arduino thanks to the Servo.h + Servo.cpp in your libraries folder. I believe the servo protocol uses pulse position modulation (PPM) which can be achieved using a PWM pin on your arduino with servo.write() function.

It is important to be saftey conscious when running these motors, as they go extremely fast. *ALWAYS wear saftey glasses* *Leave the propeller OFF until you absolutely need it*

You will have to write a function to arm(); your ESC so that you can set its speed. Basically you need to write the ESC low, high, and then low again. This forum goes over how to control a brushless motor, and is where I got framework for my arm(); function and setSpeed (); function.

You could just use a servo.write(0-180) to set your speed on your esc, but you can also map it on a scale of 0-100. You can download the motor_functions at the bottom of the step. If it doesn't work please tell me!! I can simply add it into the instructions!!

Using these functions gives you full motor control. You will have to fidget with your own code for your particular ESC. By declaring our sensor functions independently from the loop we can now run the motor and sensors at the same time. This is where I hit my first major setback. Because the motor draws so much current from the battery, it was inducing currents in my I2C bus. My sensor would work fine when the motor was off, but when the motor was running the values were pretty much meaningless.
I took the following steps to clean up the noise:
   I added a ferrite ring to the esc (retrospectively, this probably just protects the ESC from noise- learned to use these after my Zagi crashed
   I mounted the sensors as far away as possible from the motor
   Twisted all my wires to try and minimize current induction

I found these websites to help deal with my problem: //this is a thread I started on DIY drones 

This seemed minimize sensor error, but still about 5 out of every 20 values were off by +-10 degrees. To clean this up I mediated the sensor values by:
   Building an aray to hold 20 values as they appeared chronologically
   Building an array that held the last 20 values sorted in (ascending or descending order; it doesn't matter because we are looking for the median)
   I used a bubble sort to help sort my values

I found this website to help explain how to sort array efficiently:
You will have to construct multiples of these sorting functions for multiple values (x, y, z; dx, dy, dz)

My bubble sort and median functions can be downloaded at the bottom of the step!

Now that your sensors values are congruent and you have control over your AC motor, we can begin actual PID control!
The wikipedia page does a good job explaining how your program should be structured using psuedocode. There are many, many ways to implement this. It depends on how long you want to use your feedback for you integral and derrivative terms.

you can find my PID_sample at the bottom of this step!!

Tuning the PID: There are many ways to tune a PID. You can by software, guess and check, I'd try using the Zieger Nichols method.
Any way, you start by only running your output with the proportional error. set KP so that the proportional term never exceeds output capacity. Basically, the system will oscillate, you want it to dampen around your setpoint (or at the very least not increase over time!). Once you have found an appropriate KP you can either set KD or KI. Different people say different things when it comes to which one to do first, but integral term will accelerate an undercorrection toward the setpoint while the derrivative term will slow down an overcorrection.

Sorry everyone I tried to embed the videos but for some reason it did not work. Here is a link:
After playing with the code for a while it should become apparent how adaptive this type of controller is to your particular system. Remember, try and feedforward as much as you know how to! Once you are comfortable go ahead and install the controller!!

Setting Up the Plane

Setting Up the Plane
For initial testing I used a glider, so there would be no possibility of motor interference. Once level flight is achieved I have a Zagi ready to go and a multiplex Twin Star for possible drones.

I added the system to my Swift AT (Aileron Trainer). I really like this glider. Doesn't take too much wind to fly and is capable of a variety of manuevers. Flying with a glider is great if you got the wind, you can fly for hours (no motor/ engine no need to come down and refuel).

Our new board set up will be very similar to our orignial PID; we will be running two PID loops back forth very quickly to control pitch and roll with the elevator and ailerons. This should allow ample testing time in good wind.

I am using Airtronics radio equipment with a 6 channel transmitter/receiver. I used the fifth channel on the radio (a landing gear toggle) to set the autopilot on & off from the transmitter. Connect to a servo wire from the receiver and connect it to a digital input to measure the the pulse length of the two positions (toggle on/toggle off). Use the pulseIn() function to do so. Once you have a value from the two states pick one to indicate off and one for on. When the arduino reads and ON signal it should send out a digital HIGH to flip a relays or trigger some transistors. I used relays because I did not want my signal passing through a transistor (a descision I would later regret).

You can see the schematics I drew for wiring up a circuit that would throw one servo from the radio to the arduino. There are two: one for transistors and one for a radio. Note the positon of PNP and NPN transistors. When the arduino output wire is low the servo signal defaults to the radio, when it is written high it switches to the board. Likewise, on the relay circuit, the normally closed terminal is connected to the radio, and the normally open to the arduino.

you can find my functions for for the autopilot_engage at the bottom of this step

Here is a video showing the arduino receiving radio input and throwing an led, later used to arm the autopilot:

Here is my code and my schematic!

Here is my code and my schematic!
The code starts us off by navigating with only proportional error. You will have to fill in your own KP KI KD after some trial and error.
Make sure your plane is balanced before you take off! Engage the autopilot before throwing your plane and check that the throws are oriented to the correct direction.

The fallback function (at the end of the code) is used to throw the control back to the reciever. make sure you include this function at your sensor errors, or anywhere else you could get caught in a loop and you may want control back from the autopilot!

Download the code below
Swift_AutoPilot Swift_AutoPilot


Article reviews
Total number of articles is: 0 in 0 rating
You click on a star to rate article

The older news


Robotics Course

​​LV1 (Session 1)   Introduction to Robotics Basics of Robot Electronics: Basic Electronic Components Fundamental Electrical Concepts Sensors Operational Amplifier Integrating Circuit Interfacing of Sensors Motors and Controlling Circuit Interfacing of Motors   LV1 (Session 2)  ...