Embedded

Programming
for

Quadcopters
 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

$55 $5 $20

Arduino Mega 2560 Arduino Nano Clone Teensy 3.1

& Prototyping Shield 8-bit AVR 32-bit ARM
8-bit AVR 16 MHz clock 96 MHz clock
16 MHz clock 32K Flash 256K Flash
256K Flash 2K Ram 64K Ram
8K Ram
 My Project - Inertial Measurement Unit

GY-87

$8

MPU6050 - 3 axis gyroscope, 3 axis accelerometer

HMC5883L - 3 axis magnetometer
BMP180 Barometer
3.3V or 5V
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 Total Quad

Duration
Rate Movement Angle

0 0 0 0°

5 °/s 2s 10 ° 10 °

-10 °/s 2s -20 ° -10 °

-5 °/s 1s -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
(x, roll)
(y, pitch)
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;

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

http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf
 IMU - Accelerometer
(x, roll)
(y, pitch)
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;

http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf
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
Mapped
Channel Function Min/Max
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)

http://rcarduino.blogspot.com/2012/01/how-to-read-rc-
receiver-with.html
 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
Flight Controller Code

3 problems to correct
 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