Embedded
Programming
Quadcopters
for

I’m Ryan Boland
Web Developer
@ Tanooki Labs
@bolandrm (github, twitter)

1. Components
2. Quadcopter physics
3. Sensor Inputs
4. Motor Outputs
5. Safety

Frame

Electronic Speed Controllers (ESCs)
& Motors

Lithium Polymer (LiPo) Battery

Remote Control Transmitter + Receiver

Flight Controller
Microprocessor &
Inertial measurement Unit (IMU)

My Project - Custom Flight Controller
Arduino Mega 2560
& Prototyping Shield
8-bit AVR
16 MHz clock
256K Flash
8K Ram
Arduino Nano Clone
8-bit AVR
16 MHz clock
32K Flash
2K Ram
Teensy 3.1
32-bit ARM
96 MHz clock
256K Flash
64K Ram
$5$55 $20

My Project - Inertial Measurement Unit
MPU6050 - 3 axis gyroscope, 3 axis accelerometer
HMC5883L - 3 axis magnetometer
BMP180 Barometer
3.3V or 5V
GY-87
$8

Sourcing Components/Parts

Configuration - + vs X

Orientation - Angles
x axis == roll
y axis == pitch
z axis == yaw

Maneuvering

The Code

Control Loop
void loop() {
while(!imu_read());
rc_read_values();
fc_process();
}

Control Loop
void loop() {
while(!imu_read());
rc_read_values();
fc_process();
}

Orientation (IMU)
• rotational rates (x, y, z)
(degrees per second)
• angles (x, y) (degrees)

IMU - Gyroscope
measures rotational
rate in °/sec

IMU - Gyroscope
average = -2.599 (°/s)

Orientation (IMU)
• rotational rates (x, y, z)
(degrees per second)
• angles (x, y) (degrees)

Gyroscope - Angles
Rotational
Rate
Duration
Total
Movement
Quad
Angle
0 0 0 0 °
5 °/s 2 s 10 ° 10 °
-10 °/s 2 s -20 ° -10 °
-5 °/s 1 s -5 ° -15 °

Gyroscope - Angles
uint32_t gyro_last_update = micros();
void compute_gyro_angles() {
mpu6050_read_gyro(&gyro_rates);
rates.x = gyro_rates.x + GYRO_X_OFFSET;
delta_t = (micros() - gyro_last_update) / 1000000;
gyro_angles.x += rates.x * delta_t;
gyro_last_update = micros();
}

Gyroscope - Angles
uint32_t gyro_last_update = micros();
void compute_gyro_angles() {
mpu6050_read_gyro(&gyro_rates);
rates.x = gyro_rates.x + GYRO_X_OFFSET;
delta_t = (micros() - gyro_last_update) / 1000000;
gyro_angles.x += rates.x * delta_t;
gyro_last_update = micros();
}

Gyroscope - Angles
uint32_t gyro_last_update = micros();
void compute_gyro_angles() {
mpu6050_read_gyro(&gyro_rates);
rates.x = gyro_rates.x + GYRO_X_OFFSET;
delta_t = (micros() - gyro_last_update) / 1000000;
gyro_angles.x += rates.x * delta_t;
gyro_last_update = micros();
}

Gyroscope - Angles
uint32_t gyro_last_update = micros();
void compute_gyro_angles() {
mpu6050_read_gyro(&gyro_rates);
rates.x = gyro_rates.x + GYRO_X_OFFSET;
delta_t = (micros() - gyro_last_update) / 1000000;
gyro_angles.x += rates.x * delta_t;
gyro_last_update = micros();
}

Gyroscope - Angles
How is our estimation?

Gyro Drift
Occurs when gyroscope data
changes between samples

Orientation (IMU)
• rotational rates (x, y, z)
(degrees per second)
• angles (x, y) (degrees) ?

IMU - Accelerometer
• measures acceleration in terms
of g-force (g)
• requires offset calibration,
similar to gyroscope data
• z axis should be calibrated to
1G!

IMU - Accelerometer
http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf
(y, pitch)
(x, roll)
x = accel_filtered.x;
y = accel_filtered.y;
z = accel_filtered.z;
accel_angles.x = atan2(y, z) * RAD_TO_DEG;
accel_angles.y = atan2(-1 * x, sqrt(y*y + z*z)) * RAD_TO_DEG;

IMU - Accelerometer
http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf
(y, pitch)
(x, roll)
x = accel_filtered.x;
y = accel_filtered.y;
z = accel_filtered.z;
accel_angles.x = atan2(y, z) * RAD_TO_DEG;
accel_angles.y = atan2(-1 * x, sqrt(y*y + z*z)) * RAD_TO_DEG;
(median filters)

IMU - Accelerometer
http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf
(y, pitch)
(x, roll)
x = accel_filtered.x;
y = accel_filtered.y;
z = accel_filtered.z;
accel_angles.x = atan2(y, z) * RAD_TO_DEG;
accel_angles.y = atan2(-1 * x, sqrt(y*y + z*z)) * RAD_TO_DEG;

IMU - Accelerometer

IMU - Accelerometer
Susceptible to vibrations

Combining Approaches
Gyroscope - Good for short durations
Accelerometer - Good for long durations
Complementary Filter!
#define GYRO_PART 0.995
#define ACC_PART 0.005
dt = <time since last update>;
angles.x = GYRO_PART * (angles.x + (rates.x * dt)) +
ACC_PART * accel_angles.x;

Combining Approaches
Gyroscope - Good for short durations
Accelerometer - Good for long durations
Complementary Filter!
#define GYRO_PART 0.995
#define ACC_PART 0.005
dt = <time since last update>;
angles.x = GYRO_PART * (angles.x + (rates.x * dt)) +
ACC_PART * accel_angles.x;

