embedded programming for quadcopters
TRANSCRIPT
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