A talk about how I designed, built and programmed by quadcopter from scratch. Details about reading from sensors and algorithm for motor outputs.

1. Embedded
Programming
Quadcopters
for

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

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

6. Frame

7. Electronic Speed Controllers (ESCs)
& Motors

8. Lithium Polymer (LiPo) Battery

9. Remote Control Transmitter + Receiver

10. Flight Controller
Microprocessor &
Inertial measurement Unit (IMU)

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

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

13. Sourcing Components/Parts

14. Configuration - + vs X

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

16. Maneuvering

17. The Code

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

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

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

21. IMU - Gyroscope
measures rotational
rate in °/sec

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

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

24. 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 °

25. 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();
}

26. 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();
}

27. 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();
}

28. 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();
}

29. Gyroscope - Angles
How is our estimation?

30. Gyro Drift
Occurs when gyroscope data
changes between samples

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

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

33. 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;

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

35. 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;

36. IMU - Accelerometer

37. IMU - Accelerometer
Susceptible to vibrations

38. 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;

39. 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;

40. 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;

41. 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;

42. complementary filter
vs
previous approaches

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

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

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

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

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

48. Rate
Mode

49. 3 problems to correct
Flight Controller Code

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

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

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

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

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

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

56. 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;

57. 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;

58. 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;

59. 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;

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

61. Stabilize Mode
3 new PI controllers!

62. Ready to fly! (??)

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

64. Tuning

65. Safety & Handling Failure

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

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

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

69. Thanks!
@bolandrm