Combining Approaches
Gyroscope - Good for short durations
Accelerometer - Good for long durations
Complementary Filter!
#define GYRO_PART 0.995
#define ACC_PART 0.005
dt = <time since last update>;
angles.x = GYRO_PART * (angles.x + (rates.x * dt)) +
ACC_PART * accel_angles.x;

Combining Approaches
Gyroscope - Good for short durations
Accelerometer - Good for long durations
Complementary Filter!
#define GYRO_PART 0.995
#define ACC_PART 0.005
dt = <time since last update>;
angles.x = GYRO_PART * (angles.x + (rates.x * dt)) +
ACC_PART * accel_angles.x;

complementary filter
vs
previous approaches

Orientation (IMU)
• rotational rates (x, y, z)
(degrees per second)
• angles (x, y) (degrees)

Control Loop
void loop() {
while(!imu_read());
rc_read_values();
fc_process();
}

Remote Control
http://rcarduino.blogspot.com/2012/01/how-to-read-rc-
receiver-with.html
Channel Function Min/Max
Mapped
Min/Max
1 Roll (1000μs, 2000μs) (-25, 25)
2 Pitch (1000μs, 2000μs) (-25, 25)
3 Throttle (1000μs, 2000μs) (1000, 2000)
4 Yaw (1000μs, 2000μs) (-50, 50)

Control Loop
void loop() {
while(!imu_read());
rc_read_values();
fc_process();
}

Controlling Motors (ESCs)
Made to work with the
remote control.
Motor Max - 2000μs
Motor Min - 1000μs

Rate
Mode

3 problems to correct
Flight Controller Code

Flight Controller Code
motor1 = rc_throttle - roll_adjust - pitch_adjust - yaw_adjust;
motor2 = rc_throttle + roll_adjust + pitch_adjust - yaw_adjust;
motor3 = rc_throttle - roll_adjust + pitch_adjust + yaw_adjust;
motor4 = rc_throttle + roll_adjust - pitch_adjust + yaw_adjust;
correcting roll, pitch, yaw

Flight Controller Code
motor1 = rc_throttle - roll_adjust - pitch_adjust - yaw_adjust;
motor2 = rc_throttle + roll_adjust + pitch_adjust - yaw_adjust;
motor3 = rc_throttle - roll_adjust + pitch_adjust + yaw_adjust;
motor4 = rc_throttle + roll_adjust - pitch_adjust + yaw_adjust;
correcting roll, pitch, yaw

Flight Controller Code
motor1 = rc_throttle - roll_adjust - pitch_adjust - yaw_adjust;
motor2 = rc_throttle + roll_adjust + pitch_adjust - yaw_adjust;
motor3 = rc_throttle - roll_adjust + pitch_adjust + yaw_adjust;
motor4 = rc_throttle + roll_adjust - pitch_adjust + yaw_adjust;
correcting roll, pitch, yaw

Flight Controller Code
motor1 = rc_throttle - roll_adjust - pitch_adjust - yaw_adjust;
motor2 = rc_throttle + roll_adjust + pitch_adjust - yaw_adjust;
motor3 = rc_throttle - roll_adjust + pitch_adjust + yaw_adjust;
motor4 = rc_throttle + roll_adjust - pitch_adjust + yaw_adjust;
correcting roll, pitch, yaw

Flight Controller Code
motor1 = rc_throttle - roll_adjust - pitch_adjust - yaw_adjust;

The PI Controller
(Proportional-Integral)
• Calculates error based on difference
between sensor reading and pilot command
• Proportional term depends on present error
• Integral term depends on accumulation of
past errors

The PI Controller
(Proportional-Integral)
#define KP 2.0 # ???
#define KI 2.0 # ???
float error = desired_pitch - current_pitch;
proportional = KP * error;
integral += KI * error * dt;
output = proportional + integral;

The PI Controller
(Proportional-Integral)
#define KP 2.0 # ???
#define KI 2.0 # ???
float error = desired_pitch - current_pitch;
proportional = KP * error;
integral += KI * error * dt;
output = proportional + integral;

The PI Controller
(Proportional-Integral)
#define KP 2.0 # ???
#define KI 2.0 # ???
float error = desired_pitch - current_pitch;
proportional = KP * error;
integral += KI * error * dt;
output = proportional + integral;

The PI Controller
(Proportional-Integral)
#define KP 2.0 # ???
#define KI 2.0 # ???
float error = desired_pitch - current_pitch;
proportional = KP * error;
integral += KI * error * dt;
output = proportional + integral;

Flight Controller Code
motor1 = rc_throttle - roll_adjust - pitch_adjust - yaw_adjust;
motor2 = rc_throttle + roll_adjust + pitch_adjust - yaw_adjust;
motor3 = rc_throttle - roll_adjust + pitch_adjust + yaw_adjust;
motor4 = rc_throttle + roll_adjust - pitch_adjust + yaw_adjust;
correcting roll, pitch, yaw

Stabilize Mode
3 new PI controllers!

Ready to fly! (??)

Ready to fly! (??)
Tuning is hard!

Tuning

Safety & Handling Failure

Safety & Handling Failure
• Stale IMU values
• Stale remote control values
• Angles too high?
• Motor outputs too high? (indoor safe
mode)

Some Takeaways
• Be Safe
• Start small
(balancing robot?)
• Break things down
into subcomponents

Resources
How-to Guide:
https://ghowen.me/build-your-own-quadcopter-autopilot/
Similar Projects:
https://github.com/cTn-dev/Phoenix-FlightController
https://github.com/baselsw/BlueCopter
My Code:
https://github.com/bolandrm/rmb_multicopter
https://github.com/bolandrm/arduino-quadcopter (old)

Thanks!
@bolandrm