Embedded Programming for Quadcopters

Embedded Programming

Quadcoptersfor

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

MovementQuad 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 Codemotor1 = 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

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