How to build your own Quadcopter Autopilot / Flight Controller

• 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

source code  ArduPilot version of the Arduino IDE.

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).

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 layer

void 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).

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:

result = map(VALUE, FROM_MIN, FROM_MAX, TO_MIN, TO_MAX).

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.

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

#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);

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*Kp
rearMotors = 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.

f 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 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);

Stablilised Control

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 pids
float 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 earlier
long 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 45 degrees 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
}

full code 如下:

#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>

#include <PID.h>

// ArduPilot Hardware Abstraction Layer
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;

// MPU6050 accel/gyro chip
AP_InertialSensor_MPU6000 ins;

// Radio min/max values for each stick for my radio (worked out at beginning of article)
#define RC_THR_MIN   1070
#define RC_YAW_MIN   1068
#define RC_YAW_MAX   1915
#define RC_PIT_MIN   1077
#define RC_PIT_MAX   1915
#define RC_ROL_MIN   1090
#define RC_ROL_MAX   1913

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

// Arduino map function
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;
}

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

// PID array (6 pids, two for each axis)
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

void setup() 
{
  // Enable the motors and set at 490Hz update
  hal.rcout->set_freq(0xF, 490);
  hal.rcout->enable_mask(0xFF);

  // PID Configuration
  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.7);
  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);

  // Turn off Barometer to avoid bus collisions
  hal.gpio->pinMode(40, GPIO_OUTPUT);
  hal.gpio->write(40, 1);
  
  // Turn on MPU6050 - quad must be kept still as gyros will calibrate
  ins.init(AP_InertialSensor::COLD_START, 
    AP_InertialSensor::RATE_100HZ,
                        NULL);

  // initialise sensor fusion on MPU6050 chip (aka DigitalMotionProcessing/DMP)
  hal.scheduler->suspend_timer_procs();  // stop bus collisions
  ins.dmp_init();
  hal.scheduler->resume_timer_procs();
  
  // We're ready to go! Now over to loop()
}

void loop() 
{
  static float yaw_target = 0;  
  // Wait until new orientation data (normally 5ms max)
  while (ins.num_samples_available() == 0);
 
  uint16_t channels[8];

  // Read RC transmitter and map to sensible values  
  hal.rcin->read(channels, 8);

  long rcthr, rcyaw, rcpit, rcroll;  // Variables to store radio in
  
  rcthr = channels[2];
  rcyaw = map(channels[3], RC_YAW_MIN, RC_YAW_MAX, -180, 180);
  rcpit = map(channels[1], RC_PIT_MIN, RC_PIT_MAX, -45, 45);
  rcroll = map(channels[0], RC_ROL_MIN, RC_ROL_MAX, -45, 45);

  // Ask MPU6050 for orientation
  ins.update();
  float roll,pitch,yaw;  
  ins.quaternion.to_euler(&roll, &pitch, &yaw);
  roll = ToDeg(roll) ;
  pitch = ToDeg(pitch) ;
  yaw = ToDeg(yaw) ;
  
  // Ask MPU6050 for gyro data
  Vector3f gyro = ins.get_gyro();
  float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);
  
  // Do the magic
  if(rcthr > RC_THR_MIN + 100) {  // Throttle raised, turn on stablisation.
    // Stablise PIDS
   float 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(wrap_180(yaw_target - yaw), 1), -360, 360);
  
    // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
    if(abs(rcyaw ) > 5) {
      yaw_stab_output = rcyaw;
      yaw_target = yaw;   // remember this yaw for when pilot stops
    }
    
    // rate PIDS
    long 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);  

    // mix pid outputs and send to the motors.
    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);
  } 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);
       
    // reset yaw target so we maintain this on takeoff
    yaw_target = yaw;
    
    // reset PID integrals whilst on the ground
    for(int i=0; i<6; i++)
      pids[i].reset_I();

  }
}

AP_HAL_MAIN();

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 2 seconds 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.

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 192.168.0.254 - you can change this in the app
Raspberry Pi: Download 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.
ArduPilot: Download 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.

Reference : http://owenson.me/build-your-own-quadcopter-autopilot/