Ca rbon aeronautics

quadcopter
build and programming manual

learn 100 % < 250 g 10 min

hackable weight flight time
aeronautics programming
{ };
electronics
Ca rbon aeronautics
you are about to embark on an exciting
journey... building and programming a
quadcopter from scratch.

this manual will guide you every step of the

way, explaining the essentials on aeronaut-
ics, electronics and embedded program-
ming.

all components and programs are fully

hackable, meaning that you can adapt an-
ything you want and create a quadcopter
capable of stuff that goes way beyond the
scope of this manual!

good luck and most importantly: have fun!

Carbon Aeronautics quadcopter build and programming manual

Project, text and figures by Laurens Raes

The contents of this manual are the intellectual property of the company Carbon Aer-
onautics. The text and figures in this manual are licensed under a Creative Commons
Attribution - Noncommercial - ShareAlike 4.0 International Public Licence. This li-
cense lets you remix, adapt, and build upon your work non-commercially, as long as
you credit Carbon Aeronautics (but not in any way that suggests that we endorse you
or your use of the work) and license your new creations under the identical terms.

The information in this manual is provided "As Is” without any further warranty.
Neither Carbon Aeronautics or the author has any liability to any person or entity with
respect to any loss or damage caused or declared to be caused directly or indirectly by
the instructions contained in this manual or by the software and hardware described
in it. As Carbon Aeronautics has no control over the use, setup, assembly, modification
or misuse of the hardware, software and information described in this manual, no
liability shall be assumed nor accepted for any resulting damage or injury. By the act
of use, setup or assembly, the user accepts all resulting liability.

This is not a toy but an educational product and not intended for persons below the
age of 18 years old. The user is responsible for complying with the local regulations
concerning unmanned aircraft when flying outdoors, and to fly in a responsible man-
ner. This is a sophisticated product for advanced craftsman with previous experience
in the field of electronics and programming. The purpose of the safety instructions
and warnings in this manual is to attract your attention to possible dangers. They do
not by themselves eliminate any danger, nor are they fully exhaustive. They are no
substitutes for proper accident prevention measures or for the knowledge of the
electric safety rules that are expected to be known by experienced craftsmen.

First edition, August 2022.

Contents
Project 1
Concept, parts and programming..................................................8

PART I: rate mode

Project 2
LED control....................................................................................24
Project 3
Reading your battery level.............................................................28
Project 4
Sensing the rotation rate...............................................................34
Project 5
Gyroscope calibration....................................................................46
Project 6
Take your motors for a spin.........................................................52
Project 7
Receiving commands.....................................................................58
Project 8
Controlling your motors...............................................................66
Project 9
Battery management......................................................................72
Project 10
Assembling your quadcopter........................................................80
Project 11
Quadcopter dynamics....................................................................86
Project 12
Quadcopter rate control................................................................90
Project 13
The flight controller: rate mode...................................................96
 Part II: stabilization mode
Project 14
Measuring angles..........................................................................110
Project 15
The Kalman filter - one dimension...........................................120
Project 16
The flight controller: stabilize mode.........................................130

Part III: velocity mode

Project 17
Measuring altitude........................................................................142
Project 18
Measuring vertical velocity..........................................................154
Project 19
The Kalman filter - two dimensions.........................................162
Project 20
The flight controller: velocity mode..........................................174

Part IV: quadcopter design and simulation

Project 21
Motor and sensor simulation......................................................190
Project 22
Quadcopter dynamics simulation..............................................200
Project 23
Quadcopter PID controller........................................................210
Project 24
Estimate the PID values..............................................................214

Part V: expanding your horizon

 Project 1
Concept, parts and programming

radio control
desired orientation receiver transmitter
and throttle

radio waves

microcontroller electronic speed motor and propeller

control (4 x) (4 x)

motor
energy pulses
command
quadcopter (4 x) energy
orientation

orientation
sensor battery

flight controller powertrain

8
Ex plore the basics of your quadcopter
Let's start your exciting journey in the world of aeronautics, electronics and
programming with the concept behind the flying machine that you will build
and the parts that you need. This manual will help you tackle the basics and
enable you to build your own quadcopter; a drone with four motors.

The creation of flying machines is a true engineering challenge and involves solving
several problems, from aerodynamics to power systems. In the case of a quadcopter,
you rely on four motors and propellers to provide enough thrust to start flying. Obvi-
ously, these are not the only necessary components. The figure to the left displays the
basic overview of a quadcopter with three major active building blocks:

• The radio control system, which consists of a radiotransmitter and a receiver.

The position of the sticks on the radiotransmitter are transformed into com-
mands and subsequently sent to the receiver that is situated on your quadcopter.

• The flight control system, which consist of a microcontroller and some sen-
sors. The bare minimum you need to stabilize the quadcopter is an orientation
sensor, but you can add various other sensors (barometer, GPS, ultrasonic,...)
to make your flight easier. The information of your sensor and the commands
from your radiotransmitter are then processed in the microcontroller, which is
the brain of your quadcopter. The microcontroller calculates the optimal speed
of each of the four motors to keep the quadcopter in the air.

• The third building block is the powertrain, which is the high current part of the
quadcopter. The battery is the power source of the whole system and sends en-
ergy in the form of electrical current to four electronic speed controllers (ESCs);
an ESCs converts the provided current into current pulses, with a pulse length
proportional to the motor command sent from the microcontroller. This gives
a motor speed proportional to the motor command and in turn, a certain thrust
allowing you to take off!

And basically, that’s all there is to it! With the general idea behind your quadcopter
clearly understood, let’s have a look at all different physical parts that you will use.
Your quadcopter consists of three active building blocks; a radio control system,
flight controller and powertrain. Moreover, you also need a frame on which you can
mount all these active components. Some auxiliary parts are also necessary, to charge
the battery and test your microcontroller, sensors and powertrain before fixing them
to the frame.
9
 Concept, parts and programming

All necessary components are listed below and divided into parts for the frame, flight
controller, powertrain, battery and radio control. Each part is available on the con-
sumer market, so if you break a part during flight or you want to change parts, you
can easily buy it yourself. This manual is designed to guide you building your own
quadcopter while enabling you to change any aspect of it as well.

1 frame 2

lower quadcopter frame upper quadcopter frame frame spacers

1a CarbonAeronautics 1b CarbonAeronautics 1c M3 x 30 mm 2a

5V
G

1x 1x 4x

spacer fastening screws

1d M3 x 6 mm 1e battery strap
210 mm
1f landing pad
2d

12x 1x 4x + 1 reserve

cable ties standoff spacer

1g 16 mm
1h M3 x 20 mm 1i cable protector
500 mm 2g

6x + 4 reserve 4x + 2 reserve 1x

10 2j
 2 flight controller

microcontroller microcontroller connector orientation sensor

2a Teensy 4.0 2b USB A to micro B 2c GY-521 MPU-6050

VCC
GND

3V 22 21 20 19 18 17 16 15 14 SCL
5V

SDA
XDA
XCL
Y
ADO
G 0 2 3 4 5 6 7 8 9 10 11 INT

X
1x 1x 1x

barometer sensor fastening screws sensor locknuts

2d GY-BMP280 2e M3 x 20mm 2f M3

VCC
GND
SCL
SDA
CSB
SDC

1x 4x 4x

or sensor full nuts green and red LEd resistors

2g M3
2h 2i

100 Ω (2x)

510 Ω (2x)

2000 Ω (1x)

12x 1x + 1x

battery connector jumper wires diodes

112j XT60 2k female to female 10 cm 2l
Ca rbon aeronautics
Zener diode (1x)
 2000 Ω (1x)

12x 1x +Concept,
1x parts and programming

battery connector jumper wires diodes

2j XT60 2k female to female 10 cm 2l

Zener diode (1x)

(BZX79C2V4)

Diode (1x)
(1N4007)
5
1x 3x + 3 spares 1x

5a
female headers male headers breadboard
2m 40 pins - 2,54 mm 2n 40 pins - 2,54 mm - right angle 2o 400 points
+ - a b c d e f g h i j + -
1 1
2 2
3 3
4 4
5 5
6 6
7 7
8 8
9 9
10 10
11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
19 19
20 20
21 21
22 22
23 23
24 24
25 25
26 26
27 27
28 28
29 29
30 30
+ - a b c d e + -
f g h i j

2x 2x 1x

wire terminal strip jumper wires jumper wires

2p 2q male to female 10 cm
2r male to male 10 cm

1x 1x 3x + 3 spares 40x

male headers power switch slide switch

2s 40 pins - 2,54 mm - straight 2t BTS50080-1TMB
2u OS102011MS2QN1C

2x 1x 1x

12
 3 powertrain
motors Electronic speed
nsor motor fastening screws
6050
3a GEPRC GR1105 3b M2 x 4 mm 3c controllers
5000 kV HobbyKing 6A
ESC with BEC

4x 16x 4x

counter-clockwise propeller fastening

ts clockwise propellers
3d Gemfan 3018R 3e propellers 3f screws
Gemfan 3018 M2 x 8 mm

2x + 2 reserve 2x + 2 reserve 4x + 8 reserve

4 Battery
(2x)

(2x)
Batteries battery charger
4a Turnigy 2S 1300 mAh 4b Hobbyking B3AC
(1x)

e (1x) 2x 1x
V4)

5 Radio control
13
Ca rbon aeronautics
Radiotransmitter receiver and bind plug
5a Flysky FS-i6 4b Flysky FS-iA6B
 2x 1x
Concept, parts and programming

5 Radio control

Radiotransmitter receiver and bind plug

5a Flysky FS-i6 4b Flysky FS-iA6B

sWB sWC
sWA sWd

LE
d

UP
dA
TE

B/
VC
CH

C
6
CH
5
CH
4
CH
PP

CH

3
m/

2
CH
1
UP ok
TX
RX
doWn CAnCEL

PoWER
TX.V1 : 4.80 V
IntV1 : 4.99 V
sigs1 : 10
BInd kEy

1x 1x
Alternative parts
You are not limited to the parts that are described in this paragraph and chances are
you want to choose different components for various reasons such as higher thrust,
longer flight time or lower weight. The parts that can easily be swapped are the pro-
pellers, ESC (Electronic Speed Controller), motor and battery. To give you an idea of
the possibilities, this paragraph describes some successfully tested variations on the
basic quadcopter. The PID values derived later on in this manual are a good match
for all variations, but remember that the weight of your quadcopter will be affected:
the basic quadcopter weighs 247 gram while the combination of all the heaviest com-
ponents described in this paragraph weighs 278 gram.

The battery is perhaps the easiest interchangeable component. The base part is a 2S
battery with a capacity of 1300 mAh and a weight of 70 gram. Other tested possible
batteries include a 2S battery with 1000 mAh (weight: 60 gram) or a 2S battery with a
1500 mAh capacity (weight: 80 gram). Additional capacity comes at a cost in the form
of extra weight and thus a less flexible quadcopter.

The choice of your ESC and motor combination needs some more care. You should
make sure that the maximal load current of both your ESC and motor are similar: the
part with the smallest load current limits the load current of both components. Since
a motor or ESC with a higher load current generally weighs more, the optimal com-
bination consists of motors and ESC with similar load current. The base motor and
ESC combination (GEPRC GR1105 5000 kV and Hobbyking 6A ESC with BEC)
both have a load current of around 6 A. Another tested possibility is the combination
of the GEPRC GR1206 4500 kV and Hobbyking 12A ESC, both having a load cur-
rent of around 12 A. The 6 A combination of four motors and ESCs weighs around
50 gram, while the 12 A combination weighs 66 gram.
14
Another important value is the motor kV rating: this determines how fast the propel-
ler can turn at full throttle: a 5000 kV motor turns at 5000 rpm/V. Since a 2S battery
has a nominal voltage of 7.4 V, this equals to a nominal rmp of 5000 rpm/V x 7.4
V = 37 000 rpm. To lift a quadcopter with a weight between 200 and 300 gram, you
need a motor with a kV rating between 4000 and 6000, depending on the propeller.

Once you have chosen your ESC and motor combination, the propellers are next. A
larger propeller generates more thrust: this is great because it means your quadcopter
can weigh more and can successfully combat stronger wind gusts. However, the nec-
essary load current increases and your motor and ESC have to withstand this higher
current, otherwise they will overheat and possibly start burning. Larger propellers
obviously weigh more as well.
3018 3035 4024 4019
Gemfan propeller
(2 blade) (3 blade) (2 blade) (3 blade)
(weight in gram for 4 props)
3.4 g 5.6 g 6.4 g 8.4 g
GEPRC GR1105 5000 kV
+ 5A 7A 9A 12 A
Hobbyking 6A ESC with BEC
GEPRC GR1206 4500 kV
+ 5A 7A 9A 12 A
Hobbyking 12A ESC with BEC

Four different propellers are tested with the two motor/ESC combinations and tab-
ulated above together with their current at full throttle with a full 2S battery. The
colour code can be explained as:
• Green: the motor and ESC can withstand full throttle with this propeller for
longer periods of time - this combination is suitable for beginners.
• Orange: the motor and ESC can only withstand short bursts of full throttle
with this propeller - this combination is only suited for experienced flyers given
the risk of motor or ESC overheating.
• Red: this combination is not recommended given the high risk of motor and
ESC overheating.

For reference, the maximal dimensions for the battery and the propellers given the
frame width are included here as well:
• 10.2 cm is the maximal diameter of the propeller (corresponds with a 4 inch
propeller).
• The battery bay has a 12 cm x 4 cm x 3 cm dimension, but you need to leave
some space for the receiver, electronic cables, protectors and screws. This limits
the practical space to 8.5 cm x 3.3 cm x 1.5 cm.

15
Ca rbon aeronautics
 Concept, parts and programming

Additional required tools and material

To complete your build, you also require some additional tools and material. Except
for a computer, these are only necessary when starting the actual build, not when
testing the components in the first projects (except if you still need to solder headers
to your Teensy and sensors in order to test them on the breadboard).

• a soldering iron or station, to solder the motors wires, ESCs, resistors, LEDs
and male/ female headers to each other / the printed circuit frame on you quad-
copter frame.
• sufficient solder material.
• a soldering helping hand to clamp the parts you are soldering together.
• a wire stripper to strip the electrical insulation from the ESC and motor wires.
• a wire cutter to cut the ESC and motor wires.
• a computer capable of running Arduino (see arduino.cc/en/software)
• two hex keys (1.5 mm and 2 mm)
• a multimeter to check for short-circuits or bad connections.

16
General safety instructions
Battery
• Read the battery and battery charger manuals carefully before use.
• Never charge the batteries unattended.
• Before connecting the battery and your motor(s) or quadcopter for the first
time, make sure there are no short-circuits between your soldered components
using your multimeter.

Electronics
• Before connecting the electronics to a power source (such as your computer),
make sure that there are no short-circuits between your soldered components
using your multimeter.
• Remove the propellers and do not touch the motors unless you are sure that
your program is working properly to avoid losing control over your quadcop-
ter.
• Never run your motors without propellers.

Before flying
• Make sure the failsafe and safety-related code lines are implemented and work-
ing correctly.
• Check the regulations that are applicable in your country (with regard to max-
imal altitude, speed, weight,...) when flying your unmanned quadcopter out-
doors.

17
Ca rbon aeronautics
 Concept, parts and programming

Setup your microcontroller for programming

The core of your quadcopter project is the Teensy microcontroller that you will pro-
gram in such a way that it becomes the flight controller and thus brains of your
project. The Arduino software will be used to program the microcontroller, together
with Teensyduino.

You can find all information with regard to the installation of the necessary software
on the website of the Teensy manufacturer: www.pjrc.com/teensy. The installation
steps will be described here as well, but please refer to the pjrc and arduino websites
if you need additional troubleshooting.

1. Connect your new Teensy to your computer using the USB cable (see figure to the
right).

2. Your Teensy should come with the LED blink program pre-loaded; this means
that the orange LED on your Teensy should blink slowly after connection with your
computer.

3. Press and release the tiny pushbutton on the Teensy. The orange blinking LED
should stop and the red Teensy LED should be visible. This means your Teensy
works correctly.

4. Disconnect your Teensy from your computer by disconnecting the USB cable.

Verify Upload
serial monitor
File Edit Sketch Tools Help

BLINK
void setup() {
pinMode(13, OUTPUT);
}
void loop() {
digitalWrite(13, HIGH);
delay(500);
digitalWrite(13, LOW);
delay(500);
}

18
 microcontroller orange Teensy LEd
2a Teensy 4.0 (pin 13)

22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11
pushbutton

2 3 4
red Teensy LEd
(bootloader status)

3V

0
5V

G
micro-B connection
to your Teensy

UsB connection to
your computer

microcontroller connector
2b USB A to micro B

5. Download and install the Teensy Loader program, which communicates with your
Teensy board. Guidance on the installation process can be found at pjrc.com/teensy/
loader.html. Click on the operating system of your computer, read the information
and click on the Teensy Loader link to start downloading.

6. If you do not have the Arduino software (IDE) yet, download the latest version
from arduino.cc/download and install it on your computer. Guidance on the instal-
lation process can be found at arduino.cc/en/Guide/Windows or arduino.cc/en/
Guide/MacOSX or arduino.cc/en/Guide/Linux.

7. The final piece of software to install is Teensyduino, the software add-on for Ar-
duino. Download it by going to pjrc.com/teensy/td_download.html and follow the
instructions on this webpage.

8. Open the Arduino IDE; a new empty sketch should load automatically. Copy the
code in the figure to the left of this page and save the file under the name BLINK.
Now click on ‘Verify’. You will first have to save your sketch. After verification, you
should view the message ‘Done Compiling’ below on your screen. If you get an error,
verify whether you copied the code correctly.
19
Ca rbon aeronautics
 Concept, parts and programming

9. Before you can upload your verified code to your Teensy, you need to setup your
Teensy in the Arduino IDE. Go to tools and:
• Click on ‘Boards’ and ‘Teensyduino’ and select the Teensy 4.0 board.
• Verify that the USB type is ‘Serial’.
• Verify that the CPU speed is 600 MHz.
• Connect your Teensy again with your computer using the USB cable. Under
Port, a USB port should be displayed. Click on it.

10. Press the upload button on the screen. The internal Teensy LED should start
blinking again. Change the blinking speed by changing the delay time of 500 (milli-
seconds) in the code to for example 100 (milliseconds) to blink faster, or 1000 (mil-
liseconds) to blink slower. Adapt and upload the code to verify that you are truly in
control of the Teensy. When this test is successful, you are ready for the next project!
Code compatibility
The code throughout this book is compatible with the following Arduino (library)
versions:
• Arduino IDE: 1.8.16
• Teensyduino: 1.55
• BasicLinearAlgebra library: 3.2.0 (only necessary for part III)

orientation sensor barometer

2a microcontroller
Teensy 4.0
2c GY-521 MPU-6050
2d GY-BMP280
male header pins (straight)
provide the connection of the
components with the breadboard

male headers
2s 40 pins - 2,54 mm - straight

solder the male header

pins to the components
20
 File Edit Sketch Tools Help
Auto Format Ctrl+T
Archive Sketch
Fix Encoding & Reload
BLINK Manage Libraries... Ctrl+Shift+I
void setup() { Serial Monitor Ctrl+Shift+M
pinMode(13,Serial Plotter
OUTPUT); Ctrl+Shift+L
} WiFi101/WifiNINA Firmware Updater
void loop() {
digitalWrite(13, HIGH);
Board: “Teensy 4.0”
delay(500); USB Type: “Serial”
CPULOW);
digitalWrite(13, Speed: “600 MHz”
delay(500); Optimize: “Faster”
Keyboard Layout: “US English”
} Port
Get Board Info

Programmer: “AVRISP mkll”

Burn Bootloader

Solder pins to your microcontroller and sensors

You will use a breadboard to separately test the electronic components of your flight
controller. To be able to electrically connect the components with the breadboard,
you need to use straight male header pins that are soldered to your Teensy microcon-
troller, the MPU-6050 gyroscope and the BMP-280 pressure sensor. If these parts do
not come pre-soldered with header pins, you will need to solder them yourself.

For easy soldering, you can insert the pins in your breadboard and put the component
on top such that the pins are soldered straight to the microcontroller and sensors. If
you have never soldered before, you can consult the internet for some tutorials.

21
Ca rbon aeronautics
PART I: rate mode
in the first part, you will build your quad-
copter and program a flight controller that
enables you to fly in rate mode; this is the
easiest-to-implement controller that gives
you full control over the performance of
your quadcopter.

complex projects such as this one are often

cut in smaller, independent pieces that are
tested separately, before all components
are put together.

you will follow this approach and start with

simple building blocks and code, to eventu-
ally arrive at the full build and flight code.
 Project 2
LED control

resistors
2i 100 Ω 2o breadboard
400 points

+ - a b c d e f g h i j + -
1 1
2 2
3 3
4 4
5 5
6 green and red LEd
2h
6
7 7
8 8
9 9
10 10
11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
jumper wires
20
21
20
21
2r male to male 10 cm
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e + -
f g h i j

24
Us e LEDs to receive feedback
Throughout this manual, you will learn how to communicate with your quad-
copter by giving it commands. However, this communication goes only one-
way from your radiotransmitter to the quadcopter. Sometimes it is useful to
receive some feedback from the quadcopter, for example when the setup and
calibration process is finished or when the battery voltage becomes low. To do
this in an easy way without telemetry, you will use three signal LEDs.

The first led you will use is the internal led of the Teensy, which you already exper-
imented with. This orange led is controllable through pin 13 and requires no addi-
tional circuit building. Lighting this led can be useful to show that the microcontroller
receives power and is working correctly.

You will also use two additional external LEDs to signal the start and end of the setup
program. Before you are able to fly, the microcontroller will have to start the auxiliary
sensors and calibrate them. This takes about four seconds during which the quadcop-
ter is not yet able to start. During this time, you turn on the red LED to signal that
the quadcopter is still in the setup process. When the setup process is successfully
finished, you turn off the red LED and turn on the green LED. Let’s start to build
the electronic circuit necessary to light these external LEDs.

Connect two 100Ω resistors to pins Teensy

5 and 6 of your Teensy using jumper 14 11
wires. Pin 6 gives signals to the red 15 10
16 9
LED while pin 5 gives signals to the
17 8
green LED. Connect the long leg (+ 18 7
side or anode) of each LED with the 19 6
resistor and the short leg (- side or 20 5
cathode) with the negative bus line. 21 4
22 3
2
Configure your breadboard such that 3V
the ground G of the Teensy is con- 0
nected to the negative bus line as well. 5V G

The schematic view of this circuit is 100Ω 100Ω

shown to the right. You are now ready
to program your Teensy and to give Red Green
LED LED
signals to each LED.

25
 LED control

Coding
All arduino sketches consist of both a setup and a loop part. The code in the setup
part of the sketch only runs once, during startup of the microcontroller. The code in
the loop part of the sketch runs continuously when the setup part is finished.

As seen in the previous project, you can control the internal orange Teensy LED with
pin 13. Configure the pin as an output using the command pinMode() and use the
command digitalWrite() to give it the command HIGH, which will light the orange
LED and show that the microcontroller is powered and working.

To control the external LEDs, you will use the same commands. You already connect-
ed the red LED to pin 5 and the green LED to pin 6. To show that the setup process
is ongoing, you light up the red LED by giving it the HIGH command.

Now wait four seconds (=4000 milliseconds) using the delay() command in order to
simulate the setup process, which will take around four seconds to be completed in
your final quadcopter code.

To indicate that the setup process is finished, turn off the red LED using the com-
mand LOW and subsequently turn on the green LED.

The code in the loop part runs continuously. Because you do not write any commands
in this part, the green LED will be continuously illuminated as demanded in the last
line of the setup part.

Testing
Upload your new code to your Teensy using the USB cable and verify that all LEDs
light up in the correct order. Only the green LED should remain on after four sec-
onds.

26
1 void setup() { Initialize the setup
part

2 pinMode(13, OUTPUT); Turn on the internal

3 digitalWrite(13, HIGH); LED

4 pinMode(5, OUTPUT); Turn on the red LED

5 digitalWrite(5, HIGH);

6 delay(4000); Wait 4 seconds

7 digitalWrite(5, LOW); Turn off the red

8 pinMode(6, OUTPUT); LED and turn on the
9 digitalWrite(6, HIGH); green LED
10 }

11 void loop() { Start the loop part

12 }

Understanding digital output pins

You used the pins 5, 6 and 13 as digital pins, meaning that their output voltage is
binary: either HIGH (3V) or LOW (0V). This very simple command is sufficient
to turn on a LED (when the 3V voltage is applied) or turn off the LED (when
the 0V voltage is applied). The current necessary to light the LEDs is provided
with the resistors you placed in series; through Ohm’s law, you can calculate that
a voltage of 3V and a resistance of 100 Ω gives a current of 3V/100 Ω=0.03
Ampere or 30 mA.

27
Ca rbon aeronautics
 Project 3
Reading your battery level
Battery voltage

9V

Fully charged: 8.4 V

8V

Low battery: 7.4 V

Critically low: 7.2 V
7V

100% 80% 60% 40% 20% 5% 0%

Remaining battery energy
The evolution of the battery level in function of the battery voltage is displayed by
the figure above. It is important to notice that discharging your battery to a too low
voltage can degrade the battery and lead to a reduced capacity over time. Therefore,
a good guideline for prolonged battery lifetime is to not discharge your 2S battery
below its nominal voltage of 7.4V. Because the battery voltage fluctuates during flight
and can drop temporarily when you suddenly increase the throttle, your flight control-
ler will check if the voltage is above 7.5V before starting the motors.

Now how can you measure the voltage of the battery? Easy: the voltage applied to
any pin of your microcontroller can be read digitally. Unfortunately, there is one
catch: the pins of the Teensy are only 3.3V-tolerant, meaning that applying a voltage
higher than 3.3V can damage the microprocessor. Therefore, you need to use a volt-
age divider: this electronic circuit divides the voltage of the battery to a value low
enough to be used by your Teensy. Consider the first circuit displayed on the right:
through Ohm’s law, the current I is equal to the battery voltage Vbattery divided by the
resistance R1.

In the second circuit, a second resistance R2 is used. The battery voltage is now equal
to the current divided by the sum of two resistances. With the third circuit, you con-
nect a pin of the Teensy between both resistances.
28
Le arn to measure voltage and battery lifetime
A critical part of your quadcopter is the battery; it stores enough energy to
let you fly for quite a while. But how do you know when the battery is almost
empty? In this project, you will learn how the battery voltage drops during the
flight and measure it in order to estimate the remaining battery lifetime.

The battery you use in this project is a 2 cell lithium-polymer battery, where the cells
are placed in Series (=2S). Each cell has a nominal voltage of 3.7V and since the cells
are placed in series, the total nominal voltage is equal to 7.4V. A 3S battery would give
you 3 x 3.7 = 11.1V. The nominal voltage is the reference voltage of the battery, but
you will always charge the battery up to the charge voltage, which is equal to 8.4V for
a 2S battery.

When using a fully charged battery to fly your quadcopter, the battery voltage will
drop from the charge voltage of 8.4V to the nominal voltage of 7.4V and even lower
when you use more energy. This is inevitable and results in a lower thrust over time,
because the speed of the motors is proportional to the provided voltage. Fortunately
you can use this property also to your advantage, because by measuring the battery
voltage you are able to estimate the remaining battery energy.
I
7.4 V

Vbattery
Vbattery R1 I=
R1

R1
GND 7.4 V
I
Vbattery Vbattery
I=
R2 R1+R2

7.4 V R1 GND

I pin 15 VTeensy
Vbattery
I=
R2
R2

GND
VTeensy
29
 Reading your battery level

+ - a b c d e f g h i j + -
1 1
2 2
3 3
resistors
2i 510 Ω
4
5
4
5
2000 Ω 6 6
7 7
8 8
9 9
10 10
11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11
19 19
20 20
21 21
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e + -
f g h i j

Coding
Lets first declare the voltage as a floating point number. To be able to measure the
voltage multiple times without rewriting the same lines of code over and over, you
will create the function battery_voltage. This function can be called as often as you
want.

The analog voltage over pin 15 can be measured using the function analogRead().
Since the default resolution for analogRead is equal to 10 bit, a voltage of 0V gives
you the digital number 0 and the maximal input voltage of 3.3V gives the digital
number 210-1=1023. Moreover you have built a 1:5 voltage divider. This means that
the battery voltage is equal to the measured voltage divided by 1023 / (3.3 x 5) = 62.

You will visualize the voltage at pin 15 in real-time on your computer with the serial
monitor. Set the speed at which the Teensy communicates with your laptop to 57600
bits per second.

30
 Vbattery
I = Vbattery
I = R 1 + R2
R 1 + R2

VT eensy
= VT eensy
The voltage applied to the pin of theITeensy (which will be pin 15) is equal to the
I = R2
current divided by the second resistance R2. R Since
2 the current I will be the same for
the second and third circuit, the following equation holds:
Vbattery VT eensy
= I = eensy
+ R2 = I = VTR
RV1battery 2
R 1 + R2 R2

R2
VT eensy = Vbattery · R2
VT eensy = Vbattery · R 1 + R2
R 1 + R2
By choosing the value of R1 to be equal to 2000 Ω and the value of R2 to be equal to
510 Ω, VTeensy becomes equal to Vbattery
divided by 5. You have now designed a 2 kΩ
1:5 voltage divider! With a battery volt- Teensy
age of 8.4V, the voltage measured by 14 11
your Teensy equals 1.7V, low enough to 15 10
respect the 3.3V tolerance of the Teen- 16 9
17 8
sy pins.
18 7
19 6
To test your circuit, you will not yet con- 20 5
nect your battery but use the 5V output 21 4
pin of the Teensy as voltage source, and 22 3
measure this value with pin 15 and your 2
new voltage divider. Connect the 5V pin 3V
0
with a 2000 Ω resistor to pin 15 and the 5V G
ground pin with a 510 Ω resistor to pin
15 as shown on the figure to the left.
510Ω
You are now ready to code.

1 float Voltage; Read the battery volt-

2 void battery_voltage(void) { age
3 Voltage=(float)analogRead(15)/62;
4 }

5 void setup() { Setup the serial mon-

6 Serial.begin(57600); itor

31
Ca rbon aeronautics
 Reading your battery level

Measure the voltage each 50 milliseconds and print it to the serial monitor, with each
time the unit V behind.

Testing
Upload the code and open the serial monitor (Ctrl+Shift+M or click on the serial
monitor icon) with the USB adapter still connected to your Teensy. To see values
that make sense, you should set the baud rate such that it corresponds with the baud
rate that you have chosen in the code, namely 57600 baud. Now you should see the
measured values, who will be more or less equal to 5V. When you connect the battery
in a later stage, the measured voltage will vary between 8.4V and 7V.

32
7 pinMode(13, OUTPUT);
8 digitalWrite(13, HIGH);
9 }
10 void loop() { Print the battery volt-
11 battery_voltage(); age to the serial mon-
12 Serial.print(Voltage); itor
13 Serial.println("V");
14 delay(50);
15 }

1. upload code
2. open serial monitor
File Edit Sketch Tools Help

Teensymonitor
BLINK
15:22:15.581 -> 4.88V
15:22:15.628 -> 4.88V
void setup() {
15:22:15.674 -> 4.89V
pinMode(13,->OUTPUT);
15:22:15.721 4.87V
}15:22:15.768 -> 4.88V 4. check measured values
void loop() {-> 4.88V
15:22:15.815
15:22:15.861 -> HIGH);
digitalWrite(13, 4.89V
15:22:15.908
delay(500); -> 4.87V
digitalWrite(13, LOW);
delay(500); Newline 57600 baud clear output
}
3. check baud rate

33
Ca rbon aeronautics
 Project 4
Sensing the rotation rate

z y X
X
z
y
Roll around the
X axis

z X

y z
y
Pitch around
the y axis

X
z X z
X

yaw around the

z axis

34
Me asure the rotation of your quadcopter
To stabilize your quadcopter, you need measurements of its three-dimension-
al orientation. In rate control mode, it is sufficient to know the rotation rates
when rolling, pitching and yawing. A sensor able to record these rotation rates
is called a gyroscope. During this project, you will learn how to read the data
sent from your gyroscope.

The gyroscope that you will use is included in the MPU-6050, a low-cost off-the
shelve orientation sensor. While the MPU is not a very precise sensor, its accuracy is
sufficient to get great results balancing your quadcopter. You will use the gyroscope
to measure the roll rate, pitch rate and yaw rate: this means that you do not measure
absolute angles in degrees (°), but rather angular rates in degrees per second (°/s). An
angular rate of 30°/s for example, means that you rotate 30° each second and will
perform a full 360° rotation in 12 seconds (360°/ (30°/s)). You will learn later on that
you can use these rotational rates to keep the quadcopter balanced; for example when
you want the drone to stay at its current orientation, its angular rate needs to be 0°/s.

Before you continue, you need to be fully aware of the direction of the roll, pitch
and yaw rotational rates. These three rotations are visualized on the figure to the left:
• A roll rotation means that you rotate clockwise around the X axis of the gyro-
scope.
• A pitch rotation means that you rotate clockwise around the Y-axis of the gy-
roscope.
• A yaw rotation means that you rotate counter clockwise around the Z axis of
the gyroscope.

The respective axis around which you turn, is the only axis that keeps pointing to the
same direction during the turn: on the figure, this is each time the red axis.

Mounting instructions of the gyroscope on your quadcopter

Notice that the X and Y axes and their respective rotation directions are also writ-
ten physically on the MPU-6050 sensor itself. When building the quadcopter and
soldering the MPU-6050 to it, always make sure that the axes written on the sensor
are aligned with the roll, pitch and yaw axes of the quadcopter itself.

35
 Sensing the rotation rate

+ - a b c d e f g h i j + -
1 1
2 2
3 3
VCC
4 4
GND
5 5
SCL
6 6
SDA
7 XDA
7
8 XCL
8
Y
9 ADO
9
10 10
INT

X
11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11
19 19
20 20
21 21
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e + -
f g h i j

Coding
The code for the I2C protocol is rather complex, so you use a predefined library
called Wire.h. This was normally installed automatically when you installed Teensy-
duino, but you can still install it at this point if necessary: in the Arduino IDE, go to
sketch → include library → manage libraries and type Wire in the search bar. Click on
install and you are ready to use it.

Define the roll, pitch and yaw rates in degrees/s (°/s) as global variables. You will
write the output of the measurements from the sensor to these variables.

Use once again a function to get the data from the gyro. With the I2C protocol, each
device (sensor) has its unique address. For our MPU-6050, this address can be found
in the register documentation and has a default value of 0x68. Routing function Wire.
beginTransmission to this address starts the communication with the sensor.

Sensor documentation
All information about sensors and their setup can easily be accessed online; try
and look up the MPU-6050 register map and product specification documenta-
tion.
36
How can you connect the MPU-6050 with your microcontroller? The communica-
tion protocol that you will use is I2C. This protocol needs two wires: a serial commu-
nication line (=SDA) through which the data can be transferred bit by bit, and a line
that carries the clock signal (=SCL). The exact design of this protocol is beyond the
scope of your project, but one Teensy
14 11
of its advantages is the transfer
MPU-6050 VCC 15 10
of information from multiple GND 16 9
sensors using the same SDA SCL 17 8
and SCL lines to the microcon- SDA 18 7
XDA 19 6
troller. This will prove useful XCL 20 5
when you will connect a baro- ADO 21 4
metric sensor later on. INT 22 3
2
3V
The wiring of the MPU-6050 0
5V
to the Teensy is rather straight- G

forward: connect 5V to Vcc and

G to GND to feed the sensor.
Subsequently, you connect the
serial communication output
SDA on the sensor to pin 18 of
the Teensy and the clock signal
output SCL to pin 19. You are now ready to start programming.

1 #include <Wire.h> Include the Wire li-

brary

2 float RateRoll, RatePitch, RateYaw; Declare the global

variables

3 void gyro_signals(void) { Start I2C communi-

4 Wire.beginTransmission(0x68); cation with the gyro

37
Ca rbon aeronautics
 Sensing the rotation rate

Some registers can be accessed to select some predefined options of the MPU-6050.
One of these options is a low pass filter, which will be necessary to filter out high fre-
quency vibrations and hence sharp increases and decreases in rotation rates that are
caused by running motors. The configuration register, where you can activate this op-
tion, has the hexadecimal address 0x1A according to the documentation (this is equal
to a decimal address of 26). The options for the low-pass filter correspond to bits 0,
1 and 2 in this address (the Digital Low Pass Filter DLPF setting). You choose a low
pass filter with a cut-off frequency of 10 Hz, which corresponds to a value for the
DLPF setting of 5. This corresponds in turn to the following binary representation:
00000101. Converting this to a hexadecimal value gives an address of 0x05.

The table from the register documentation that explains the configuration register
of the MPU-6050 is displayed on the right. The binary representation for setting the
value for the DLPF is given in the third column.

In addition to the low pass filter, you also need to set the sensitivity scale factor of the
sensor. The measurements of the MPU-6050 are recorded in LSB (Least Significant
Bit). Choose a sensitivity setting of FS_SEL=1 to set the scale factor to 65.5 LSB/
(°/s). This means that 1°/s corresponds to 65.5 LSB. You will take into account this
scale factor later on in the code. The gyroscope configuration register to activate this
option has the hexadecimal address 0x1B (or a decimal address of 27). The FS_SEL
setting of 1 corresponds to a 2-bit binary representation of 01. The other settings in
the register can be set to zero, giving a binary representation of 00001000. Converting
this to a hexadecimal value gives an address equal to 0x08.

The table from the register documentation that explains the gyroscope configuration
register of the MPU-6050 is displayed on the right. The binary representation for the
setting of the FS_SEL value is given in the third column.

Now you are ready to start importing the measurement values of the gyro. These
are located in the registers that hold the gyroscope measurements, which have the
hexadecimal numbers 43 to 48. You start writing to address 0x43 to indicate the first
register you will use.

Request 6 bytes from the MPU-6050 such that you can pull the information of the 6
registers 43 to 48 from the sensor.

38
5 Wire.write(0x1A); Switch on the low-
6 Wire.write(0x05); pass filter
7 Wire.endTransmission();

Register
Register
(Deci- Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0
(Hex)
mal)
1A 26 - - EXT_SYNC_SET[2:0] DLPF_CFG[2:0]
0 0 0 0 0 1 0 1

8 Wire.beginTransmission(0x68); Set the sensitivity

9 Wire.write(0x1B); scale factor
10 Wire.write(0x8);
11 Wire.endTransmission();

Register
Register
(Deci- Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0
(Hex)
mal)
1B 27 XG_ST YG_ST ZG_ST FS_SEL[1:0] - - -
0 0 0 0 1 0 0 0

12 Wire.beginTransmission(0x68); Access registers stor-

13 Wire.write(0x43); ing gyro measure-
14 Wire.endTransmission(); ments

15 Wire.requestFrom(0x68,6);

39
Ca rbon aeronautics
 Sensing the rotation rate

Registers 43 and 44 contain the gyro measurements of the rotational rate around
the X axis, in LSB (Least Significant Bit). According to the documentation, they are
the result of a unsigned 16-bit measurement. This means you will declare GyroX as
an unsigned 16-bit integer int16_t. Because the measurement of the rotational rate
around the X axis is spread out over two registers with each 8 bits, you will have to
merge this information by calling Wire.read() twice.

You repeat the same code for registers 45 and 46 (rotational rate around the Y axis)
and registers 47 and 48 (rotational rate around the Z axis).

The measurements are expressed in LSB but you want this information in °/s, not
LSB. You have set the LSB sensitivity scale factor of the MPU-6050 equal to 65.5
LSB/(°/s). Therefore you just divide the values in LSB by 65.6 LSB/(°/s) to get the
measurement values in °/s. Take care of converting the 16-bit integer values of the
measurements in LSB to a floating point representation. As discussed earlier this pro-
ject, the roll rate corresponds to the rotation around the X axis, the pitch rate to the
rotation around the Y axis and the yaw rate to the rotation around the Z axis.

Set the clock speed of the I2C protocol to 400 kHz. This value comes from the
product specifications of the MPU-6050 which states that communication with all
registers of the device must be performed using I2C at 400 kHz. Use a delay of 250
milliseconds to give the MPU-6050 time to start.

To activate the MPU-6050, write to the power management register, which has the
hexadecimal number 6B. All bits in this register have to be set to zero in order for the
device to start and continue in power mode. Hence the hexadecimal address becomes
0x00.

Terminate the connection with the gyroscope and end the setup section.

In the loop part of the code, call your function and print the roll, pitch and yaw rates
on the serial monitor. Wait 50 milliseconds after each loop to be able to read the val-
ues on the serial monitor and close the loop function.

40
16 int16_t GyroX=Wire.read()<<8 | Wire.read(); Read the gyro meas-
urements around the
X axis

17 int16_t GyroY=Wire.read()<<8 | Wire.read();

18 int16_t GyroZ=Wire.read()<<8 | Wire.read();

19 RateRoll=(float)GyroX/65.5; Convert the measure-

20 RatePitch=(float)GyroY/65.5; ment units to °/s
21 RateYaw=(float)GyroZ/65.5;
22 }

23 void setup() {
24 Serial.begin(57600);
25 pinMode(13, OUTPUT);
26 digitalWrite(13, HIGH);

27 Wire.setClock(400000); Set the clock speed

28 Wire.begin(); of I2C
29 delay(250);

30 Wire.beginTransmission(0x68); Start the gyro in pow-

31 Wire.write(0x6B); er mode
32 Wire.write(0x00);

33 Wire.endTransmission();
34 }

35 void loop() { Call the predefined

36 gyro_signals(); function to read the
37 Serial.print("Roll rate [°/s]= "); gyro measurements
38 Serial.print(RateRoll);
39 Serial.print(" Pitch Rate [°/s]= ");
40 Serial.print(RatePitch);

41
Ca rbon aeronautics
 Sensing the rotation rate

Testing
Upload the code to your microcontroller and open the serial monitor. You will notice
that not all values are equal to zero even though you do not move the MPU-6050:

Roll rate [°/s]= -8.70 Pitch Rate [°/s]= 0.89 Yaw Rate [°/s]= 1.95
Roll rate [°/s]= -8.69 Pitch Rate [°/s]= 0.92 Yaw Rate [°/s]= 1.97
Roll rate [°/s]= -8.66 Pitch Rate [°/s]= 0.87 Yaw Rate [°/s]= 1.94

It is normal when you do not have the same values as mentioned above. You will
learn more on how to solve this phenomenon through calibration in the next project.

What are registers?

Registers are places on a microcontroller (the Teensy but also the MPU-6050,
since this sensor has a microcontroller as well) that are used as:
• Fast storage locations to store data temporary.
• Locations where you can set predefined options.

You select a register by using its unique address, which is given in the documen-
tation of the microcontroller or sensor. With the I2C arduino library, you use the
function Wire.write(address) to select the register of choice.

If you select a register to set some predefined option, you once again use the Wire.
write function. You find the predefined options once again in the documentation.
Usually each register has a number of bits: you can set each bit to 0 or 1, which
corresponds to different options. Converting the resulting binary representation
to a hexadecimal representation gives you the argument for the Wire.write func-
tion.

If you select a register to read data, use the function Wire.read() after selecting
the address and reserving the necessary bytes. Let's go back to the example of the
low pass filter. Assume you want to set a low pass filter of 20 Hz instead of 10
Hz. You already know that the register address is 0x1A. The documentation of
the MPU-6050 says that you need to set the value of DLPF_CFG to 4 for the 20
Hz filter. Moreover, DLPF_CFG occupies the first three bits of the 0x1A register:

42
41 Serial.print(" Yaw Rate [°/s]= ");
42 Serial.println(RateYaw);
43 delay(50);
44 }

Register Register
Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0
(Hex) (Decimal)

1A 26 - - EXT_SYNC_SET[2:0] DLPF_CFG[2:0]

The number 4 in a three bit binary representation is equal to 1 0 0: you just mul-
tiply each binary number with 2n, where n is the bit number:
Bit2 Bit1 Bit0
Binary representation 1 0 0
2n 22=4 21=2 20=1
Decimal representation 1x4+0x2+0x1=4

If you decide to not set an option for bit3 to bit7 and stick with the default value,
the full 8 bit binary and decimal representation becomes:
Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0
Binary representation 0 0 0 0 0 1 0 0
2n 27=128 26=64 25=32 24=16 23=8 22=4 21=2 20=1
Decimal representation 0 x 128 + 0 x 64 + 0 x 32 + 0 x 16 + 0 x8 + 1 x 4 + 0 x 2 + 0 x 1 = 4

The Wire function use hexadecimal representation: conversion from decimal to

hexadecimal is a little bit more complex, but you can use an online converter. 4 in
hexadecimal form is equal to 0x04.

43
Ca rbon aeronautics
 Sensing the rotation rate

A low pass filter?

In line six of the code, you choose the option to send the measurement data
through a low-pass filter with a cut-off frequency of 10 Hz. This is a crucial line
of code, as the flight controller would not be able to stabilize the drone without it.
Why? Your gyroscope is a very sensitive sensor and its readings will be dramatically
affected by the vibrations caused by the brushless motors. The sample rate of the
gyroscope is equal to 8 kHz, which corresponds to a measurement each 1/8000 =
0.000125 seconds. The high frequency vibrations from the motors will cause small
but very fast accelerations of the quadcopter frame, which are recorded by the
gyroscope. The faster the motors spin, the larger the vibrations become; the figure
to the right shows the readings of the gyroscope with the motors switched off,
switched on and with increasing throttle. During all three cases, the quadcopter
stays stationary on the ground so the rotation rate should be equal to 0°/s. From
the figure you observe that once the motors are started, the unfiltered gyroscope
values start to fluctuate a lot. It is clear that it becomes impossible to stabilize your
quadcopter with measurement values that vary between 40 and -40°/s while the
quadcopter itself remains stationary and the real rotation rate is equal to 0 °/s.

To solve this issue, you use a low pass filter with a cut-off frequency of 10 Hz.
The filter attenuates the measurements of the sensor with a frequency of 10 Hz
and higher. This means that sensor variations that happen faster than 1/10=0.1
seconds will only have a limited impact on the final measurement values. The val-
ue of 10 Hz is chosen through trail-and-error during testing of the quadcopter;
motor vibration frequencies change with each brushless motor and damping of
the vibration depends on the whole frame. The blue line on the figure to the right
shows the filtered values; they are not affected by the vibrations caused by the
brushless motors and are hence suited for your flight controller.

44
Rotation rate measured by gyroscope

60 °/s motor speed increase

30 °/s

0 °/s

filtered
start of motors
-30 °/s
unfiltered

-60 °/s
0 2 4 6 8 10
Time [seconds]

45
Ca rbon aeronautics
 Project 5
Gyroscope calibration

+ - a b c d e f g h i j + -
1 1
2 2
3 3
VCC
4 4
GND
5 5
SCL
6 6
SDA
7 XDA
7
8 XCL
8
Y
9 ADO
9
10 10
INT X

11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
20 20
21 21
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e + -
f g h i j

Coding

You need four additional variables for calibration: the calibration values for the roll,
pitch and yaw rotation rate and a variable to keep track of the number of values you
have already recorded to use for the calibration.

46
Te ach your gyroscope the correct rotation rates
With this short project, you will teach your gyroscope the correct rotation rates
using a technique called calibration. You will use known rotation rates to cor-
rect the values given by your sensor.

At the end of the previous project, you saw that the rotation measurements given by
your gyroscope were not correct; even though you did not move the MPU-6050, it
still gave non-zero values. You still need to tell the instrument what its physical refer-
ence point is. Adjusting the measurements of a sensor such that they correspond with
real physical values is called calibration.
Teensy
In the case of a gyroscope, 14 11
MPU-6050 VCC 15 10
the easiest reference value that
GND 16 9
you can use is the rotation rate SCL 17 8
when the sensor is not moving: SDA 18 7
XDA 19 6
this rotation rate should obvi-
XCL 20 5
ously be zero. Because the gyro ADO 21 4
measurements always tend to INT 22 3
fluctuate due to small vibrations 2
3V
in the environment, you take 0
the average of a large number 5V G
of uncorrected measurement
values when the sensor is not
moving, calculate their average
value and subtract this average
value from all future measure-
ment values. You can easily in-
tegrate these additional calibra-
tion steps in the code of the previous project. The electronic circuit stays the same.
1 #include <Wire.h>
2 float RateRoll, RatePitch, RateYaw;

3 float RateCalibrationRoll, RateCalibrationPitch, Declare the calibra-

RateCalibrationYaw; tion variables
4 int RateCalibrationNumber;

5 void gyro_signals(void) {
6 Wire.beginTransmission(0x68);
47
 Gyroscope calibration

7 Wire.write(0x1A);
8 Wire.write(0x05);
9 Wire.endTransmission();
10 Wire.beginTransmission(0x68);
11 Wire.write(0x1B);
12 Wire.write(0x08);
13 Wire.endTransmission();
14 Wire.beginTransmission(0x68);
15 Wire.write(0x43);
16 Wire.endTransmission();
17 Wire.requestFrom(0x68,6);
18 int16_t GyroX=Wire.read()<<8 | Wire.read();
19 int16_t GyroY=Wire.read()<<8 | Wire.read();
20 int16_t GyroZ=Wire.read()<<8 | Wire.read();

In the setup part of the program, create a loop in which you take 2000 measurement
values from the gyroscope. Each value is taken 1 millisecond after the other (hence
the delay(1)) which means this step takes 2000 x 1 ms= 2 seconds. You add all meas-
ured values in the Roll/Pitch/YawRateCalibration variables. During this measure-
ment step, it is important to not move your gyroscope as the goal is to determine the
measured values at a rotation rate of zero.

Take the average calibration value by dividing the sum of the 2000 measurement
values by 2000. Now you have the measurement values at which the rotation rates
are zero.

Once the setup is finished and you have determined the calibration values, subtract
them from the measured values in order to get the correct physical values. Print the
corrected values to the serial monitor.

48
21 RateRoll=(float)GyroX/65.5;
22 RatePitch=(float)GyroY/65.5;
23 RateYaw=(float)GyroZ/65.5;
24 }
25 void setup() {
26 Serial.begin(57600);
27 pinMode(13, OUTPUT);
28 digitalWrite(13, HIGH);
29 Wire.setClock(400000);
30 Wire.begin();
31 delay(250);
32 Wire.beginTransmission(0x68);
33 Wire.write(0x6B);
34 Wire.write(0x00);
35 Wire.endTransmission();
36 for (RateCalibrationNumber=0; Perform the calibra-
37 RateCalibrationNumber<2000; tion measurements
38 RateCalibrationNumber ++) {
39 gyro_signals();
40 RateCalibrationRoll+=RateRoll;
41 RateCalibrationPitch+=RatePitch;
42 RateCalibrationYaw+=RateYaw;
43 delay(1);
44 }
45 RateCalibrationRoll/=2000; Calculate the calibra-
46 RateCalibrationPitch/=2000; tion values
47 RateCalibrationYaw/=2000;
48 }
49 void loop() {
50 gyro_signals();
51 RateRoll-=RateCalibrationRoll; Correct the measured
52 RatePitch-=RateCalibrationPitch; values
53 RateYaw-=RateCalibrationYaw;
54 Serial.print("Roll rate [°/s]= ");
55 Serial.print(RateRoll);
56 Serial.print(" Pitch Rate [°/s]= ");
57 Serial.print(RatePitch);
58 Serial.print(" Yaw Rate [°/s]= ");
59 Serial.println(RateYaw);
60 delay(50);
61 }

49
Ca rbon aeronautics
 Gyroscope calibration

Testing
When you run the code and open the serial monitor, the roll, pitch and yaw rates
should be almost zero when you do not move the gyroscope. Remember that during
the setup phase, when no values are yet displayed on the serial monitor, you should
not move the gyroscope in order to ensure a correct calibration.
• Roll rate [°/s]= 0.09 Pitch Rate [°/s]= -0.10 Yaw Rate [°/s]= -0.03
• Roll rate [°/s]= 0.09 Pitch Rate [°/s]= -0.09 Yaw Rate [°/s]= -0.03
• Roll rate [°/s]= 0.04 Pitch Rate [°/s]= -0.04 Yaw Rate [°/s]= -0.03

Now try to experiment by moving the gyroscope in the directions displayed in the
figure to the right. When you for example pitch around the Y axis from 0 to 45°,
wait and go back to 0°, the pitch rate should first increase with a value proportional
on how fast you rotate, subsequently fall to around 0°/s and then go negative with a
value proportional on how fast you rotate back to 0°.

Pitch from 0 to 45° and hold at 45°:

• Roll rate [°/s]= 0.01 Pitch Rate [°/s]= 0.02 Yaw Rate [°/s]= 0.00
• Roll rate [°/s]= -0.06 Pitch Rate [°/s]= 185.71 Yaw Rate [°/s]= -0.10
• Roll rate [°/s]= -0.05 Pitch Rate [°/s]= 0.06 Yaw Rate [°/s]= 0.02

Pitch from 45° back to 0° and hold at 0°:

• Roll rate [°/s]= -0.05 Pitch Rate [°/s]= 0.06 Yaw Rate [°/s]= 0.02
• Roll rate [°/s]= 0.65 Pitch Rate [°/s]= -177.02 Yaw Rate [°/s]= 0.39
• Roll rate [°/s]= -0.03 Pitch Rate [°/s]= 0.06 Yaw Rate [°/s]= 0.00

Try the same test for the other directions to verify that your code is working properly.

Time to fly?
The calibration needs to be performed during each start-up procedure, because
the gyroscope measurement values tend to drift over time. You cannot start the
motors yet during calibration, because their vibrations will impact the quality of
the calibration. This means that the setup procedure takes some seconds before
you can actually start the motors and begin your flight. That is why some projects
ago, you learned to signal the status of your quadcopter with the red and green
LEDs. Be mindful also to not move your quadcopter during this startup phase,
as this will affect the calibration quality and hence the smoothness of your sub-
sequent flight.

50
 z y X
X
z
y
Roll around the
X axis

z X

y z
y
Pitch around
the y axis

X
z X z
X

yaw around the

z axis

51
Ca rbon aeronautics
 Project 6
Take your motors for a spin

1 x lower quadcopter frame

1a CarbonAeronautics
Battery
4a Turnigy 2S 1300 mAh 3f
2 x propeller fastening screws
M2 x 8 mm

motor
3a GEPRC GR1105 5000 kV 3e counter-clockwise propeller
Gemfan 3018

make sure the fastening screws do

not touch the motor windings to
avoid destroying the motor
battery connector tape
2j XT60
4 x motor fastening screws
wire terminal strip
3b M2 x 4 mm
2p
Electronic speed controller
3c HobbyKing 6A ESC with BEC
LE
d

UP
dA
T E

B/
VC
CH

C
6
CH
5
CH
4
CH
PP

CH 1

3
m

2
/C
H

receiver
4b Flysky FS-iA6B
Radiotransmitter
5a Flysky FS-i6

sWB sWC
sWA sWd

receiver and bind plug

4b Flysky FS-iA6B
channel 3 channel 2
(throttle) (pitch)

channel 4 channel 1
LE
d

(yaw) (roll)
UP
dA
TE

B/
VC
CH

C
6
CH
5
CH
4
CH
PP

CH

3
m

2
/C
H1

UP ok
TX
RX
doWn CAnCEL

PoWER
TX.V1 : 4.80 V
IntV1 : 4.99 V
bind plug
sigs1 : 10
BInd kEy

52
Te st your radio, motors and ESCs
You will now test your radiotransmitter, receiver and motors. Use this oppor-
tunity to calibrate each ESC and verify that all motors spins in the correct
direction.

Each radio, ESC (Electronic Speed Controller) and motor combination needs to be
tested, calibrated and verified before you start soldering all parts together. Let’s start
with the first motor:

a. Set up your test stand

1. Slide a propeller down the motor shaft and push it firmly on the top of the
motor. For motor 1, you need a counter-clockwise propeller: the leading edge
of this propeller must be the first to turn counter-clockwise as shown on the
picture to the left.
2. Fasten the propeller with two long M2 screws but be sure that the screw does not
touch the inner motor windings.
3. Attach the motor to the drone frame with four short M2 screws.
4. Attach the drone frame firmly to your desk using tape.
5. Connect the three black motor wires using a wire terminal strip with the black,
blue and red cables coming out of the ESC. It does not matter yet which motor
wire is connected with which ESC wire.
6. Connect the red and black cable of the XT60 plug using another wire strip with
the red and black power wires of the ESC. Do not connect the battery yet.
7. Connect the white, red and black cables with channel 3 of the receiver as visual-
ised on the picture.

b. Bind the receiver to the radiotransmitter through the bind plug

1. Make sure the throttle stick on the radiotransmitter is in the lowest possible
downward position.
2. Turn on the POWER button of your radiotransmitter while simultaneously
holding the BIND KEY button. The text RX Binding... should be displayed.
3. Connect the bind plug with the B/VCC pins on the receiver as shown on the
picture. Keep the connection between the receiver and ESC in place.
4. Connect your battery using the XT60 plug. The red LED on the receiver should
illuminate and your radiotransmitter should beep one time, indicating that bind-
ing is successful. The text SigS1 on the radiotransmitter confirms binding of the
receiver. Disconnect the battery again.
5. Remove the bind plug. When connecting the battery again, your radiotransmitter
should automatically connect to your receiver.
53
 Take your motors for a spin

c. Test your motor and verify its turning direction

1. Turn on the radiotransmitter through the POWER button.
2. Connect your battery using the XT60 plug. You should hear one beep from your
radiotransmitter indicating that it is connected with the receiver, and subsequent-
ly two beeps from the motor to indicate that you are connected to a 2S battery
followed by two more beeps from the motor indicating that the ESC preparation
is completed.
3. Now slowly increase the throttle stick on your radiotransmitter to turn on the
motor and spin it at increasing speeds.
4. Verify that the propellers are spinning in the correct direction (motors 1+3:
counter-clockwise, motors 2+4: clockwise). If they do not spin in the correct di-
rection, remove the battery and switch two of the ESC wires going to the motor
with each other to reverse the spinning direction. Note: if you connect the black,
blue and red ESC wires in the correct order with the black motor wires as shown
in the figure, the spinning direction will always be correct.

d. Calibrate the ESC

ESC calibration always necessary to be able to control your quadcopter. Through cali-
bration, you tell the ESC what the upper and lower positions of the radiotransmitter
sticks are. Carry out the calibration with the help of the following steps:
1. Make sure your battery is not plugged in and the radiotransmitter is turned off.
2. Turn on the radiotransmitter and put the throttle stick to its uppermost position.
When connecting the battery, this will make sure the ESC goes in programming
mode.
3. Do not connect the battery yet but first read these instructions: after con-
necting the battery, you will first hear one beep from the radiotransmitter, next
you hear subsequent beeps from the motor. You will need to move the throttle
stick to its lowest position between the first and the fourth beep of the mo-
tor! If you are too late, do not attempt anymore to lower the throttle stick but
just disconnect the battery and try again starting from step 1.
4. When you understand the instructions from step 3, connect the battery, wait
until the beep from the radiotransmitter has passed and lower the throttle stick
to its lowest position between the first and the fourth beep of the motor.
5. After two seconds, the motor should give once again two times two beeps indi-
cating that the calibration is finished. Congratulations, you finished your motor
setup! Go once again to step c to test the throttle response.

c. Testing motors 2 to 4
Steps a, c and d need to be carried out for the other motors and ESC too. Try to
reverse the spinning directions of some motors by switching the ESC wires
and changing the propellers.

54
 clockwise propeller leading edge for counter-clockwise
3d Gemfan 3018R
propeller

counter-clockwise propeller
3e Gemfan 3018
leading edge for clockwise
propeller

switch two wires to reverse

motor direction

ESC programming
Not only ESC calibration can be carried out by putting the ESC into program-
ming mode, but other settings can be adjusted as well. The first four beeps are
followed by four beeps with a slightly different noise, indicating a different setting.
By moving your throttle stick down during these beeps, you can choose to activate
the corresponding setting. Through this method, you can choose from multiple
settings; more information can be found in the datasheet of your ESC.

55
Ca rbon aeronautics
 Take your motors for a spin

ESC control through PWM

You control the speed of your motor through the ESC, which in turn gets its
commands from channel 3 (the throttle channel) of your receiver. The receiver
sends Pulse Width Modulated (PWM) signals to the ESC to this channel, indicat-
ing a desired throttle value between 0 and 100%. But what is a PWM signal, really?

In essence, a PWM signal is just a signal that varies between a HIGH voltage (for
example 5V) and a LOW voltage (for example 0V), or between 1 and 0 to keep
the concept simple. It is the time length during which the signal stays HIGH that
is used to transfer information. Suppose for example that a time length of 1 ms
HIGH corresponds with no throttle (0%) and a time length of 2 ms HIGH cor-
responds with full throttle (100%). The PWM frequency of most receivers and
ESCs is 4 ms (or 1/0.004=250 Hz), meaning that your chosen signal repeats itself
every 4 ms with a length that corresponds with your throttle command. This con-
cept is illustrated in the figure below and will be used later on to send commands
to your ESCs using your Teensy instead of directly from the receiver.

Sample each
4 ms Full throttle

100%

Throttle 50%

4 ms = 1/250 Hz
0%
No throttle Time

PWm signal
1.5 ms 1.5 ms 2 ms 1 ms

0
Time

56
57
Ca rbon aeronautics
 Project 7
Receiving commands

Sample each
4 ms Full throttle

100%

Throttle 50%

4 ms = 1/250 Hz
0%
No throttle Time

PWm signal
1.5 ms 1.5 ms 2 ms 1 ms

0
Time

PPm signal
1.5 ms 1.5 ms 2 ms 1 ms

0
Time

58
 Pr ocess commands sent to the receiver
The commands that you give through your radio controller are transmitted
via radio waves and picked up by your receiver. The receiver then converts the
radio waves to a signal which can be read by your microcontroller. You will
now learn how to convert these signals from the receiver to variables that can
be used further in the flight code.

There are multiple methods to transfer information through digital signals, one of
which you already learned: Pulse Width Modulation (PWM). PWM is an easy method
to send information of one radiochannel from the receiver to the microcontroller.
However, receiving information from multiple radiochannels would require one sig-
nal cable to the microcontroller for each channel. This is cumbersome when you need
a lot of channels, meaning you will have to master another technique: Pulse Position
Modulation (PPM).

You already saw that you can use the width of a PWM signal to transfer information:
a signal width of 1 ms (=1000 µs) corresponds for example with no throttle (0%) and
a width of 2 ms (=2000 µs) corresponds with full throttle (100%). Using the tech-
nique of Pulse Position Modulation, you are able to transfer the same information
using the position of the signal in time, instead of the width. With PPM, the width of
each signal stays the same but its position changes each time depending on the value
of the radiochannel.

An example is displayed to the left: the first throttle value sampled from the radio-
channel is equal to 50%. Using PWM, this corresponds to a signal width of 1.5 ms
(=1500 µs). With PPM, the signal starts after 1.5 ms and has the same width each
time. When 4 ms have passed, another throttle value is sampled and the cycle begins
again.

Setting up your radiotransmitter in PPM mode

The standard operating mode of your transmitter is PWM, not PPM! This means
you have to configure the transmitter to use PPM for this project with the following
steps:
Switch on the transmitter → hold the OK button for two seconds → choose Sys-
tem → go down and choose "RX setup" → go down and choose "Output mode"
→ choose "PPM" and hold the CANCEL button for two seconds to save your
choice.

59
 Receiving commands

Now what about receiving information from multiple radiochannels? Consider a

two-channel example: you want to receive information about the throttle and the
pitch input. These inputs are independent of each other and require two separate
signal cables if you would use PWM. When receiving the two PWM signals in the
microcontroller, you will need two different analogRead() commands. Because they
cannot be processed simultaneously in the microcontroller, either the throttle or the
pitch values will be sampled (=read) first, after which the other signal follows directly.
Besides the need for two signal cables, this also requires more calculation time from
the microcontroller.

You are now ready to take advantage of the interesting property of PPM: because
only the position of the signals changes and not their width, sampling both the throt-
tle and pitch signals directly after each other allows you to keep track of their original
values by measuring the time from each rising signal value. This means that with one
signal cable, information from multiple signals can be ‘transported’.

+ - a b c d e f g h i j + -
1 1
2 2
3 3
4 4
LE
d

5 5
6 6
UP
dA
TE

7 7
B/
VC

8 8
CH

C
6
CH
5
CH

9 9
4
CH
PP

CH

3
m

10 10
/C
H1

11 11
12 12
jumper wires
2q
13 13
14 14
15 15 male to female 10 cm
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

16 16
17 17
18 18
19 19
20 20
21 21
22 22
23 23 Receiver B/Vcc
2 3 4

24 24 CH6
25 25
CH5
3V

26 26
27 27 CH4
0

28 5V 28 CH3
G

29 29 CH2
30 30 G Vcc PPM/CH1
+ - a b c d e + -
f g h i j
Radiotransmitter
5a Flysky FS-i6
Teensy
14 11
15 10
16 9
17 8
18 7
sWB sWC 19 6
sWA sWd
20 5
21 4
channel 3 channel 2 22 3
(throttle) (pitch) 2
3V
channel 4 channel 1 0
(yaw) (roll) 5V G

UP ok
TX
RX
doWn CAnCEL

PoWER
TX.V1 : 4.80 V
IntV1 : 4.99 V
sigs1 : 10
BInd kEy

60
 Sample throttle
100% 30°

Throttle Pitch
input 50% 0° input
Sample pitch

0%
-30°
Time

PWm signal
throttle 1.5 ms 1.5 ms 2 ms

0
Time

PWm signal
pitch 1 ms 1.5 ms 1.75 ms

0
Time

1
1.5 ms 1.5 ms 2 ms
PPm signal
throttle & pitch 1.75 ms
1 ms 1.5 ms

0
Time
Using this knowledge, you can now easily built the connection from the receiver to
the Teensy. Channel 1 of the receiver has also "PPM” written on it, which means that
you can use these connectors to send PPM signals to your microcontroller. Use a
cable to connect the second connector of the receiver starting from above to pin 14
of the Teensy as displayed on the figure to the left. Connect the third and fourth con-
nector of the receiver to 5V and ground on the Teensy respectively. This will ensure
that the receiver is fed from the microprocessor. You are now ready to start coding.

61
Ca rbon aeronautics
 Receiving commands

Coding
The code for handling Pulse Position Modulation is rather complex, so you again
use a predefined library called PulsePosition.h. This library is normally already in-
stalled when you installed Teensyduino, but you can still install it at this point with
the "manage libraries tool" like you already did with Wire.h. Next you create a PPM
input object, which is in this case the receiver input. You track each pulse starting
from their rising edge.

Use two global variables for this project: one array which can store up to eight chan-
nel values (initialized as eight zeroes) and one integer that stores the number of chan-
nels transmitter by the receiver.

To be able to read the receiver data multiple times in the code, you create a function.
This function will first check how many channels are available by writing .available
behind the PPM input object; if there are channels available, it reads the value of each
channel and stores it in the array of receiver values.

Radiotransmitter
5a
Read the values sent from the receiver by calling the function defined in line 5. Print
Flysky FS-i6
the available number of channels fol-
lowed by the values for each channel.

sWA
sWB sWC Channels 1, 2, 3 and 4 correspond respec-
sWd

tively with the roll, pitch, yaw and throttle

channel 3 channel 2 inputs. Because the array numbering in
(throttle) (pitch) the Arduino IDE starts with 0 instead of
channel 4 channel 1 1, ReceiverValue[0] actually corresponds
(yaw) (roll) with channel 1 or the value for roll.

UP
TX
ok Finally, you have to use a delay of 50
doWn
RX
CAnCEL

PoWER
milliseconds to be able to read the values
TX.V1 : 4.80 V
IntV1 : 4.99 V
sigs1 : 10
that will be displayed on the serial mon-
BInd kEy itor.
62
1 #include <PulsePosition.h> Use the PulsePosi-
2 PulsePositionInput ReceiverInput(RISING); tion library

3 float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0}; Declare the variables

4 int ChannelNumber=0; to store channel info

5 void read_receiver(void){ Define a function to

6 ChannelNumber = ReceiverInput.available(); read the receiver data
7 if (ChannelNumber > 0) {
8 for (int i=1; i<=ChannelNumber;i++){
9 ReceiverValue[i-1]=ReceiverInput.read(i);
10 }
11 }
12 }
13 void setup() {
14 Serial.begin(57600);
15 pinMode(13, OUTPUT);
16 digitalWrite(13, HIGH);
17 ReceiverInput.begin(14);
18 }
19 void loop() { Read and display the
20 read_receiver(); PPM information on
21 Serial.print("Number of channels: "); the serial monitor
22 Serial.print(ChannelNumber);
23 Serial.print(" Roll [µs]: ");
24 Serial.print(ReceiverValue[0]);
25 Serial.print(" Pitch [µs]: ");
26 Serial.print(ReceiverValue[1]);
27 Serial.print(" Throttle [µs]: ");
28 Serial.print(ReceiverValue[2]);
29 Serial.print(" Yaw [µs]: ");
30 Serial.println(ReceiverValue[3]);
31 delay(50);
32 }

63
Ca rbon aeronautics
 Receiving commands

Testing
Now you are ready to read the data send from the transmitter:
• Connect the Teensy to your computer through the USB cable;
• Upload to code to your Teensy and open the serial monitor.

If the radio transmitter is not yet switched on, the LED on the receiver should blink.
The values displayed on the serial monitor will all be zero, except for the number of
channels, which should be equal to -1. This is the default value when no channels are
discovered.

When you switch on your radio transmitter, the LED on the receiver should stop
blinking and the transmitter commands should be displayed on the serial monitor in
microseconds [µs]. Since the roll, pitch and yaw sticks are always centred, the values
you record will be around 1500 µs for the corresponding channels.

Now change the positions of the roll, pitch, throttle and yaw sticks and verify that
they vary between 1000 and 2000 µs. You have now successfully made a radio-con-
nection between your transmitter, the receiver and microcontroller!

Switching your radiotransmitter back to PWM mode

For the next projects, the receiver needs to stay in PPM mode. However, if you
want to redo the previous project in which you controlled one motor and one ESC
through PWM without a microcontroller in between, do not forget to switch your
radiotransmitter back to its default PWM setting. This can be carried out using the
same procedure as switching to PPM mode, which you learned at the start of this
project.

64
11:26:52.994 -> Number of channels: -1 Roll [µs]: 0.00 Pitch [µs]: 0.00 ...
11:26:53.041 -> Number of channels: -1 Roll [µs]: 0.00 Pitch [µs]: 0.00 ...

11:26:53.089 -> Number of channels: 8 Roll [µs]: 1499.97 Pitch [µs]: 1498.99 ...
11:26:53.135 -> Number of channels: 8 Roll [µs]: 1498.96 Pitch [µs]: 1500.00 ...
11:26:53.181 -> Number of channels: 8 Roll [µs]: 1498.96 Pitch [µs]: 1496.99 ...

65
Ca rbon aeronautics
 Project 8
Controlling your motors

+ - a b c d e f g h i j + -
1 1
2 2
3 3
4 4
5 5
LE
d

6 6
UP

7 7
jumper wires
dA

2r
TE

8 8
B/
VC
CH

C
6

9
CH

9
male to male 10 cm
5
CH
4
CH
PP

CH

10 10
m

2
/C
H1

11 11
12 12
13 13
14 14
15 15
22 21 20 19 18 17 16 15 14

16
5 6 7 8 9 10 11

16
17 17
18 18
19 19
20 20
21 21
22 22
23 23
2 3 4

24 24
25 25
26
3V

26
27 27
0

jumper wires
2q
28 5V 28
G

29 29

+ -
30
a b c d e f g h i j
30
+ -
male to female 10 cm

66
Us e PPM to control motor speed
As the name implies, an electronic speed controller is able to control the speed
of your brushless motor. In this project, you will learn how to send commands
to this controller and thus the motor through your microcontroller.

The electronic speed controller (ESC) is like your Teensy a microcontroller, but it
has only one purpose: adapting the voltage going to the motor in such a way that the
motor changes its speed. The speed at which a brushless motor turns depends on the
supplied voltage, according to the kV constant. A motor rated at 4000 kV turns at
4000 rpm/V. A supplied voltage of 3V gives a turning rate of 4000 rpm/V x 3V=12
000 rpm. If you would directly connect your battery to the brushless motor, it would
turn at a constant speed, as the voltage supplied by the battery does not change.

So how does the ESC adapts the voltage supplied to the brushless motor? By closing
and opening the connection between the battery and the brushless motor, you can
change the average voltage supplied to the motor. The longer the connection stays
closed, the higher the supplied average voltage will be (with the maximum being the
voltage supplied by the battery if the connection remains closed the whole time). You
can control this average voltage through pulse-width modulation. This means that
you need to sent a PWM signal from the Teensy to the electronic speed controller in
order to control the brushless motor.

You will now control the first brushless motor through the physical circuit displayed
on the left;
• Use some tape to make sure that the motors do not lift off.
• Re-use the same circuit with whom you already connected the receiver when
testing the motors.
• Connect the three wires coming out of the other side of the ESC to the wires of
the brushless motors using the wire terminal strip.
• Connect the power output of your ESC with the XT60 cables using another wire
terminal strip. Be careful not to switch the polarity of the cables!
• Connect the white signal cable from the ESC to pin 1 of the Teensy.
• Connect the 5V and 0V of the ESC to 5V and GND of the Teensy respectively.
Besides the power circuit going to the motor, your ESC has a second circuit that
stays at 5V all the time and which enables you to power both the motors and your
electronics with the same battery.

67
 Controlling your motors

Only connect the battery once you uploaded the code to the Teensy and disconnect-
ed your USB cable from your computer (as a precautionary measure if something is
wrong with the wiring).

Make sure you use motor 1 (turning counter-clockwise) for this project. If you notice
during testing that it spins clockwise instead of counter-clockwise, just switch two
wires coming out of the motor with each other using the wire terminal strips.

In the code, you will control the brushless motor using a PWM signal sent from the
Teensy to the ESC but you will use a PPM signal coming from the receiver to the
Teensy. The throttle (so channel 3 or ReceiverValue[2] in Arduino language) will be
used to set the speed of the brushless motor. The PWM values that you send to the
ESC are the same as the PWM values that you get from the receiver: 1000 µs gives no
throttle (motor does not turn), while 2000 µs gives full throttle.

Coding

Define the value for the throttle as a floating point number. The future value for this
variable lies between 1000 and 2000 µs.

You will send the PWM signals from pin 1 of your Teensy to motor 1. Configuring
pin 1 to send PWM signals can be done with the function analogWriteFrequency(pin
number, PWM frequency). The PWM frequency used in most ESCs is equal to 250
Hz (=1/250=0.004 s=4000µs). This value can be found in the manual of your ESC.
68
 motor 1
counter-clockwise

Front
2S LIPO Battery

XT60 plug

Teensy
Receiver B/Vcc 14 11
CH6 15 10
CH5 16 9
CH4 17 8
CH3 18 7
CH2 19 6
G Vcc PPM/CH1 20 5
21 4
22 3
2
3V
0
5V G

5V ESC 1
0V

1 #include <PulsePosition.h>
2 PulsePositionInput ReceiverInput(RISING);
3 float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0};
4 int ChannelNumber=0;

5 float InputThrottle; Define the throttle

variable
6 void read_receiver(void){
7 ChannelNumber = ReceiverInput.available();
8 if (ChannelNumber > 0) {
9 for (int i=1; i<=ChannelNumber;i++){
10 ReceiverValue[i-1]=ReceiverInput.read(i);
11 }
12 }
13 }
14 void setup() {
15 Serial.begin(57600);
16 pinMode(13, OUTPUT);
17 digitalWrite(13, HIGH);
18 ReceiverInput.begin(14);

19 analogWriteFrequency(1, 250); Send PWM signals

from your Teensy

69
Ca rbon aeronautics
 Controlling your motors

By default, the resolution of PWM signals sent by the Teensy is 8 bit. This means that
the signal ranges between 0 and 28-1=255. For our application, this would give a too
coarse control, so you will change from an 8 bit to a 12 bit resolution; the PWM signal
going from the Teensy to the ESC ranges between 0 and 212-1=4095. For a frequency
of 250 Hz=4000 µs, 0 then corresponds with 0 µs and 4095 corresponds with 4000
µs. When you want to sent a PWM command in µs to the ESC, do not forget to mul-
tiply that value with 4095/4000=1.024.

SAFETY RELATED LINES: in order to avoid a motor start when you did not yet
touch the radiotransmitter (for example when the throttle stick was not in the lowest
position after your last flight), you add some additional lines before finishing the setup
process. If the values sent from channel 3 (ReceiverValue[2] = the throttle channel)
are bigger than 1050 µs or lower than 1020 µs, the code will not continue. When you
move the throttle stick around its lowest value range (between 0.2 and 0.5%), the code
continues and you can start the motor.

You already know that the throttle corresponds to channel 3 or ReceiverValue[2]

from the radiotransmitter. The value of the throttle ranges from 1000 (no throttle) to
2000 µs (full throttle). You send this value to pin 1 and subsequently also the ESC and
motor 1 through the analogWrite function. Remember to convert the throttle values
in µs to their 12 bit equivalent by multiplying them with 1.024.

Testing
Once you uploaded the code successfully, disconnect the USB from the Teensy and
connect the battery. Normally the LEDs on both the Teensy and the receiver should
be illuminated as they receive power from the battery. Next, switch on your radio-
transmitter (remember it should still be in PPM mode, not PWM); you should hear
two times two beeps from your motor. Now slowly increase the throttle on your
radiotransmitter to start the motor. Verify that the motor spins in the correct (coun-
ter-clockwise) direction.

Troubleshooting when the motor does not start:

• Verify that your receiver and Teensy get power by looking at their LEDs.
• Verify that your radiotransmitter is connected with your receiver (the LED on
the receiver should not blink but should stay illuminated).
• Verify that the setting of your radiotransmitter is correct (in PPM mode).
• Verify the connections to the motor.

70
20 analogWriteResolution(12); Set the PWM fre-
21 delay(250); quency

22 while (ReceiverValue[2] < 1020 || Avoid uncontrolled

ReceiverValue[2] > 1050) { motor start
23 read_receiver();
24 delay(4);
25 }
26 }

27 void loop() { Send the throttle in-

28 read_receiver(); put to the motor.
29 InputThrottle=ReceiverValue[2];
30 analogWrite(1,1.024*InputThrottle);
31 }

71
Ca rbon aeronautics
 Project 9
Battery management

2S LIPO Battery

tab
XT60 plug

High-side
switch

1 3 5 7
2 4 6

2 kΩ
Diode
Slide switch

on

Teensy off
Zener
14 11 510Ω diode
15 10
16 9
17 8
18 7
19 6
20 5
21 4
22 3
2
3V
0
5V G

510Ω
5V ESC 1
0V

72
Mo nitor and protect your battery
Batteries store energy - a lot of energy. When this amount of energy is acci-
dentally released in a short period of time, due to for example a short circuit,
it can cause significant damage. Monitoring and closely protecting the battery
is therefore very important.

A first obvious feature for each battery management system is a switch: you want to
be able to turn off the power from the battery towards your motors at all time, even
during full throttle. A standard (breadboard) slide switch can only switch off currents
less than 1 A at 12 V; these type of switches are insufficient for our application: at full
throttle, the current drawn by all four motors can easily surpass 20 A.

Additionally, you want some form of battery protection as well. When the battery
is accidentally short-circuited, all the energy in the battery will be released nearly in-
stantaneous. This causes the point with the highest resistance in the short-circuit to
heat up and start burning; this can be a cable, a trace on the printed circuit board or
the battery itself. The burning of the cable or the trace will cause the short circuit to
disappear because the conductive band is broken, at the cost of destroying the print-
ed circuit board. But when the battery itself has the highest resistance in the short
circuit, it can burn and explode causing further damage.

A third feature is some form of current measurement to be able to monitor the

battery level more closely - more about this will be explained further in this project.
For your quadcopter application, you use a special transistor that integrates all of the
above features: a high side power switch. This type of transistor is placed between
the positive side of the battery and the load (hence the name high side). An Infineon
BTS50055-1TMB or BTS50080-1TMB transistor will be used for this application
because these switches are capable of conducting rather high load currents of respec-
tively 70 and 37.5 A.

To be able to conduct these high loads, you need to use the tab of the high side
switch, then connect output pins 1, 2, 6 and 7 with each other before making the
connection with the load (which will be the ESCs in this case). You will connect pin
3 with a slide switch that is in turn connected with the high-current GND line; the
transistor is switched on when a current flows between pin 3 and the GND. When the
transistor is switched off, the voltage between pin 3 and the GND almost equals the
battery voltage (maximally 8.4V for a 2S battery). The transistor has the capability to
switch off the load current at full throttle, so you can instantaneously switch off your
quadcopter at full power using the slide switch.
73
 Battery management

A downside of this high load current is the very high short circuit current limit: up to
180 A. This means that a short circuit in your printed circuit board can nonetheless
cause serious damage!
Monitoring the current
You already learned that measuring the voltage of your battery enables you to predict
its remaining lifetime. Whenever you put the battery under heavy load, which will be
the case when you throttle up the motors, the voltage will drop significantly without
a real decrease in battery level, reducing your capability to accurately monitor the
remaining battery level during flight. The only way to overcome this problem is to
directly measure the current drawn from the battery.

A special feature of your Infineon transistor is its current sensing capability: a current
that is smaller but proportional to the load current flows from output pin 5 of the
transistor. According the datasheet of the transistor, the current sense ratio is typi-
cally equal to 14 000. This means that a load current of 14 A through the transistor
corresponds with a current of 1 mA from pin 5. With your Teensy, you cannot meas-
ure a current, only voltages. That is why you use a resistor to convert the current to
a measurable voltage. Using Ohm’s law, you know that a current of 1 mA through a
resistor of 510 Ω will cause a voltage drop over the resistor of 1 mA x 510 Ω = 510
mV or 0.51 V. In other words, a current of 14 A through the transistor corresponds
to a voltage drop over the resistor of 0.1 V. Assuming this remains proportional over
the full measuring range gives a voltage to current ratio of 0.51 V/14 A or 0.036 V/A.

You will connect the 510 Ω resistor to pin 21 of your Teensy for voltage measure-
ment. Before wiring everything up, you also need to consider the effect of a short
circuit in the high current part of your current sensing circuit. In the event of a short
circuit, the load current can easily exceed 180 A for a couple of milliseconds before
the transistor switches off. Such a high current will lead to a high voltage over our
resistor: 180 A x 0.036 V/A = 6.5 V. Knowing that the input pins of our Teensy are
only 3.3V-tolerant, a voltage this high will probably destroy the microcontroller. To
solve this issue, you will use a Zener diode. A Zener is a special diode that does not
conduct any current below a certain fixed voltage, the "Zener voltage". For this pro-
ject, you use a diode that has a Zener voltage of 2.4 V and needs a minimum current
of 5 mA to start conducting at the right time. Adding the Zener diode in parallel to
the resistor will cause the voltage over your Teensy to never exceed 2.4 V, protecting
the microcontroller in the event of a short circuit on the load side. Of course the
opposite can still happen: if the battery is connected in reverse and you have a short
circuit, the Zener diode will not protect the Teensy because there is no "negative”
Zener voltage limit. That’s why you add a normal diode as well to prevent any nega-
tive voltage occurring over the Teensy.

74
Beware - you are not protected from all type of short circuits!
The installation of high side load switch does not prevent all types of short circuits
and hence damage to your battery and/or quadcopter can still occur. Therefore,
always make sure that you do not have any shorts in your circuit using your multim-
eter before connecting the battery. One example of a short circuit that the switch
does not protect against is visualized in the figure below. Your quadcopter comes
with a connection for a video transmitter (VTX), in case you want to add an FPV
camera. This VTX connection is directly connected to the positive and negative
power lines but it has a much smaller trace width than the power lines. The VTX
traces are therefore not suited to accommodate high currents, and certainly not a
short circuit current that can exceed 180 A before it is shut off by the high side load
switch. This means that if you would try to create a short circuit by connecting the
VTX Vbat and GND lines to each other and subsequently connect the battery, the
traces on your power distribution board will start to burn and be destroyed.

Power distribution VTX

short circuit
trace trace

Voltage at Teensy pin 21 [V]

3 zener voltage
2.4 V
2
0.036 V/A with
510 Ω resistor
1

0
0 20 40 60 66 A 80
Current through power switch [A]

75
Ca rbon aeronautics
 Battery management

Combining current and voltage for battery monitoring

It is not possible to test the current measurement on a solderless breadboard, since
it requires currents too high for the traces on the breadboard. Instead, this part will
explain the necessary coding and reasoning for accurate battery monitoring. All lines
will be directly implemented in the flight controller and can be tested once the quad-
copter is fully constructed.

The variables necessary for accurately monitoring the battery capacity - besides the
voltage and current - are the remaining battery capacity, the battery capacity at start,
the current that you have consumed during flight and the default battery capacity.
All battery capacity variables have the unit mAh; a battery of 1300 mAh can sustain
a current of 1 A or 1000 mA during a period of 1300 mAh/1000 mA = 1.3 hours.
The default battery used in this project has a capacity of 1300 mAh; initialize the
BatteryDefault variable with this number.

The current can be read from the voltage of pin 21 using the function analogRead.
Remember that the default resolution for analogRead is equal to 10 bit, so a voltage
of 0 V gives you the digital number 0 and the maximal input voltage of 3.3 V gives
the digital number 210-1=1023. Moreover you have built a system for which you have
a voltage of 0.036 V for each Ampere flowing through the power switch. This means
that de current flowing through the power switch is equal to the measured digital
number at pin 21 divided by ((1023 / 3.3 V) x 0.036 V/A). Or equivalently, multiply-
ing the measured digital number with 0.089.

When connecting the battery, you first need an indication of the actual battery capaci-
ty before you can calculate its evolution during flight. Luckily, the measurement of the
battery voltage using our voltage divider and pin 15 is highly accurate if the motors
are not started yet (and thus no voltage drop is present). In the setup phase, you hence
determine the battery level using pin 15. There is only a (quasi) linear relation of the
battery voltage to its capacity between 8.3 V and 7.5 V. If the voltage is higher than
8.3 V, you considered it to be at 100 % capacity (=1300 mAh) and you turn off the
red LED. If the voltage lies below 7.5 V, you consider the battery to be at a capacity
of 30% or less and the red LED stays illuminated. The following linear approxima-
tion between voltage and capacity is valid for the 1300 mAh - 2S battery:
Remaining capacity [%] = 82 · Voltage − 580

This approximation can be extracted experimentally with a sophisticated battery

charger that indicates both the charged current and 1000the
mA
actual voltage and subse-
CurrentConsumed (k)[mAh] = CurrentMeasured (k)[A]· A
·0.004 s+CurrentConsumed (k−1)[mAh]
quently plot it in a graph, like the one on top of the3600 nexths page.

76
 Battery voltage
Fully charged: 8.4 V Remaining battery = 82 . Voltage - 580
8.3 V
8V

Low battery: 7.4 V

Critically low: 7.2 V
7V

100% 80% 60% 40% 30% 20% 5% 0%

Remaining battery energy

1 float Voltage, Current, BatteryRemaining, BatteryAtStart; Define the battery

2 float CurrentConsumed=0; monitoring variables
3 float BatteryDefault=1300;

4 void battery_voltage(void) { Measure the voltage

5 Voltage=(float)analogRead(15)/62; and current of the
6 Current=(float)analogRead(21)*0.089; circuit
7 }

8 void setup() { Determine the bat-

9 digitalWrite(5, HIGH); tery capacity prior to
10 battery_voltage(); flight
11 if (Voltage > 8.3) { digitalWrite(5, LOW);
12 BatteryAtStart=BatteryDefault; }
13 else if (Voltage < 7.5) {
14 BatteryAtStart=30/100*BatteryDefault ;}
15 else { digitalWrite(5, LOW);
16 BatteryAtStart=(82*Voltage-580)/100*
17 BatteryDefault; }
18 }

77
Ca rbon aeronautics
 Battery management

During flight, you use the measured current to follow the evolution of your battery
capacity. SinceRemaining
each iteration k in[%]
capacity our= main loop −
82 · Voltage takes
580 0.004 seconds, the consumed
current can be calculated with the formula:

1000 mA
CurrentConsumed (k)[mAh] = CurrentMeasured (k)[A]· A
·0.004 s+CurrentConsumed (k−1)[mAh]
3600 hs

The remaining capacity is subsequently calculated by subtracting the consumed cur-

rent from the battery capacity at startup. When the battery capacity falls below 30%,
illuminate the red LED.

Operating the quadcopter without power switch

The high-side power switch is one of the more exotic components in this project and
can be hard to come by. Most quadcopters made by hobbyist do not contain such a
feature. Although it is not recommended, you can decide to exclude the power switch
by shorting the battery tab on the printed circuit board, as illustrated on the figure
to the right. Be aware that this removes any short-circuit protection and any control
over switching on and off your quadcopter, other than physically connecting or re-
moving the battery via the XT60 plug. The slide switch, (Zener) diodes and 510 Ω
resistor are not necessary anymore because you will not have any current sensing ca-
pabilities. This means that a voltage measurement is the only way of monitoring the
remaining battery energy. The code for this basic method is displayed below; once
the voltage drops below 7.5 V the red LED is illuminated. Remember that when
voltage drops during for example a sudden throttle increase, the measured voltage
does not give an accurate reflection of the remaining battery energy.
1 float Voltage;
2 void battery_voltage(void) {
3 Voltage=(float)analogRead(15)/62;
4 }
5 void setup() {
6 if (Voltage > 7.5) digitalWrite(5, LOW);
7 }
8 void loop() {
9 battery_voltage();
10 if (Voltage < 7.5) digitalWrite(5, HIGH);
11 else digitalWrite(5, LOW);
12 }

78
19 void loop() { Calculate the battery
20 battery_voltage(); capacity during flight
21 CurrentConsumed=Current*1000*0.004/3600+
22 CurrentConsumed;
23 BatteryRemaining=(BatteryAtStart-
24 CurrentConsumed)/BatteryDefault*100;
25 if (BatteryRemaining<=30) digitalWrite(5, HIGH);
26 else digitalWrite(5, LOW);
27 }

2S LIPO Battery

XT60 plug

shorting the power switch

1 3 5 7
2 4 6

2 kΩ

Teensy
14 11
15 10
16 9
17 8
18 7
19 6
20 5
21 4
22 3
2
3V
0
5V G

510Ω
5V ESC 1
0V

79
Ca rbon aeronautics
 2S LIPO Battery

tab
XT60 plug

High-side
switch
Receiver B/Vcc
motor 4 motor 1
CH6 1 3 5 7 clockwise counter-clockwise
CH5 2 4 6
CH4 Front
CH3 2 kΩ
CH2 Diode
G Vcc PPM/CH1
on

Teensy off
Zener

Slide switch
14 11 510Ω diode
MPU-6050 VCC 15 10
Project 10

GND 16 9
SCL 17 8
SDA 18 7
XDA 19 6
XCL 20 5 5V ESC 4
0V
ADO 21 4
INT 22 3
Assembling your quadcopter

2
BMP-280 VCC 3V 5V ESC 3
GND 0 0V
5V motor 3 motor 2
SCL G
counter-clockwise clockwise
SDA
CSB
SDC 510Ω 100Ω 100Ω 5V ESC 2
0V
Red Green
LED LED

5V ESC 1
0V

80
Bu ild your quadcopter
Now that you understand and tested all electronic components of your quad-
copter, it is finally time to put it all together! The lower quadcopter frame
will house all power electronics and the motors, while the upper frame mainly
holds the control electronics and the power distribution.

The upper part of the quadcopter frame houses all the connections necessary for
powering and controlling your quadcopter. In essence, it is a so called Printed Circuit
Board or PCB: this is nothing more than alternating layers of insulating material and
conductive copper. Inside your PCB, all necessary connections between the compo-
nents are provided in the form of conductive traces. This means that most wires in
the schematic of your quadcopter electronics on the left are already integrated in the
upper quadcopter frame. The upper and bottom layer of your upper frame PCB are
visualized below, together with the traces which are coloured similarly to the wires
in the schematic. Notice that the power traces are much wider than the signal traces.

81
 Assembling your quadcopter

You are now ready to start the quadcopter assembly. During the first two steps, you
position the four ESC and motors on the lower quadcopter frame and solder each of
the three wires coming out of the ESC and motors to the frame. To ensure a correct
rotation direction of the motors, make sure that the cables do not cross each other.
The black and red wires coming out of the ESC should match the colours indicated
on the frame itself.
ESC and spacer assembly
4 x frame spacers
1c M3 x 30 mm

4 x electronic speed controllers

3c HobbyKing 6A ESC with BEC

be carefull to position the EsC and

its wires correctly

the black wires are always positioned 1 x lower quadcopter frame

towards the inside of the frame and 1a CarbonAeronautics
the red wires towards the outside

4 x spacer fastening screws

1d M3 x 6 mm
Motor and propeller assembly
counter-clockwise propeller
3e Gemfan 3018

4 x motors
3a GEPRC GR1105 5000 kV

8 x propeller fastening screws

clockwise propeller 3f
3d Gemfan 3018R
M2 x 8 mm

clockwise propeller
the motor wires should not cross each other 3d Gemfan 3018R
to ensure a correct motor rotation

make sure the fastening screws do not touch the

counter-clockwise propeller
3e Gemfan 3018
motor windings to avoid destroying the motor
16 x motor fastening screws
3b M2 x 4 mm

82
Use the lower frame to hold the receiver and the actual battery. Attach the receiver
with a cable tie to the frame and provide a future connection for its signal and power
to the microcontroller using a jumper wire. Attach the battery to the frame with a
battery strap, making future battery replacement easy. Protect the cables coming from
the ESCs using cable protectors, which you cut in pieces such that they cover the
full length of the frame spacers. Cut some additional holes to allow the cables to go
the ESC itself and to the upper frame. Once finished, start mounting the necessary
headers and switches on the upper quadcopter frame.
Battery and receiver assembly

receiver jumper wires

2k female to female 10 cm

receiver
C
B/VC 6
CH
CH5
4b Flysky FS-iA6B
CH4
CH3

battery
CH2 1

4a
dATE CH
UP PPm/

LEd
Turnigy 2S 1300 mAh

5 x cable ties
1g 16 mm
cable protector
battery strap 1i
1e 210 mm
(cut in four pieces of 30 mm)

cut a hole in the lower & upper part of the cable

protector to allow the cables to pass through

Headers and switches assembly

upper quadcopter frame
1b CarbonAeronautics

female headers
2m 2.54 mm

male headers
2n 2.54 mm - right angle

power switch
2u slide switch 2t BTS50080-1TMB
OS102011MS2QN1C
83
Ca rbon aeronautics
 Assembling your quadcopter

Now solder the additional electronics to the upper quadcopter frame: the resistors,
diodes and battery connector. Assemble the orientation sensor and the barometer to
the frame using fastening screws, full nuts and locknuts. The use of the barometer will
be explained further in the project. Slide the microcontroller in the headers to finalize
the upper frame. To connect the upper to the lower frame, first solder the ESC power
cables to the upper frame then connect both frames using fastening screws to each
other with the spacers.
Electronics assembly

orientation sensor
2c GY-521 MPU-6050
microcontroller
2a Teensy 4.0
barometer
2d GY-BMP280
4 x sensor fastening screws
2e M3 x 20mm
resistors
2i 100 Ω (2x) + 12 x sensor full nuts
510 Ω (2x) + 2000 Ω (1x)
2g M3

4 x sensor locknuts
2l diodes
Zener diode (1x) + Diode (1x)
2f M3
green and red LEd
2h
battery connector
2j XT60

Upper and lower frame assembly

2 x cable ties
1g 16 mm
4 x standoff spacer (optional)
1h M3 x 20 mm
C
B/VC 6
CH
CH5

8 x spacer fastening screws

1d
CH4
CH3
TE CH2 1
CH
UPdA PPm/

LEd M3 x 6 mm

strip the EsC cables to fit through the upper

PCB holes and solder them from above

84
Testing
Now that construction is finished, it is time to check the correct wiring and solder-
ing of all components. Do this before connecting the battery. While this may
sound boring, it is an essential step after assembling any complex product and will
make troubleshooting in the next phases easier. A good test procedure would be to:
• Verify all connections using a multimeter and the schematic given at the be-
ginning of this project.
• Verify that there are no short circuits between wires/pins that should not be
connected with each other, also through the use of your multimeter. Pay extra
attention to the absence of shorts between the red and black wire of the XT60
battery connector.
• Next, apply power to the prototype board using the USB port of the Teensy.
Install the previous Arduino programs that you developed to illuminate the
LEDs, read the gyro data and read the receiver data. Verify that they function
correctly; this ensures a thorough check of your correct soldering and wiring.

When you are sure that there are no short circuits, connect the battery to continue
testing:
• Measure the battery voltage by installing the correct Arduino program.

In the last step, you will test the motors and their correct rotation direction. Con-
nect one ESC with channel 3 of the receiver, just like you did previously. Make
sure you configure your radiotransmitter back to PWM instead of PPM. Turn on
the radiotransmitter through the POWER button. Connect your battery with the
XT60 plug. You should once again hear one beep from your radiotransmitter which
indicates that it is connected with the receiver, and subsequently four beeps from
the motor. Now slowly increase the throttle stick and verify that the motor turns in
the required direction. Carry out the same test for all motors.

When (one of) the motors do not work, verify that:

• The battery is connected;
• The red LED on the receiver is illuminated continuously (a blinking led in-
dicates that the transmitter is not connected, no led means no power to the
ESC);
• The ESC is connected to channel 3 of the receiver;
• The transmitter setting is PWM instead of PPM.

If none of the above verifications solves the problem, verify that you soldered all
respective wires correctly and resolder where necessary.

85
Ca rbon aeronautics
 Project 11
Quadcopter dynamics

motor 4 z X motor 4 z X

throttle
y y input
motor 3 motor 3
motor 1 motor 1 output motor 1 = 50% power = 50%
output motor 2 = 50% power = 50%
output motor 3 = 50% power = 50%
motor 2 Hover in motor 2 output motor 4 = 50% power = 50%
same position

motor 4 z X

motor 4 z X y
motor 3
throttle
y input
motor 3
motor 1
output motor 1 = 75% power = 75%
motor 1 output motor 2 = 75% power = 75%
output motor 3 = 75% power = 75%
motor 2 Increase altitude output motor 4 = 75% power = 75%
along the z axis motor 2

Whenmotor you4 zwant to change the direction of your drone things become more tricky;
X
motor 4
X
assume you want the droneRolltoaround staythe at the same altitude but move sideways tothrottle
y
the
input
roll
input
y z
right
motor 3
(= roll around themotor
X axis).
1 X The
axis throttle input will be equaloutputto 50%motor 1 for
= 25%all motors
power = 50% - 25%
as you do not change altitude, but in order to initiate this motorsidewards
1 movement the
output motor 2 = 25% power = 50%
output motor 3 = 75% power = 50%
- 25%
+ 25%
power output of motors 3 and 4 (=the left motors) should be output
motor 2 motor 3
higher than the power
motor 4 = 75% power = 50% + 25%

output of motors 1 and 2 (=the right motors). This motor means 2 that you need an additional
roll input, which will lower the power of motors 1 and 2 with for example 25% and
at themotor
same4 ztime increase X the power ofmotor motors
3
3 and 4 with 25%. The same reasoning
holds for the pitch input and the
Pitch aroundyaw input, but with zother motor combinations as
throttle
input
pitch
input
y y
displayed in the figure motor
motor 3
on the
1 right.
the y axis
motor 2 output motor 1 = 25% power = 50% - 25%
output motor 2 = 75% power = 50% + 25%
output motor 3 = 75% power = 50% + 25%
A nice propertymotor
of 2this definition ofmotor
throttle,
4 roll, pitch and yaw input is that you output motor 4 = 25% power = 50% - 25%

can write all movements as a linear combination of eachmotorother,

1 for all motor outputs:
X

output
motor 4 z
motor 1 = throttle
X
input - roll input
z - pitch input - yaw input
X
output motor 2 = throttle input - motor roll 1input + pitch input + yaw input throttle yaw
y yaw around the input input
output motor 3 =motor
motor 3
throttle
1
input + roll input + pitch input
z axis - yaw input
output motor 1 = 25% power = 50% - 25%
output motor 4 = throttle input + roll input - pitch input + yaw
output motor input
2 = 75% power = 50% + 25%
motor 2 output motor 3 = 25% power = 50% - 25%
motor 2 output motor 4 = 75% power = 50% + 25%
motor 4
y 86
motor 3
 Le arn how your quadcopter travels in space
With all ingredients for a control system available and tested, it is time to learn
how a quadcopter moves through space with your inputs given to the radio-
transmitter.

You already know how to control the motors with your Teensy, how to measure the
rotation rate with the gyroscope and how to receive and read commands with your
radiotransmitter
z
and receiver. These are all zessential ingredients, but you still need to
motor 4 X motor 4 X
learn how they have to work together to be able to fly. throttle
y y input
motor 3 motor 3
motor 1 motor 1
The first thing you need to understand is how you can use the four motors 50% of your
output motor 1 = 50% power =
output motor 2 = 50% power =
50%
quadcopter to steermotor 2 it in the directions
Hover in you want.motor
You 2 do this by changing the50% power
output motor 3 = 50% power =
output motor 4 = 50% power =
50%
and thus rotation speed of same theposition
motors. Let's assume a perfect world for this project:
no wind disturbances, instantaneousmotor motor
4 z reaction and X a uniform weight distribu-

tion. motor
To let
4 z
your quadcopter X
hover in ythe same position, each motor will have to work
motor 3
at around 50% of its power as shown on the left figure. To increase the altitude throttle at
y
which
motor 3 your drone is flying, you can simply increase the power of all four motors to
motor 1
input
output motor 1 = 75% power = 75%
for example 75%. To keep the quadcopter level, it is important that all motors should
motor 1 output motor 2 = 75% power = 75%
output motor 3 = 75% power = 75%
increase their power
motor 2 at the same
Increase time in order motor
altitude to 2keep the outputquadcopter
motor 4 = 75% powerlevel.
= 75% The
along the z axis
command to keep all motors at the same power level will be called the throttle input.
motor 4
motor 4 z
X X
y throttle roll
y Roll around the z input input
motor 3 X axis
motor 1 output motor 1 = 25% power = 50% - 25%
output motor 2 = 25% power = 50% - 25%
motor 1
output motor 3 = 75% power = 50% + 25%
motor 2 motor 3 output motor 4 = 75% power = 50% + 25%

motor 2

motor 3
motor 4 z X
z throttle pitch
y Pitch around input input
y
motor 3 the y axis
motor 1 output motor 1 = 25% power = 50% - 25%
motor 2
output motor 2 = 75% power = 50% + 25%
output motor 3 = 75% power = 50% + 25%
motor 2 motor 4 output motor 4 = 25% power = 50% - 25%

motor 1
X

motor 4 z X z
X motor 1
throttle yaw
y yaw around the input input
motor 3
z axis
motor 1 output motor 1 = 25% power = 50% - 25%
output motor 2 = 75% power = 50% + 25%
motor 2 output motor 3 = 25% power = 50% - 25%
motor 2 output motor 4 = 75% power = 50% + 25%
motor 4
y
motor 3

87
 Quadcopter dynamics

In reality you do not send a power percentage to the motor but rather a PWM value
between 1000 and 2000 µs, where 1000 µs corresponds with 0% motor output and
2000 µs with 100% motor output. In your code, the throttle input will vary between
1000 and 1800 µs to leave 20% motor output available at all times for rolling, pitching
and yawing.

From receiver commands to desired rotation rates

The receiver sends commands to the microcontroller that vary between 1000 and
2000 µs according to the position of the radiotransmitter stick. For the throttle stick,
this corresponds nicely to 0 and 100% power. For the roll, pitch and yaw sticks, whose
default position is physically in the middle of the radiotransmitter at 1500 µs, you
need to transform the PWM values to physical rotation rates. You can choose your
maximal and minimal desired rotation rate; the higher the values the more agile your
drone will be, but also the harder to control. For now, take the limit values of 75 °/s
and -75 °/s. The transformation from the PWM values to the rotational rates is then
5a Radiotransmitter
visualised in the figure below together with the corresponding
Flysky FS-i6 linear correlation.

desiredRateRoll
desiredRatePitch
desiredRateyaw sWA
sWB sWC
sWd

desiredRate = 0.15 (ReceiverValue - 1500) channel 3 channel 2

75 °/s (throttle) (pitch)

channel 4 channel 1
(yaw) (roll)

0 °/s
UP ok
TX
RX
doWn CAnCEL

PoWER
TX.V1 : 4.80 V
IntV1 : 4.99 V
sigs1 : 10
-75 °/s BInd kEy

1000 µs 1500 µs 2000 µs ReceiverValue[0] (=channel 1)

ReceiverValue[1] (=channel 2)
ReceiverValue[3] (=channel 4)

88
 Transforming desired rotation rates to motor input?
You might think a second transformation is necessary: from the desired roll/
pitch/yaw rotational rates to the motor roll/pitch/yaw input. This is true, but it
is very difficult to measure which rotational rates correspond to a certain motor
power level. Moreover, even if you obtain accurate data for this transformation, a
huge problem remains; the reasoning in this project holds only for a quadcopter
in a perfect world. In the real world however, even small disturbances will desta-
bilize your quadcopter if you would introduce a fixed transformation.

For example, assume that the weight of your quadcopter is slightly higher in the
back than in the front. This means that in order to hover, the back motors will
need a slightly higher power output than the front motors. Any wind disturbance
during the flight will have to be corrected immediately by varying the output of
each motor accordingly. It is impossible to adjust for both imperfections using
manual corrections within normal human reaction times; these phenomena need
to be corrected by your fast microcontroller and a technique called PID feedback
control.

89
Ca rbon aeronautics
 Project 12
Quadcopter rate control

desired error between desired and motor power

rotation rate measured rotation rate command
+ Controller motor power
-

measured quadcopter
rotation rate rotation rate
gyroscope

Now suppose that the controller just consists of the difference between the desired
and the measured rotation rate, multiplied with a constant P:

Inputmotor = P · (DesiredRate − Rate)

Inputmotor = P · (DesiredRate − Rate)
By defining the error during
Input = P iteration
motor each · (DesiredRate
k of the
− control
Rate) loop:
Error(k) = DesiredRate(k) − Rate(k)
Error(k) = DesiredRate(k) − Rate(k)
Error(k) = DesiredRate(k) − Rate(k)
you can simplify the equation(k)rewriting
Input =P the =
(k) motor input during iteration k as:
P · Error(k)
motor term
Inputmotor (k) = Pterm (k) = P · Error(k)
Inputmotor (k) = Pterm (k) = P · Error(k)
 k·Ts
Inputmotor (k) = Pterm (k) + Iterm (k) = P · Error(k) + I ·  k·Ts Error(t) · dt
where Pterm is the proportional term of the controller. The response of such a con-
Inputmotor (k) = Pterm (k) + Iterm (k) = P · Error(k) + I · 0k·Ts Error(t) · dt
troller to a change in the desired rotation rate is
Inputmotor (k) = Pterm (k) + Iterm (k) = P · Error(k) + I ·
visualised 0on the graph to the right:
Error(t) · dt
the higher you choose the value for P to be, the faster the 0 actual rotation rate will
(Error(k) + Error(k − 1)) · Ts
approach
Inputmotorthe(k) desired
= P ·Error(k)+I
rotation rateterm (k−1)+I·
and the smaller the settling
(Error(k) + Error(ktime,
− which
1)) · Tsis a good
(k) = P 2
thing. However,
Inputmotor a ·Error(k)+I
larger P will termalso(k−1)+I·
give a larger overshoot,
(Error(k) meaning
+ Error(k
2 − that
1)) · Tsthe quad-
Inputmotor
copter might (k)bounce
= P ·Error(k)+I term (k−1)+I·
violently when changing the desired rotation rate. Whatever the
2
value of P thereInput
might also
motor (k)be=a Psteady state
term (k) error:
+ Iterm (k)the
+D actual rotation rate never reach-
term (k)
es the desired rotation (k)You
rate.
Inputmotor = Povercome
term (k) + Ithis
term (k) + by
issue (k) an integral term: this
adding
Dterm
term will sum the past
Input (k) =hence
errors
motor (k) + Iterm (k)
Ptermeliminating the+steady
Dterm (k)
state error.
d
Inputmotor (k) = Pterm (k) + Iterm (k) + D · d Error(t) 90
Inputmotor (k) = Pterm (k) + Iterm (k) + D · dt Error(t)
d
Inputmotor (k) = Pterm (k) + Iterm (k) + D · dt Error(t)
dt
Le arn how to stabilize your quadcopter
With normal human reaction times, it is not possible to keep a quadcopter sta-
ble in the air. In this project you learn how to use a fast control loop to stabilize
the quadcopter automatically while also taking into account the commands
you give it with the radiotransmitter.

To stabilize your quadcopter, you need to use a very fast automated control loop
that sends new commands to each of the four motors, multiple times per second.
The control system that you will use for your quadcopter will be a 250 Hz system;
this means that every 1/250 = 0.004 seconds, all four motors will receive new com-
mands. These commands are generated depending on the commands you give your-
self through the radiotransmitter, but are also generated automatically based on the
actual rotation rate of the quadcopter in space, which is measured by your gyroscope.

The closed control loop that you will use for the roll, pitch and yaw rotation rates is
displayed on the left. You use the gyroscope sensor to measure the actual rotation rate
of the quadcopter and compare it to the desired rotation rate, which you have sent
from the radiotransmitter. The error between both is transformed by the controller
to a motor power command that is sent to each of the four motors. The resulting
change in motor power changes the rotation rate of the quadcopter to a value that
should be closer to the desired rotation rate than before. The actual rotation rate is
measured once again and the process restarts. Each loop occurs every 0.004 seconds
during flight.
Rotation rate

desired rotation rate

(desiredRate)

30 °/s
overshoot steady state error

actual rotation rate

0 °/s (Rate) - P controller

settling time

0s 1s 2s 3s Time

91
 Error(k) = DesiredRate(k) − Rate(k)
Input motor=
Error(k) =DesiredRate(k)
P · (DesiredRate
−− Rate)
Rate(k)
Quadcopter rate control
Input motor(k)
Inputmotor = P= ·P(DesiredRate
term (k) = P · Error(k)
− Rate)
The addition of the integral term=can be implemented in the control equation through:
motor (k) = Pterm (k) = P · Error(k)
Error(k)
Input DesiredRate(k) − Rate(k)
 k·Ts
Inputmotor (k) = PError(k) = DesiredRate(k) − Rate(k)
term (k) + Iterm (k) = P · Error(k) + I · Error(t) · dt
Inputmotor = P · (DesiredRate − Rate)  0k·Ts
Inputmotor (k) = Pterm (k) = P · Error(k)
Input
where Ts is(k)
themotor the=length
Pterm (k)
of +
one Iterm (k) = P0.004
iteration, · Error(k) + I 250
s for our · HzError(t)
0
· dt Dis-
control loop.
cretization of the integral
Inputcan easily
(k) =be done
Pterm = (Error(k)
(k)through: + Error(k − 1)) · Ts
P · Error(k)
Inputmotor (k) = P ·Error(k)+I
Error(k) = DesiredRate(k)
motor
term (k−1)+I·
− Rate(k)
2 ss
 k·T
Inputmotor (k) = Pterm (k) + Iterm (k) = P · Error(k) (Error(k) + I+· Error(k − 1)) · dt
Error(t) Ts
Inputmotor (k) = P ·Error(k)+Iterm (k−1)+I· 0
2s
 k·T
Inputmotor Input(k) = Pterm
Input
motor (k)(k) +
=(k) I=
Pterm
termP(k)
(k) =P = ·PError(k)
· Error(k) + I ·
term (k) + Iterm (k) + Dterm0(k)
Error(t) · dt
The figure on the right motor shows the response of the Proportional-Integral (PI) con-
(Error(k) + Error(k − 1)) · Ts
troller;
Input the (k) =
steady state error disappeared
motor (k) = term
P ·Error(k)+I (k−1)+I·
(k) but the(k)
+ Iterm system
+ Dterm still(k)
has a large overshoot
motor Input Pterm
 k·Ts 2
and a long settling time. A final improvement can be
(Error(k) realized by
d+ Error(k adding a derivative
· dt − 1)) · Ts
motor (k) = Pterm (k) + Iterm (k) = P · Error(k) + I ·
InputInput Error(t)
term. Since a (k)
motor = Pmotor
derivative
Input (k) =aPfunction
·Error(k)+I
along term
term
(k−1)+I·
(k) +predicts
Iterm (k)its + future
0 D
dt
value,
· Error(t) 2 this term will help
to reduce the overshoot Input and(k)hence
= P the
termsettling
(k) + I time:(k) + D dterm (k)
Inputmotor (k) = Pterm (k) + Iterm (k) + D · Error(t)
motor term
(Error(k) + Error(k dt − 1)) · Ts
Inputmotor (k) = P ·Error(k)+Iterm (k−1)+I·
Inputmotor (k) = Pterm (k) + Iterm (k)2 + Dterm (k)
(Error(k) d + Error(k − 1)) · Ts
Inputmotor (k) =motor
Input (k) = Pterm (k)
P ·Error(k)+I term+(k−1)+I·
Iterm (k) + D · Error(t)
dt 2
Inputmotor (k) = Pterm (k) + Iterm (k) + Dterm (Error(k)
(k)
(Error(k) + Error(k −− 1))
1)) · Ts
Input
the derivative (k)
will = P
bemotor·Error(k)+I
discretized as well, (k−1)+I·
term giving +D the
· +final d − Error(k
motor
Input (k) = Pterm (k) + Iterm (k) D · discrete
Error(t)
T 2 equation for a PID
dt
(Error(k) − Error(k − 1))
s
controller:
+ Dd ·
Inputmotor (k) = Pterm (k) + Iterm (k) + D · Error(t) (Error(k) + TError(k s − 1)) · Ts
Input motor (k) = P ·Error(k)+Iterm (k−1)+I· dt
2
(Error(k) − +(Error
Error(k − 1)) · Ts Roll (k − 1)) · Ts
Roll (k) + Error
Input (k) = P ·Error(k)+Iterm (k−1)+I·
Roll (k) = PRoll ·ErrorRoll (k)+I
Inputmotor + D ·(k−1)+IRoll ·
term,Roll Ts 2 2
As the figure shows, the PID controller (Error(k)
(Error
allows the(Error(k)
+Roll (k)
quadcopter
Error(k −(Error
1))
Error(k
to s (k(k)
−
Tapproach
·Roll + Error
1))
1)) the Roll (k − 1)) · Ts
de-
(k) = (k)+I ·+D· (k−1)+I − Error −
Roll
Inputmotor (k)Roll
Input = P ·Error(k)+I
P term (k−1)+I·
·Error Roll+ DRoll ·
2
Roll term,Roll
2
Roll
sired rotation rate quite fast with a small (Error(k) overshoot−and settling T
Ts stime.
(ErrorRoll − 1)) Roll (k − 1))
(k) − Error
Error(k
++D · ·
DRoll
Ts Ts(ErrorRoll (k) + ErrorRoll (k − 1)) · Ts
Obviously, the(k)
InputRoll PID=P controller
Roll ·Errorneeds
Roll (k)+I to be implemented
term,Roll (k−1)+IRoll for· all three rotation rates;
2
roll, pitch and yaw. For the roll rotation rate(Error for example, the
Roll (k) − Error
PID Roll
(Error equation
(k (k)
− 1))+becomes:
ErrorRoll (k − 1)) · Ts
InputRoll (k) = PRoll ·ErrorRoll+(k)+I (k−1)+IRoll ·
Roll
·
DRollterm,Roll
(ErrorRollT(k) s + Error 2
Roll (k − 1)) · Ts
InputRoll (k) = PRoll ·ErrorRoll (k)+Iterm,Roll (k−1)+I (ErrorRoll · Roll (k) − ErrorRoll (k − 1))
+ DRoll · 2
(ErrorRoll (k) − ErrorRoll (kTs− 1))
+ DRoll ·
Ts

which can be further simplified by saving the Iterm and Error during each iteration
for the next iteration through the equations PrevErrorRoll=ErrorRoll (k-1) and PrevIter-
mRoll =ItermRoll(k-1). This way, the iteration indexes k can be removed from the above
equation:
(ErrorRoll + P revErrorRoll ) · Ts
InputRoll = PRoll ·ErrorRoll +P revItermRoll +IRoll ·
2
(ErrorRoll − P revErrorRoll )
+ DRoll ·
Ts
92

ErrorRoll = DesiredRateRoll − RateRoll

 Rotation rate

desired rotation rate

(desiredRate)
overshoot
30 °/s

actual rotation rate

(Rate) - PI controller

0 °/s
settling time

0s 1s 2s 3s Time
Rotation rate

desired rotation rate

(desiredRate)

30 °/s overshoot

actual rotation rate

(Rate) - PID controller

0 °/s
settling time

0s 1s 2s 3s Time

93
Ca rbon aeronautics
 (ErrorRoll + P revErrorRoll ) · Ts
InputRoll = PRoll ·ErrorRoll +P revItermRoll +IRoll ·
Quadcopter2rate control
(ErrorRoll − (Error Roll + P
P revError )
revError Roll ) · Ts
InputRoll = PRoll ·ErrorRoll +P+revIterm
Roll
DRoll · Roll +IRoll ·
The error for the roll rate is given by the difference between Ts the desired2roll rate and
the measured roll rate by the gyroscope: (ErrorRoll − P revErrorRoll )
+ DRoll ·
Ts
ErrorRoll = DesiredRateRoll − RateRoll

To make the notation Error

easier, Roll = DesiredRate
simplify the PID equation for Roll
Roll − Rate the roll rate by saying that
the
InputRoll = f (ErrorRoll , PRoll , IRoll , DRoll , P revErrorRoll , Pparameters:
motor input for the roll rate is function of the different revItermRoll )

InputRoll = f (ErrorRoll , PRoll , IRoll , DRoll , P revErrorRoll , P revItermRoll )

The above equations can be copied for the pitch and yaw rates in order to get the full
PID controller.

In the right figure, the full control loop with equations is visualized. Start each loop
with obtaining the commands from receiver, corresponding to the position of the
sticks on your radiotransmitter. Transform these receiver values to the desired roll,
pitch and yaw rates and the value for the throttle. Next, obtain the actual roll, pitch
and yaw rates of the quadcopter from your gyroscope. These are subtracted from the
desired rotation rates to obtain the error between both. Now you have the necessary
information for the PID equations, which gives you the motor input values for roll,
pitch and yaw. Introduce these input values in the motor power commands that were
derived in project 11. With the first iteration now complete, wait until 0.004 seconds
have passed to start the next iteration.
PID tuning
An important unknown that you did not yet determine are the values for P, I and D.
These constants need to be chosen in such a way that their combination stabilizes
the flight of your quadcopter. The following values are a good compromise be-
tween agility and stability for your quadcopter for all tested motor/ESC/propeller/
battery combinations (see project 1):
• PRateRoll= PRatePitch= 0.6
• IRateRoll= IRatePitch= 3.5
• DRateRoll= DRatePitch=0.03

Notice that the values for the roll and pitch rates are equal; this is evident since the
quadcopter is (almost) symmetrical in both directions. For the yaw rates, the PID
values are:
• PRateYaw= 2
• IRateYaw= 12
• DRateYaw=0

94
 InputMotor1= InputThrottle-InputPitch-InputRoll-InputYaw Wait until 0.004 s
InputMotor2= InputThrottle+InputPitch-InputRoll+InputYaw have passed to start
PrevErrorRoll = ErrorRoll InputMotor3= InputThrottle+InputPitch+InputRoll-InputYaw next iteration
PrevErrorPitch = ErrorPitch InputMotor4= InputThrottle-InputPitch+InputRoll+InputYaw 250 Hz
PrevErrorYaw = ErrorYaw motor power commands loop
PrevItermRoll=ItermRoll
PrevItermPitch=ItermPitch Input =f(Error , P , I , D ,PrevError ,PrevIterm )
Roll Roll Roll Roll Roll Roll Roll
PrevItermYaw=ItermYaw Input =f(Error , P , I , D ,PrevError ,PrevIterm )
Pitch Pitch Pitch Pitch Pitch Pitch Pitch
store for next iteration InputYaw=f(ErrorYaw, PYaw, IYaw, DYaw,PrevErrorYaw,PrevItermYaw)
PId controller

ErrorRoll= DesiredRateRoll-RateRoll - RateRoll

ErrorPitch= DesiredRatePitch-RatePitch RatePitch
ErrorYaw= DesiredRateYaw-RateYaw RateYaw
error calculation + gyroscope
measurements
ReceiverValue[0] DesiredRateRoll = 0.15 (ReceiverValue[0]-1500)
ReceiverValue[1] DesiredRatePitch = 0.15 (ReceiverValue[1]-1500)
ReceiverValue[2] InputThrottle= ReceiverValue[2]
ReceiverValue[3] DesiredRateYaw = 0.15 (ReceiverValue[3]-1500)
radiotransmitter commands transformation to desired values

Finding these optimal values is not easy; there exist some basic methods to obtain
them through calculations, but in the end you will always have to test and retest to
find the values that work for your quadcopter. Usually the trail and error method is
done by first choosing and testing a P value, then a value for I and finally also a value
for D. The values can be chosen with the following guidelines:

• A high P value increases the responsiveness of your quadcopter, but a too

high P value will cause your quadcopter to overcorrect and experience high
frequency oscillations.
• A high I value stops unwanted drifting of your quadcopter, but a too high I
value will cause your quadcopter to feel unresponsive.
• Finally the D value reduces the oscillations caused by the P value. Setting the D
value too high causes motor vibrations.

It can be quite cumbersome to test different PID values, fortunately it only needs
to be done once for each design.

95
Ca rbon aeronautics
 Project 13
The flight controller: rate mode
1 #include <Wire.h>
2 float RatePitch, RateRoll, RateYaw; Define the gyro vari-
3 float RateCalibrationPitch, RateCalibrationRoll, ables (projects 4 and
4 RateCalibrationYaw; 5)
5 int RateCalibrationNumber;

6 #include <PulsePosition.h>
7 PulsePositionInput ReceiverInput(RISING); Define the receiver
8 float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0}; variables (project 7)
9 int ChannelNumber=0;

10 float Voltage, Current, BatteryRemaining, BatteryAtStart; Define the battery

11 float CurrentConsumed=0; variables (project 9)
12 float BatteryDefault=1300;
Define the parameter
13 uint32_t LoopTimer; containing the length
of each control loop

14 float DesiredRateRoll, DesiredRatePitch, All variables neces-

DesiredRateYaw; sary for the PID con-
15 float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw; trol loop are declared
16 float InputRoll, InputThrottle, InputPitch, InputYaw; in this part, including
17 float PrevErrorRateRoll, PrevErrorRatePitch, the values for the P, I
PrevErrorRateYaw; and D parameters
18 float PrevItermRateRoll, PrevItermRatePitch,
PrevItermRateYaw;
19 float PIDReturn[]={0, 0, 0};
20 float PRateRoll=0.6 ; float PRatePitch=PRateRoll;
float PRateYaw=2;
21 float IRateRoll=3.5 ; float IRatePitch=IRateRoll;
float IRateYaw=12;
22 float DRateRoll=0.03 ; float DRatePitch=DRateRoll;
float DRateYaw=0;
96
 Cr eate your first flight controller
After a lot of hard work, you now have all the ingredients to create your first
flight controller and test it on your quadcopter. Let’s put all pieces together!

23 float MotorInput1, MotorInput2, MotorInput3, Declare the input

MotorInput4; variables to the mo-
tors

24 void battery_voltage(void) { Battery function

25 Voltage=(float)analogRead(15)/62; (projects 3 and 9)
26 Current=(float)analogRead(21)*0.089;
27 }

28 void read_receiver(void){ Receiver function

29 ChannelNumber = ReceiverInput.available(); (project 7)
30 if (ChannelNumber > 0) {
31 for (int i=1; i<=ChannelNumber;i++){
32 ReceiverValue[i-1]=ReceiverInput.read(i);
33 }
34 }
35 }

36 void gyro_signals(void) { Gyro function

37 Wire.beginTransmission(0x68); (project 4)
38 Wire.write(0x1A);
39 Wire.write(0x05);
40 Wire.endTransmission();
41 Wire.beginTransmission(0x68);
42 Wire.write(0x1B);
43 Wire.write(0x08);
44 Wire.endTransmission();
45 Wire.beginTransmission(0x68);
46 Wire.write(0x43);
47 Wire.endTransmission();
48 Wire.requestFrom(0x68,6);
49 int16_t GyroX=Wire.read()<<8 | Wire.read();
50 int16_t GyroY=Wire.read()<<8 | Wire.read();
51 int16_t GyroZ=Wire.read()<<8 | Wire.read();
97
 The flight controller: rate mode

Define a function that is called for each PID calculation of the roll, pitch and yaw ro-
tation rates. You saw the equations for each term already in project 12. An important
addition here is the limit of 400 µs on the I term; this term is used to avoid integral
windup. Integral windup is a phenomena in which the integral term accumulates a
significant error due saturation and causes a large overshoot. For example when the
quadcopter cannot achieve the desired setpoint because it did not yet lift off the
ground. Another limit is placed on the full output, to avoid a significant imbalance
between to roll, pitch and yaw commands to the motor.

Return the values for the motor command, the error and the integral term from the
PID equations to the main program.

To ensure a bumpless restart after landing your quadcopter, the PID error and inte-
gral values that are passed to the next iterations need to be reset once you land the
quadcopter. This is also necessary to avoid any windup as well.

At the start of the setup phase, the red LED connected with pin 5 is illuminated to
show that the microcontroller is still in the setup phase. As usual, the LED on the
Teensy itself is illuminated as well to show that it receives power.

98
52 RateRoll=(float)GyroX/65.5;
53 RatePitch=(float)GyroY/65.5;
54 RateYaw=(float)GyroZ/65.5;
55 }

56 void pid_equation(float Error, float P , float I, float D, PID function

float PrevError, float PrevIterm) {
57 float Pterm=P*Error;
58 float Iterm=PrevIterm+I*(Error+
PrevError)*0.004/2;
59 if (Iterm > 400) Iterm=400;
60 else if (Iterm <-400) Iterm=-400;
61 float Dterm=D*(Error-PrevError)/0.004;
62 float PIDOutput= Pterm+Iterm+Dterm;
63 if (PIDOutput>400) PIDOutput=400;
64 else if (PIDOutput <-400) PIDOutput=-400;

65 PIDReturn[0]=PIDOutput; Return the output

66 PIDReturn[1]=Error; from the PID func-
67 PIDReturn[2]=Iterm; tion
68 }
69 void reset_pid(void) { PID reset function
70 PrevErrorRateRoll=0; PrevErrorRatePitch=0;
PrevErrorRateYaw=0;
71 PrevItermRateRoll=0; PrevItermRatePitch=0;
PrevItermRateYaw=0;
72 }
73 void setup() { Visualize the setup
74 pinMode(5, OUTPUT); phase using the red
75 digitalWrite(5, HIGH); LED
76 pinMode(13, OUTPUT);
77 digitalWrite(13, HIGH);

78 Wire.setClock(400000); Communication with

79 Wire.begin(); the gyroscope and
80 delay(250); calibration (project 4
81 Wire.beginTransmission(0x68); and 5)
82 Wire.write(0x6B);
83 Wire.write(0x00);
84 Wire.endTransmission();

99
Ca rbon aeronautics
 The flight controller: rate mode

85 for (RateCalibrationNumber=0;
RateCalibrationNumber<2000;
RateCalibrationNumber ++) {
86 gyro_signals();
87 RateCalibrationRoll+=RateRoll;
88 RateCalibrationPitch+=RatePitch;
89 RateCalibrationYaw+=RateYaw;
90 delay(1);
91 }

When the time consuming part of the setup process is finished, illuminate the green
LED to show that the quadcopter is able to start. However, only dim the red LED
when the battery voltage is higher than 7.5 V.

SAFETY RELATED LINES: just before finishing the setup process, you need to
check that the throttle stick is in its lowest position. Otherwise, if you accidentally
left the throttle stick in a higher position and the radiotransmitter is not nearby, the
motors could suddenly start after the setup process without you controlling it. With
these lines, you stay in an infinite while loop until you move the throttle stick between
1020 and 1050 µs (so moving it from the lowest position to a slightly higher position).

In the last line of the setup process, start a timer that will count the time in the
loop process and go to the next iteration after exactly 4000 µs or 0.004 s, to create a
1/0.004 s = 250 Hz control loop.

100
92 RateCalibrationRoll/=2000;
93 RateCalibrationPitch/=2000;
94 RateCalibrationYaw/=2000;

95 analogWriteFrequency(1, 250); Set the PWM fre-

96 analogWriteFrequency(2, 250); quency to 250 Hz
97 analogWriteFrequency(3, 250); and the resolution to
98 analogWriteFrequency(4, 250); 12 bit for all motors
99 analogWriteResolution(12); (project 8)

100 pinMode(6, OUTPUT); Show the end of the

101 digitalWrite(6, HIGH); setup process and
102 battery_voltage(); determine the initial
103 if (Voltage > 8.3) { digitalWrite(5, LOW); battery voltage per-
104 BatteryAtStart=BatteryDefault; } centage (project 9)
105 else if (Voltage < 7.5) {
106 BatteryAtStart=30/100*BatteryDefault ;}
107 else { digitalWrite(5, LOW);
108 BatteryAtStart=(82*Voltage-580)/100*
BatteryDefault; }

109 ReceiverInput.begin(14); Avoid accidental lift

110 while (ReceiverValue[2] < 1020 || off after the setup
ReceiverValue[2] > 1050) { process
111 read_receiver();
112 delay(4);
113 }

114 LoopTimer=micros(); Start the timer

115 }

116 void loop() { Measure the rotation

117 gyro_signals(); rates and subtract
118 RateRoll-=RateCalibrationRoll; the calibration values
119 RatePitch-=RateCalibrationPitch; (project 5)
120 RateYaw-=RateCalibrationYaw;

121 read_receiver(); Read the receiver

commands (project
7)

101
Ca rbon aeronautics
 The flight controller: rate mode

Transform the commands from the receiver in µs to the desired roll, pitch and yaw
rates in °/s as explained in project 11. The throttle command remains in µs.

Calculate the difference between the desired rotation rates and the measured rotation
rates.

Start the PID calculations for each of the three rotation rates,. The outputs of these
calculations are stored in the array PIDReturn; the roll/pitch/yaw input for the mo-
tors is stored in position 0, the error value and value for the Iterm that needs to be
used for the next iteration is stored in positions 1 and 2. Retrieve these values each
time for the corresponding rotation rate to be able to use them in the next iteration.

With the throttle stick, you are able to go to 2000 µs, which would give maximal pow-
er to all four motors. However, this would give no room to stabilize the roll, pitch and
yaw rates. That is why you limit the throttle value to 1800 µs or 80%.

Now calculate the motor inputs with the quadcopter dynamics equations you saw
during project 11. Remember to convert the throttle values in µs to their 12 bit equiv-
alent by multiplying them with 1.024.

102
122 DesiredRateRoll=0.15*(ReceiverValue[0]-1500); Calculate the desired
123 DesiredRatePitch=0.15*(ReceiverValue[1]-1500); roll, pitch and yaw
124 InputThrottle=ReceiverValue[2]; rates
125 DesiredRateYaw=0.15*(ReceiverValue[3]-1500);

126 ErrorRateRoll=DesiredRateRoll-RateRoll; Calculate the errors

127 ErrorRatePitch=DesiredRatePitch-RatePitch; for the PID calcula-
128 ErrorRateYaw=DesiredRateYaw-RateYaw; tions

129 pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, Execute the PID cal-

DRateRoll, PrevErrorRateRoll, culations
PrevItermRateRoll);
130 InputRoll=PIDReturn[0];
PrevErrorRateRoll=PIDReturn[1];
PrevItermRateRoll=PIDReturn[2];
131 pid_equation(ErrorRatePitch, PRatePitch,
IRatePitch, DRatePitch, PrevErrorRatePitch,
PrevItermRatePitch);
132 InputPitch=PIDReturn[0];
PrevErrorRatePitch=PIDReturn[1];
PrevItermRatePitch=PIDReturn[2];
133 pid_equation(ErrorRateYaw, PRateYaw,
IRateYaw, DRateYaw, PrevErrorRateYaw,
PrevItermRateYaw);
134 InputYaw=PIDReturn[0];
PrevErrorRateYaw=PIDReturn[1];
PrevItermRateYaw=PIDReturn[2];

135 if (InputThrottle > 1800) InputThrottle = 1800; Limit the throttle

output

136 MotorInput1= 1.024*(InputThrottle-InputRoll Use the quadcopter

-InputPitch-InputYaw); dynamics equations
137 MotorInput2= 1.024*(InputThrottle-InputRoll (project 11)
+InputPitch+InputYaw);
138 MotorInput3= 1.024*(InputThrottle+InputRoll
+InputPitch-InputYaw);
139 MotorInput4= 1.024*(InputThrottle+InputRoll
-InputPitch+InputYaw);

103
Ca rbon aeronautics
 The flight controller: rate mode

Make sure that the inputs to the motors do not exceed 2000 µs after the dynamic
equations to avoid overloading them.

To avoid stopping the motors in mid-flight, keep them turning at 18% when the mo-
tor input decreases below 1180 µs (= the ThrottleIdle value).

SAFETY RELATED LINES: the previous lines would mean you can never switch
off the motors, as they keep turning at minimally 18%. Just before sending the com-
mands to the motors, you add the condition that if the throttle stick is brought to its
lowest position (below 1050 µs), all four motors stop (e.g. the value of ThrottleCut-
Off is 1000 µs or 0% power). Usually you would do this after landing the quadcopter.
The PID parameters need to be reset in case you want to have a bumpless restart.

Now you are finally ready to sent the commands to each of the four motors.

The last step in the iteration is to wait until the 4000 µs or 0.004 s have passed using
a while loop. When this condition is met, reset the timer to the actual time and the
program can proceed to the next iteration. Congratulations, you created a 250 Hz
control loop!

104
140 if (MotorInput1 > 2000)MotorInput1 = 1999; Limit the maximal
141 if (MotorInput2 > 2000)MotorInput2 = 1999; power commands
142 if (MotorInput3 > 2000)MotorInput3 = 1999; sent to the motors
143 if (MotorInput4 > 2000)MotorInput4 = 1999;

144 int ThrottleIdle=1180; Keep the quadcopter

145 if (MotorInput1 < ThrottleIdle) MotorInput1 = motors running at
146 ThrottleIdle; minimally 18% pow-
147 if (MotorInput2 < ThrottleIdle) MotorInput2 = er during flight
148 ThrottleIdle;
149 if (MotorInput3 < ThrottleIdle) MotorInput3 =
150 ThrottleIdle;
151 if (MotorInput4 < ThrottleIdle) MotorInput4 =
152 ThrottleIdle;

153 int ThrottleCutOff=1000; Make sure you are

154 if (ReceiverValue[2]<1050) { able to turn off the
155 MotorInput1=ThrottleCutOff; motors
156 MotorInput2=ThrottleCutOff;
157 MotorInput3=ThrottleCutOff;
158 MotorInput4=ThrottleCutOff;
159 reset_pid();
160 }
161 analogWrite(1,MotorInput1); Sent the commands
162 analogWrite(2,MotorInput2); to the motors
163 analogWrite(3,MotorInput3);
164 analogWrite(4,MotorInput4);

165 battery_voltage(); Keep track of battery

166 CurrentConsumed=Current*1000*0.004/3600+ level (project 9)
CurrentConsumed;
167 BatteryRemaining=(BatteryAtStart-
CurrentConsumed)/BatteryDefault*100;
168 if (BatteryRemaining<=30) digitalWrite(5, HIGH);
169 else digitalWrite(5, LOW);

170 while (micros() - LoopTimer < 4000); Finish the 250 Hz

171 LoopTimer=micros(); control loop
172 }

105
Ca rbon aeronautics
 The flight controller: rate mode

Before you fly... radiotransmitter failsafe

Imagine you are flying your quadcopter and suddenly, your radiotransmitter loses
power or signal. What will happen to your quadcopter? Well, you did not program
any return to home function, so it will just keep flying until it runs out of battery or
crashes. To avoid this, your radiotransmitter can tell the receiver to give a throttle
command of 1000 µs when it loses connection. This means that the motors of
your quadcopter will stop turning an it will fall to the ground - not the ideal solution
but better to have some damage then a quadcopter on the loose.

Power on the transmitter → hold the OK button for two seconds → choose Sytem
→ go down and choose "RX setup" → go down and choose "Failsafe". Choose
channel 3 (the throttle channel) then click UP or DOWN to activate the failsafe of
channel 3 (ON). Now lower the throttle stick to its lowest position and press and
hold the CANCEL function to tell the receiver it should sent a throttle command
of 1000 µs or 0% when it loses contact with the radiotransmitter. Return to the
previous screen on the transmitter, which should show -100% at channel 3. The
failsafe is now set.

Start-up and flying your quadcopter

When you have set the radiotransmitter failsafe, it is time to start flying. After you
connect the battery and turn on the slide switch, the red LED should be lighted
indicating the ongoing startup process. Wait a couple of seconds without touch-
ing your quad (to avoid calibration errors). When the green LED illuminates, you
can move the throttle stick slightly upward and each motor will beep four times
indicating that you are ready to go. Increase the throttle stick to 30% power such
that the motors are turning but the quad is not yet taking off, then turn off the
radiotransmitter to test the failsafe. If after a second all motors turn off, the
test is successful.

You can now start your first flight. Flying in rate control mode is rather difficult,
so be sure to fly outside at a large grass field without any people nearby to mini-
mize any damage to the quadcopter or others in the event of a crash. You can play
with the PID values to optimize the quadcopters response to your liking.

106
107
Ca rbon aeronautics
Part II: stabilization
mode
flying in rate mode is difficult for begin-
ners, because you have to manually adjust
your quadcopter back to a level position.

in this part, you will learn how to combine

different measurements to get the absolute
angles of your quadcopter. This allows you
to create a more advanced flight controller
that levels itself automatically when you re-
lease the roll and pitch sticks.

You will build and expand upon the code

you developed earlier, and your analytical
skills will be challenged further
 Project 14
Measuring angles

y
z

Pitch angle: -45°

Roll angle: 0° yaw around
z axis
X (positive direction)
z y Pitch angle: 0°
Roll angle: -45°

y y
z

yaw around
z axis
(negative direction)
X 45°

Pitch angle: 0°
Roll angle: 45°

110
 Le arn to measure angles - twice
At this point the rotation rates of your quadcopter have no more secrets for
you. Now it is time to explore a more difficult topic; how can you measure the
absolute roll and pitch angles of your quadcopter?

The absolute roll and pitch angles of your quadcopter are a key component for a
flight controller that works in stabilize mode; knowing the angles allows you to level
the quadcopter exactly and make flying a lot easier. But how can you measure the ab-
solute roll and pitch angles? In this project, you will explore two methods, both with
their own (dis)advantages.

1. Integrating the gyro rotation rates

A first and very easy solution to obtain the absolute angles consists of integration
of the rotation rates that are measured by the gyroscope. For the pitch, this can be
represented with the equation;
 k·Ts
Anglepitch = Ratepitch · dt
0

With Ratepitch in degrees per second (°/s), Anglepitch in degrees (°), Ts the duration of

one (0.004 s) iterationAngle
and pitch
k the(k) number
= Angle
Angle ofk·T
pitch =pitch
iterations.
(k
s
− 1) +
Rate RateDiscretization
pitch · dtpitch
(k) · Ts of this integral
to use in your code gives the equation: 0

AccY
Anglepitch (k) = Angle roll )(k
tan(θpitch = − 1) + Ratepitch (k) · Ts
s

If this looks too easy... well that’s because  unfortunately  it is. When the quadcopter
AccY
is yawing left (or right) aroundθroll
the=Ztan(θ
axis
atan roll
 ) = AccYany pitch
without  rotation rate around the
s
Y axis, the pitch angle will nonetheless decrease + Acc
Acc2X (or 2
increase)
Z as the direction of the
Y axis changes. During this yaw movement, 

the roll angle
 will increase (or decrease)

AccY
as well because the direction ofθrollthe X axis
= atan  also changes.
−Acc Hence even with a zero roll
θpitch = atan  Acc2X + Acc2Z 
X
or pitch rotation rate, the roll and pitch angles Acccan change.
Y + AccZ
2 2 This phenomenon is vis-
ualized in the figure to the left for a pure yaw movementusing an inclined plate with
a fixed angle of 45°. θ = atan  
−AccX 
pitch
Acc2Y + Acc2Z
You can integrate the change of the roll and pitch angle with the yaw movements in
the equations, but you will fortunately not need to do this for this application; as you
will see later, this error is not the only issue with integrating the gyro measurements.
For now, let's explore the second method to obtain angles; using an accelerometer.

111
 Measuring angles

2. The accelerometer

Your MPU-6050 sensor is not only a gyroscope but also contains an accelerometer.
As the name implies, the accelerometer measures the acceleration of the sensor along
the X, Y and Z directions. Remember that the gyroscope measures the rotation rates
around the X, Y and Z directions, not along. From basic physics, you remember that
we experience a gravitational acceleration anywhere on earth and that this gravita-
tional acceleration is equal to the gravitational constant; 1 g or 9,81 m/s². This means
that when you let your MPU-6050 sensor lie flat on a table without moving it, the
measurement of the acceleration along the Z direction (or AccZ) is equal to 1 g. The
acceleration along the X and Y axes will be zero in this case. Similarly, when you po-
sition the sensor such that one of the other axes lies perpendicular to the surface of
the table, the corresponding acceleration is equal to 1 g.
AccX = 0 AccX = 0 AccX = 1 g
y X
Accy = 0 Accy = 1 g Accy = 0
Accz = 1 g z Accz = 0 Accz = 0

y X
X
y
z
z

Off course, any other direction not along one of the three main axes will result in
a nonzero acceleration value for all three directions AccX, AccY and AccZ. Through
some clever mathematical equations, this accelerometer property will enable you to
calculate the exact roll and pitch angles of your quadcopter. Let’s assume you roll
around the X axis until you reach the angle θroll. To visualize this transformation, a box
bounded by the X, Y and Z directions is sketched on the figure below.
θroll
Accz s θ s2=Accz2+AccX2
Roll around Accz roll

AccX the X axis Accy

AccX tan θroll =
Accy s
Accy θ
Y

C
C ND L
roll
Y
X

D L A A L O V G SC DA
C A
C N SC SD XD XC AD INT S D L
V G X C O
X

X D T
A IN

θpitch
Accz s θ s2=Accz2+Accy2
Pitch around Accz pitch

Accy the y axis -AccX

VC
GND C
Accy tan θpitch =
GNDVCC -AccX SCL s
SCL SD
XDA A
SDA
XCLXDA Y AccX ADO
XCL
θpitch
ADO INT Y
INT AccX
X

112
  k·Ts
Anglepitch = Ratepitch · dt
From your basic trigonometry knowledge, 0 you know that the tangent of the angle
 k·T
of a triangle is equal to the length
Angleof the opposite
pitch =
side· dt
Ratepitch of the triangle divided by the
s

length of the adjacentAngle

sidepitch
of (k)
the=triangle. 0 In the case of the angle θ , the opposite
(k − 1) + Ratepitch (k) · Ts
Anglepitchk·T s
roll
pitch =
side is equal to AccY while the Angle
adjacent side0 is Rate to· adtcertain length s:
equalpitch
Anglepitch (k) = Anglepitch (k − 1) + Ratepitch (k) · Ts
AccY
tan(θroll )(k
Anglepitch (k) = Angle = − 1) + Rate
pitch s pitch (k) · Ts

AccY
Using the Pythagoras rule on the triangle )=
rollformed
s by s, AccX and AccZ, you are able
tan(θ
to derive that s²=AccX²+AccZ²,θroll
thus the 
= tan(θ
atan angle

roll ) = θ
AccYcan be
roll  expressed by the equation:
 Acc2X s+ Acc2Z 
AccY
θroll = atan   
 Acc2 + Acc2  
X
Acc Z
−Acc
roll =
 YX 
θθpitch =atan
atan 
 
Through similar reasoning and with the help of2X2Y +the
 Acc
Acc next
+ Acc
Acc
22
ZZ  figure you can express the
pitch angle by: θpitch = atan  
−AccX 

Acc2 + Acc2 
Y Z
−Acc
θpitch = atan  
X 
Acc2Y + Acc2Z

And that’s it, you are now able to calculate the roll and pitch angles from the values of
your accelerometer! Now transform this into a working code. For this part, you only
need your Teensy and MPU-6050; you can choose to test on a breadboard, or directly
on your assembled quadcopter.
+ - a b c d e f g h i j + -
1 1
2 2
3 3
VCC
4 4
GND
5 5
SCL
6 6
SDA
7 XDA
7
8 XCL
8
Y
9 ADO
9
10 10
INT
X

11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
20 20
21 21
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e + -
f g h i j

113
Ca rbon aeronautics
 Measuring angles

Coding
Modify the code that you have already developed to read the gyroscope in order to
extract the accelerometer measurements of the MPU-6050 as well.

Start with defining the variables that will hold the acceleration values in the X, Y and
Z direction, together with the roll and pitch angles.

First configure the accelerometer output. This can be done by choosing the AFS_
SEL setting using register 0x1C (see the MPU-6050 documentation). The options
for the accelerometer correspond to bits 3 and 4. You will choose a full scale range
of ±8 g, which corresponds to an LSB sensitivity of 4096 LSB/g and a value for the
AFS_SEL setting of 2, or a 0 for bit 3 and a 1 for bit 4. This corresponds in turn to
the following 8 bit binary representation: 00010000. Converting this to a hexadecimal
value gives 0x10.

The values of the accelerometer are located in the registers with hexadecimal num-
bers 3B to 40. Start writing to address 0x3B to indicate the first register and request 6
bytes from the address of the sensor, 0x68. The accelerometer measurements in LSB
are once again spread out over two registers with each 8 bits.

114
1 #include <Wire.h>
2 float RateRoll, RatePitch, RateYaw;

3 float AccX, AccY, AccZ; Define the acceler-

4 float AngleRoll, AnglePitch; ometer variables

5 float LoopTimer;
6 void gyro_signals(void) {
7 Wire.beginTransmission(0x68); Switch on the low-
8 Wire.write(0x1A); pass filter (project 4)
9 Wire.write(0x05);
10 Wire.endTransmission();

11 Wire.beginTransmission(0x68); Configure the accel-

12 Wire.write(0x1C); erometer output
13 Wire.write(0x10);
14 Wire.endTransmission();

15 Wire.beginTransmission(0x68); Pull the accelerom-

16 Wire.write(0x3B); eter measurements
17 Wire.endTransmission(); from the sensor
18 Wire.requestFrom(0x68,6);
19 int16_t AccXLSB = Wire.read() << 8 |
Wire.read();
20 int16_t AccYLSB = Wire.read() << 8 |
Wire.read();
21 int16_t AccZLSB = Wire.read() << 8 |
Wire.read();

22 Wire.beginTransmission(0x68); Configure the gyro-

23 Wire.write(0x1B); scope output and pull
24 Wire.write(0x8); rotation rate meas-
25 Wire.endTransmission(); urements from the
26 Wire.beginTransmission(0x68); sensor (project 4)
27 Wire.write(0x43);
28 Wire.endTransmission();

115
Ca rbon aeronautics
 Measuring angles

To convert the accelerometer measurements from LSB to g, remember that you con-
figured the AFS_SEL setting to an LSB sensitivity of 4096 LSB/g. To get the meas-
urements in g, just divide the measurements in LSB by 4096 LSB/g.

At the start of this project, you learned how to calculate the roll and pitch angles
from the accelerometer values. You can use these equations at this point, as long as
you take into account that the arctangens calculated by Arduino returns a result in
radians, not in degrees. To convert the angles from radians to degrees, just divide the
results by �/180.

Testing and calibration

When you run the code with the MPU-6050 flat on a table without moving and open
the serial monitor, you will notice that the acceleration values in the X, Y and Z direc-
tions are not exactly 0, 0 and 1 g as they should be but rather:

Acceleration X [g]= 0.04 Acceleration Y [g]= -0.02 Acceleration Z [g]= 1.11

Acceleration X [g]= 0.04 Acceleration Y [g]= -0.03 Acceleration Z [g]= 1.11
Acceleration X [g]= 0.03 Acceleration Y [g]= -0.03 Acceleration Z [g]= 1.10

It is normal when you do not have the same values as mentioned above, because each
sensor is slightly different. Calibration is once again necessary to correct these values.
Because the MPU-6050 has to be exactly level when doing the accelerometer calibra-
tion, it recommended to do this beforehand;
• Normally your MPU-6050 lies already flat on the table. The acceleration in the
Z direction should be 1 in this case; for the above values for example, this would
give a correction value of 0.11.
• For the calibration of the acceleration in the X direction, you have to tilt the
MPU-6050 vertically along the X axis. You should now get a value close to 1.
Note once again the difference between the value you get and 1 g.
• Now do the same for the acceleration in the Y direction.

116
29 Wire.requestFrom(0x68,6);
30 int16_t GyroX=Wire.read()<<8 | Wire.read();
31 int16_t GyroY=Wire.read()<<8 | Wire.read();
32 int16_t GyroZ=Wire.read()<<8 | Wire.read();
33 RateRoll=(float)GyroX/65.5;
34 RatePitch=(float)GyroY/65.5;
35 RateYaw=(float)GyroZ/65.5;

36 AccX=(float)AccXLSB/4096; Convert the meas-

37 AccY=(float)AccYLSB/4096; urements to physical
38 AccZ=(float)AccZLSB/4096; values

39 AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ* Calculate the abso-

AccZ))*1/(3.142/180); lute angles
40 AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*
AccZ))*1/(3.142/180);
41 }

42 void setup() {
43 Serial.begin(57600);
44 pinMode(13, OUTPUT);
45 digitalWrite(13, HIGH);
46 Wire.setClock(400000); Communication with
47 Wire.begin(); the gyroscope and
48 delay(250); calibration (project 4
49 Wire.beginTransmission(0x68); and 5)
50 Wire.write(0x6B);
51 Wire.write(0x00);
52 Wire.endTransmission();
53 }

54 void loop() {
55 gyro_signals();
56 Serial.print("Acceleration X [g]= "); Print the accelerome-
57 Serial.print(AccX); ter values
58 Serial.print(" Acceleration Y [g]= ");
59 Serial.print(AccY);
60 Serial.print(" Acceleration Z [g]= ");
61 Serial.println(AccZ);
62 delay(50);
63 }

117
Ca rbon aeronautics
 Measuring angles

The three calibration values you get should be subtracted from lines 36 to 38 in the
space Run the program again and verify
code, so insert your own values in the yellow space.
that the acceleration in all directions is correct.

Next, verify that the calculated roll and pitch angles are correct. You can easily check
this by replacing lines 56 to 61 the code with the lines written on the right. When the
MPU-6050 lies level on a table, the value for both angles should be very close to zero
if you have done a correct calibration.

Accelerometer trigonometry or gyro integration?

You explored two different methods to calculate the roll and pitch angles; one meth-
od integrated the rotation rates coming from the gyroscope, and the second method
used trigonometry on accelerometer measurements. It is now time to evaluate the
advantages and disadvantages of each method. Most disadvantages come with the
gyro integration method;
• Pure integration of the roll and pitch rotation rates does not take into account
roll and pitch angle changes when yawing, as you already saw before. This means
that the calculated angles will not be fully correct during flight.
• With integration, you add the change in angle to the previous angle for each
iteration. Because each measurement has an error, this also means that you will
add the errors of each measurements. This causes an ever increasing error as
shown on the picture to the right; after three minutes, the angle deviation is
already equal to 1°.
• Your integration always starts from an angle equal to zero. If the surface on
which the quadcopter sits is not level, the angles will be wrong.

By testing the accelerometer and the resulting angles calculated through trigonom-
etry, you will see that none of the above disadvantages occur for this type of meas-
urement. So why all this trouble, why can’t you just use the accelerometer? Well,
unfortunately the accelerometer is extremely sensitive to vibrations; it measures ac-
celeration after all. So sensitive, that even with the low pass filter you already con-
figured in the MPU-6050, the resulting angles cannot be handled by your PID con-
troller. This is visible in the picture to the right; the gyro integration over time stays
quite continuous, while the accelerometer angles are not continuous at all. This effect
will be magnified when the motors are running. The disadvantages of both methods
necessitate a different solution, which you will explore with the next project.

118
36 AccX=(float)AccXLSB/4096-0.05
-0.05; Correct the acceler-
37 AccY=(float)AccYLSB/4096+0.01
+0.01; ometer values after
38 AccZ=(float)AccZLSB/4096-0.11
-0.11; calibration

54 void loop() {
55 gyro_signals();
56 Serial.print("Roll angle [°]= "); Check the measured
57 Serial.print(AngleRoll); roll and pitch angles
58 Serial.print(" Pitch angle [°]= ");
59 Serial.println(AnglePitch);
60 delay(50);
61 }

Angle

1°
gyro integration

0,5° accelerometer trigonometry

0°

-0,5°
0 1 2 3
Time [minutes]

119
Ca rbon aeronautics
 Project 15
The Kalman filter - one dimension

+ - a b c d e f g h i j + -
1 1
2 2
3 3
VCC
4 4
GND
5 5
SCL
6 6
SDA
7 XDA
7
8 XCL
8
Y
9 ADO
9
10 10
INT

X
11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
20 20
Anglekalman21(k) = Anglekalman (k − 1) + T21s · Rate(k)
22 22
23 23
24 24
25 25
(k) = (k 1) + Ts2 · 42
2 3 4

U ncertaintyangle
26
U ncertaintyangle −26
27 27
28 28
3V

29 29
0

Anglekalman (k) = Angle

30 kalman (k)5V
+ Gainkalman · (Angle(k)
30 − Anglekalman )
G

+ - a b c d e + -
f g h i j

U ncertaintyangle (k)
A final equation is necessary to update
Gainkalman = the uncertainty on the new angle prediction,
U ncertaintyangle (k) + 32
once again by using the Kalman Gain:

U ncertaintyangle (k) = (1 − Gainkalman ) · U ncertaintyangle (k)

Congratulations, you now learned how to combine the two methods, each with their
S(k)to= predict
own uncertainties and errors, the1)most
F · S(k − + G · accurate
U (k) value for the angle. This
approach can be followed for both the roll and pitch angles.

P (k) = F · P (k − 1) · F T + Q 120

T
Co mbine two imperfect measurements
During the previous project you explored two methods of measuring and cal-
culating angles and found out that the disadvantages of both make them un-
suitable for a flight controller. However, what if you can combine both meas-
urements to get rid of their individual disadvantages? This is exactly what you
are going to do during this project, through an iterative mathematical concept
called the Kalman filter.

Let’s begin by rewriting the equation that you used to transform the rotation rate of
the gyroscope to the angle, with Ts the time of one iteration k (0.004 s in our case):
Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)

Assume now that this calculation gives you a prediction for the angle, but not its final
value, because it is prone to a number of errors. Calculate the uncertainty on the pre-
U ncertainty
Angle (k)(k) = U ncertainty
= Angle 1) (k
(k −angle + T−s 1) + Ts2 · 42
· Rate(k)
diction of the angle askalman
theangle
sum of the kalman
uncertainty on the previous angle prediction
(iteration k-1) and the uncertainty on the evolution of the angle:
Anglekalman (k) = Angle
U ncertainty kalman
angle (k)U+
(k) = Gainkalman
ncertainty · (Angle(k)
angle · 42 kalman )
(k − 1) + T−s2 Angle

The uncertainty on the evolution of the angle is estimated as Ts2 . 42 because:

• The standard deviation σ of the rotation rate measurement
U ncertainty (k) error is 4°/s, giving
Anglekalman (k) = Gain
Angle kalman=(k) + Gainkalman angle
· (Angle(k) − Anglekalman )
a variance σ² of 4²=16. The rotation
kalman
rate measurement
U ncertainty angle (k) + 3
2 error is an estimation; it

includes the actual imperfection of the sensor itself, but also the fact that you do
not take into account the yaw rotation rate in the (k)
U ncertainty angle calculation.
U ncertaintyGain (k) = =(1 − Gainkalman ) ·angle
U ncertaintyangle (k)
• Because the rotation rate is multiplied
anglekalman
with T (=0.004 32s) in the equation, this has
Anglekalman (k) = Anglekalman (k − 1) + T+
U ncertainty s
angle (k)
s · Rate(k)
to be taken into account in the variance calculation as well, using the factor Ts2.
Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)
(k) = F
U ncertainty S(k) (1 ·−S(k − 1) + G) · U (k)
Gain ncertainty (k)
In the next step you determine
angle
the so-called Kalman gain. This
kalman angle
U ncertaintyangle (k) = U ncertaintyangle (k − 1) + Ts2 · 42
gain weighs your
prediction of the angle (Anglekalman(k)) as calculated above with the measured angle
(Angle) using our accelerometer
U ncertainty to=obtain
angle (k) a new angle
U ncertainty prediction
T (k − 1) +forTs2the
· 42angle:
P (k)==FF· ·S(k
S(k) P (k−−1)1)+· F +(k)
G·U Q
Anglekalman (k) = Anglekalman (k) + Gainkalman · (Angle(k) − Anglekalman )

Now how
Angle you(k)
dokalman = Angle
calculate this (k) + Gain
Kalman gain?·kalman
The (Angle(k)
T T ·gain is defined as the
− Angle )
relative ratio
P (k) ==F H· P· (k − 1)H· F +
P (k) +
kalman kalman
L(k) RQ
of the uncertainty on the predicted angle to the uncertainty
U ncertainty angle (k) on the measured angle
Gainkalman =
with the accelerometer: U ncertaintyangle (k) + 32
U ncertaintyangle (k)
Gainkalman =H T · P (k) · H T +
K = PL(k)(k) · = H = P (k) · angle
U ncertainty H T ·R(k) + −1
L(k) 32
L(k)
U ncertaintyangle (k) = (1 − Gainkalman ) · U ncertaintyangle (k)
In the equation, you assume that the standard deviation σ of the accelerometer meas-
urement error is equal toangle
U ncertainty 3°.(k) = (1H−T Gainkalman ) T· U ncertainty angle (k)
= P=(k)
S(k)
K · + K=· (M
S(k) (k)· −
P (k) H H· ·L(k)−1
S(k))
S(k) = FL(k)
· S(k − 1) + G · U (k)
121
S(k) = F · S(k − 1) + G · U (k)
P (k)
S(k) = S(k)=+(IK−· K F ) ·−PH
(M· (k) (k)· S(k))
P (k) = F · P (k − 1) · F T + Q
 Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)
The Kalman filter - one dimension

Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)

angle (k)
General
U ncertainty = U ncertainty
form of the Kalman + Ts2 · 42
angle (k − 1) filter

The Kalman filter that you derived in this project is specifically adapted to predict
U ncertaintyangle (k) = U ncertaintyangle (k − 1) + Ts2 · 42
nglekalman (k) the roll or
= Angle pitch angle. This is a one dimensional Kalman filter, as the so-called
kalman (k) + Gainkalman · (Angle(k) − Anglekalman )
‘state’ of the system consists of only one value: the roll (or pitch) angle. This ap-
proach can be expanded to multi-dimensional states using vectors and matrices.
nglekalman (k) = Anglekalman (k) + Gainkalman · (Angle(k) − Anglekalman )
The ‘general’=formU ncertainty angle (k)filter is written below and will be used when
of the Kalman
Gain
(k)the
+ 3quadcopter
2
kalman
you are estimating the altitude
U ncertainty angleof further on. For comparison, the
values for all vectors and matrices
U ncertainty in
(k) our current example are also written.
Gainkalman =
angle

· U(k) + 3system:
2
1. Predict
U ncertainty angle (k) the
= (1current
U state
−ncertainty
Gainkalman ) of
angle the
ncertainty angle (k)
S=state vector (Anglekalman)
U ncertaintyangle (k) = (1 − Gainkalman ) · U ncertaintyF=state
angle (k)
transition matrix (1)
S(k) = F · S(k − 1) + G · U (k) G=control matrix (0.004)
U=input variable (Rate)
S(k) = F · S(k − 1) + G · U (k)
P (k) = the
2. Calculate (k − 1) · F T +
F · Puncertainty ofQthe prediction:
P=prediction uncertainty vector (Un-
P (k) = F · P (k − 1) · F T + Q certaintyangle)
L(k) = H · P (k) · H T + R Q=process uncertainty (Ts2 . 42)

L(k) = HT · P (k) · H + R T
Coding
K = P (k) ·
H
= P (k) · H T · L(k)−1
L(k)
Connect your MPU-6050
T to your Teensy to test the Kalman filter.
H
K = P (k) · = P (k) · H T · L(k)−1
S(k) = S(k) + K · (M (k) − H · S(k))
L(k)

S(k) = S(k) + K · (M (k) − H · S(k))

P (k) = (I − K · F ) · P (k)

P (k) = (I − K · F ) · P (k)

Define the roll and pitch angles coming from our Kalman filter. Your initial guess for
the angle values is zero, because the quadcopter will generally take off from a rather
level surface. Off course, the surface will never be exactly level, so you take the uncer-
tainty (=variance σ²) on the initial guess for the angles to be (2°)². If you take off from
a surface that is not level at all, the Kalman filter will use the accelerometer values to
quickly correct this initial wrong guess.

Define the output from the Kalman filter; this are two variables: the Kalman predic-
tion for the state (the angle in our case) and the uncertainty on this prediction. Both
variables are updated during each iteration.
122
 S(k) = F · S(k − 1) + G · U (k)
S(k) = F · S(k − 1) + G · U (k)
U ncertaintyangle (k) = (1 − Gainkalman ) · U ncertaintyangle (k)
S(k) = F · S(k − 1) + G · U (k)
P (k) = F · P (k − 1) · F T + Q
P (k) =the
3. Calculate F ·P Kalman F T + Q the uncertainties on the predictions and
(k − 1) ·gain
S(k) = F · S(k − 1) + G · Ufrom(k)
P (k) = F · P (k − 1) · F T + Q
measurements:
L(k) = H · P (k) · H T + R L= Intermediate matrix
L(k) = H · P (k) · H T + R
P (k) = F · P (k − 1) · F T + Q K=Kalman gain
L(k) = HT · P (k) · H T + R H=Observation matrix (=1)
H
K = P (k) · T = P (k) · H T · L(k)−1 R=Measurement uncertainty (T 2 . 32)
H
L(k) s
K = PL(k)(k) · = H · = P (k)
P (k) · H· TH+ ·RL(k)
T −1
L(k)
H T
4. K
Update
= P (k)the· predicted
= P (k) · state of the
H T · L(k)−1
system with the measurement of the
L(k)
S(k) = S(k) + K · (M (k) − H · S(k))
state through the Kalman gain:
S(k) = S(k)H+TK · (M (k) − TH · S(k))
K = P (k) · = P (k) · H · L(k)−1
S(k) = S(k)L(k)
+ K · (M (k) − H · S(k)) M=measurement vector (Angle)
P (k) = (I − K · F ) · P (k)
P (k) = (I − K · F ) · P (k)
S(k) = S(k) + K · (M (k) − H · S(k))
P (k)
5. Update the= uncertainty
(I − K · F ) · Pof(k)the predicted state:

P (k) = (I − K · F ) · P (k) I=unity matrix (=1)

1 #include <Wire.h>
2 float RateRoll, RatePitch, RateYaw; Define the gyroscope
3 float RateCalibrationRoll, RateCalibrationPitch, and accelerometer
RateCalibrationYaw; variables
4 int RateCalibrationNumber;
5 float AccX, AccY, AccZ;
6 float AngleRoll, AnglePitch;
7 uint32_t LoopTimer;

8 float KalmanAngleRoll=0, Define the predicted

KalmanUncertaintyAngleRoll=2*2; angles and the uncer-
9 float KalmanAnglePitch=0, tainties
KalmanUncertaintyAnglePitch=2*2;

10 float Kalman1DOutput[]={0,0}; Initialize the output

of the filter

123
Ca rbon aeronautics
 The Kalman filter - one dimension

Create the function 11 void kalman_1d(float KalmanState,

that calculates the float KalmanUncertainty, float KalmanInput,
predicted angle and float KalmanMeasurement) {
uncertainty using the 12 KalmanState=KalmanState+0.004*KalmanInput;
Kalman equations 13 KalmanUncertainty=KalmanUncertainty + 0.004
* 0.004 * 4 * 4;
14 float KalmanGain=KalmanUncertainty * 1/
(1*KalmanUncertainty + 3 * 3);
15 KalmanState=KalmanState+KalmanGain * (
KalmanMeasurement-KalmanState);
16 KalmanUncertainty=(1-KalmanGain) *
KalmanUncertainty;

Kalman filter output 17 Kalman1DOutput[0]=KalmanState;

Kalman1DOutput[1]=KalmanUncertainty;
18 }

Read the rotation 19 void gyro_signals(void) {

rates, acceleration 20 Wire.beginTransmission(0x68);
and angles from the 21 Wire.write(0x1A);
MPU-6050 (project 22 Wire.write(0x05);
14) 23 Wire.endTransmission();
24 Wire.beginTransmission(0x68);
25 Wire.write(0x1C);
26 Wire.write(0x10);
27 Wire.endTransmission();
28 Wire.beginTransmission(0x68);
29 Wire.write(0x3B);
30 Wire.endTransmission();
31 Wire.requestFrom(0x68,6);
32 int16_t AccXLSB = Wire.read() << 8 |
Wire.read();
33 int16_t AccYLSB = Wire.read() << 8 |
Wire.read();
34 int16_t AccZLSB = Wire.read() << 8 |
Wire.read();
35 Wire.beginTransmission(0x68);
36 Wire.write(0x1B);
37 Wire.write(0x8);
38 Wire.endTransmission();

124
The next step is to create the function for the Kalman filter. Four variables are nec-
essary to initiate this function;
• the Kalman prediction for the previous state (the angle in this case);
• the uncertainty on the Kalman prediction for the previous state;
• the input for the new Kalman prediction of the state (the rotation rate from the
gyroscope in this case);
• the measurement that will be compared with the new Kalman prediction of the
state (the angles measured by the accelerometer in this case).

You use these four variables to solve the five equations that were explained on the
previous pages.

The output of the Kalman filter function consists of a prediction for the state (the
angle) and the corresponding uncertainty.

39 Wire.beginTransmission(0x68);
40 Wire.write(0x43);
41 Wire.endTransmission();
42 Wire.requestFrom(0x68,6);
43 int16_t GyroX=Wire.read()<<8 | Wire.read();
44 int16_t GyroY=Wire.read()<<8 | Wire.read();
45 int16_t GyroZ=Wire.read()<<8 | Wire.read();
46 RateRoll=(float)GyroX/65.5;
47 RatePitch=(float)GyroY/65.5;
48 RateYaw=(float)GyroZ/65.5;

49 AccX=(float)AccXLSB/4096-0.05
0.05; Do not forget to put
50 AccY=(float)AccYLSB/4096+0.01
+0.01; your own accelerom-
51 AccZ=(float)AccZLSB/4096-0.11
-0.11; eter calibration values
here (project 14)
52 AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*
AccZ))*1/(3.142/180);
53 AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*
AccZ))*1/(3.142/180);
54 }
55 void setup() {
56 Serial.begin(57600);
57 pinMode(13, OUTPUT);
58 digitalWrite(13, HIGH);
59 Wire.setClock(400000);
125
Ca rbon aeronautics
 The Kalman filter - one dimension

60 Wire.begin();
61 delay(250);
62 Wire.beginTransmission(0x68); Communication with
63 Wire.write(0x6B); the gyroscope and
64 Wire.write(0x00); calibration (project 4
65 Wire.endTransmission(); and 5)
66 for (RateCalibrationNumber=0;
RateCalibrationNumber<2000;
RateCalibrationNumber ++) {
67 gyro_signals();
68 RateCalibrationRoll+=RateRoll;
69 RateCalibrationPitch+=RatePitch;

When the rotation rates from the gyro and the angles from the accelerometer are
measured, start the iteration for the Kalman filter. As already seen, the output of the
filter will be the Kalman prediction for the roll and pitch angles together with their
uncertainties.

Angle

accelerometer
4° trigonometry
motor speed increase

2°
motors stopped

0°

kalman filter
-2°
motors started

-4°
5 10 15 20
Time [s]
126
70 RateCalibrationYaw+=RateYaw;
71 delay(1);
72 }
73 RateCalibrationRoll/=2000;
74 RateCalibrationPitch/=2000;
75 RateCalibrationYaw/=2000;
76 LoopTimer=micros();
77 }
78 void loop() {
79 gyro_signals();
80 RateRoll-=RateCalibrationRoll; Calculate the rotation
81 RatePitch-=RateCalibrationPitch; rates
82 RateYaw-=RateCalibrationYaw;
83 kalman_1d(KalmanAngleRoll, Start the iteration
KalmanUncertaintyAngleRoll, RateRoll, AngleRoll); for the Kalman fil-
84 KalmanAngleRoll=Kalman1DOutput[0]; ter with the roll and
KalmanUncertaintyAngleRoll=Kalman1DOutput[1]; pitch angles
85 kalman_1d(KalmanAnglePitch,
KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
86 KalmanAnglePitch=Kalman1DOutput[0];
KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

87 Serial.print("Roll Angle [°] "); Print the predicted

88 Serial.print(KalmanAngleRoll); angle values
89 Serial.print(" Pitch Angle [°] ");
90 Serial.println(KalmanAnglePitch);
91 while (micros() - LoopTimer < 4000);
92 LoopTimer=micros();
93 }

Testing
The measurement results for the angles coming directly from the accelerometer and
the angles as predicted by the Kalman filter are shown in the figure to the left, without
any vibrations from the motors and when the motors are started. In both cases the
quadcopter stayed stationary on the ground; you can observe the noisiness of the
accelerometer values, with angles that vary between plus and minus 3° around the real
quadcopter angle. The angle calculation from the Kalman filter on the other hand
stays very stable and is therefore more suited as input for your new flight controller.

127
Ca rbon aeronautics
 The Kalman filter - one dimension

What is the Kalman gain, physically?

A key element of the Kalman filter is the Kalman gain. This gain weighs the im-
portance of the angle prediction, through the gyro integration, with the measured
angle using the accelerometer. As the gain is a weighting factor, its value always
lies between zero and one. A high Kalman gain gives a large importance to the
measurement (e.g. the accelerometer), while a low Kalman gain gives a larger im-
portance to the prediction (e.g. the integration of the rotation rate).

For the angle Kalman filter, the evolution of the gain in time is given in the fig-
ure below. As you can see, the Kalman gain is high initially, because of the initial
importance of the absolute accelerometer values. But rather quickly, the angle
prediction using the integration of the rotation rate becomes more important.
Essentially the Kalman filter uses the gyroscope integration prediction most of
the time during the flight. The accelerometer pitch angles are used to make sure
that the gyroscope integration does diverge too much from the accelerometer
pitch angles, for example due to drift. You now truly have a method to combine
best of both measurements!
kalman gain
(angle calculation)

0.3

0.2

0.1 steady state ≈ 0.005

0
0 0.2 0.4 0.6 0.8
Time [s]

128
129
Ca rbon aeronautics
 Project 16
The flight controller: stabilize mode

desired angle desired rotation motor power

angle + error Angle rotation rate + rate error Rate command motor
- Controller - Controller power

measured quadcopter
rotation rate rotation rate
gyroscope

kalman
angle measured quadcopter
kalman filter angle angle
Accelerometer

Radiotransmitter
desiredAngleRoll 5a Flysky FS-i6
desiredAnglePitch

desiredAngle = 0.10 (ReceiverValue - 1500)

50°
sWB sWC
sWA sWd

channel 3 channel 2
(throttle) (pitch)

channel 4 channel 1
0° (yaw) (roll)

UP ok
TX
RX
doWn CAnCEL

PoWER
TX.V1 : 4.80 V

-50° IntV1 : 4.99 V

sigs1 : 10
BInd kEy

1000 µs 1500 µs 2000 µs ReceiverValue[0] (=channel 1)

ReceiverValue[1] (=channel 2)

130
 St abilize your quadcopter based on its angles
With your previous flight controller, you stabilized your quadcopter based on
its rotation rates measured by the gyroscope. You experienced that this made
your quadcopter rather difficult to fly. An easier-to-fly flight controller stabiliz-
es your drone based on its angles. To achieve this stabilization, you will use a
so-called cascaded controller.

Flying a quadcopter with a flight controller based on rotation rates is rather difficult,
because each time you have to manually adjust the quadcopter back to a level position;
releasing the roll and pitch sticks stops the rotation rate because the command sent
from the radiotransmitter becomes 0°/s, but the flight angle remains the same and
does not return to 0°. So if you were flying with a roll angle of 30° at the moment
you release the stick, the angle would remain equal to 30°.

You will now implement a controller that stabilizes the quadcopter based on its an-
gles; this will be easier to fly because when you release the roll and pitch stick of the
radiotransmitter, the quadcopter will self-level itself to a roll and pitch angle of 0°.
Angle control is not necessary for the yaw direction, as you usually do not want the
yaw angle to go back to a reference point during flight. To implement roll and pitch
angle control, you will keep the PID controller that you used for the rate mode as the
so-called inner control loop, and add a second PID controller in front that uses the
angles instead of the rotation rates as outer loop. This is a cascaded controller.

The idea for this cascaded controller is illustrated with the figure on the left. You have
already programmed the inner loop with the rate PID controller in your first flight
controller; the desired rotation rate will not be given by the radiotransmitter values
in this case, but by the angle controller through the outer loop. The angle is calculat-
ed using the Kalman filter from the gyroscope and accelerometer measurements as
learned in the previous project. For this controller, only a P term is necessary; the P
values for roll and pitch can be set equal to 2 for good stability & performance.

The last thing that you need to do, is to transform the values sent from the receiver to
physical roll and pitch angles, as you did before with the roll, pitch and yaw rotation
rates. In this case, you will choose the minimal and maximal values for the desired
angles to be -50° and +50°; this will be sufficient to achieve high speeds with your
quadcopter. Since you already programmed an angle Kalman filter and a PID control-
ler has no more secrets to you, you are now ready to develop a stabilize-mode flight
controller.

131
 The flight controller: stabilize mode

Coding
1 #include <Wire.h> Initialize the same
2 float RateRoll, RatePitch, RateYaw; variables that you al-
3 float RateCalibrationRoll, RateCalibrationPitch, ready needed for rate
RateCalibrationYaw; mode (project 12)
4 int RateCalibrationNumber;
5 #include <PulsePosition.h>
6 PulsePositionInput ReceiverInput(RISING);
7 float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0};
8 int ChannelNumber=0;
9 float Voltage, Current, BatteryRemaining, BatteryAtStart;
10 float CurrentConsumed=0;
11 float BatteryDefault=1300;
12 uint32_t LoopTimer;
13 float DesiredRateRoll, DesiredRatePitch,
DesiredRateYaw;
14 float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;
15 float InputRoll, InputThrottle, InputPitch, InputYaw;
16 float PrevErrorRateRoll, PrevErrorRatePitch,
PrevErrorRateYaw;
17 float PrevItermRateRoll, PrevItermRatePitch,
PrevItermRateYaw;
18 float PIDReturn[]={0, 0, 0};
19 float PRateRoll=0.6; float PRatePitch=PRateRoll;
float PRateYaw=2;
20 float IRateRoll=3.5; float IRatePitch=IRateRoll;
float IRateYaw=12;
21 float DRateRoll=0.03; float DRatePitch=DRateRoll;
float DRateYaw=0;
22 float MotorInput1, MotorInput2, MotorInput3,
MotorInput4;
23 float AccX, AccY, AccZ; Initialize the accel-
24 float AngleRoll, AnglePitch; erometer variables
(project 14)

25 float KalmanAngleRoll=0, Define the Kalman

KalmanUncertaintyAngleRoll=2*2; variables (project 15)
26 float KalmanAnglePitch=0,
KalmanUncertaintyAnglePitch=2*2;
27 float Kalman1DOutput[]={0,0};

132
28 float DesiredAngleRoll, DesiredAnglePitch; Define the desired
29 float ErrorAngleRoll, ErrorAnglePitch; roll and pitch angles
and corresponding
errors for the outer
loop PID controller

30 float PrevErrorAngleRoll, PrevErrorAnglePitch; Define the values

31 float PrevItermAngleRoll, PrevItermAnglePitch; necessary for the out-
er loop PID control-
32 float PAngleRoll=2; float PAnglePitch=PAngleRoll; ler, including the P, I
33 float IAngleRoll=0; float IAnglePitch=IAngleRoll; and D parameters
34 float DAngleRoll=0; float DAnglePitch=DAngleRoll;

35 void kalman_1d(float KalmanState, The Kalman filter

float KalmanUncertainty, float KalmanInput, function (project 15)
float KalmanMeasurement) {
36 KalmanState=KalmanState+0.004*KalmanInput;
37 KalmanUncertainty=KalmanUncertainty + 0.004
* 0.004 * 4 * 4;
94 float KalmanGain=KalmanUncertainty * 1/
(1*KalmanUncertainty + 3 * 3);
38 KalmanState=KalmanState+KalmanGain * (
KalmanMeasurement-KalmanState);
39 KalmanUncertainty=(1-KalmanGain) *
KalmanUncertainty;
40 Kalman1DOutput[0]=KalmanState;
Kalman1DOutput[1]=KalmanUncertainty;
41 }

42 void battery_voltage(void) { Battery voltage func-

43 Voltage=(float)analogRead(15)/62; tion (project 9)
44 Current=(float)analogRead(21)*0.089;
45 }
46 void read_receiver(void){ Receiver function
47 ChannelNumber = ReceiverInput.available(); (project 7)
48 if (ChannelNumber > 0) {
49 for (int i=1; i<=ChannelNumber;i++){
50 ReceiverValue[i-1]=ReceiverInput.read(i);
51 }
52 }
53 }

133
Ca rbon aeronautics
 The flight controller: stabilize mode

54 void gyro_signals(void) { Gyro and accelerom-

55 Wire.beginTransmission(0x68); eter function (project
56 Wire.write(0x1A); 14)
57 Wire.write(0x05);
58 Wire.endTransmission();
59 Wire.beginTransmission(0x68);
60 Wire.write(0x1C);
61 Wire.write(0x10);
62 Wire.endTransmission();
63 Wire.beginTransmission(0x68);
64 Wire.write(0x3B);
65 Wire.endTransmission();
66 Wire.requestFrom(0x68,6);
67 int16_t AccXLSB = Wire.read() << 8 |
Wire.read();
68 int16_t AccYLSB = Wire.read() << 8 |
Wire.read();
69 int16_t AccZLSB = Wire.read() << 8 |
Wire.read();
70 Wire.beginTransmission(0x68);
71 Wire.write(0x1B);
72 Wire.write(0x8);
73 Wire.endTransmission();
74 Wire.beginTransmission(0x68);
75 Wire.write(0x43);
76 Wire.endTransmission();
77 Wire.requestFrom(0x68,6);
78 int16_t GyroX=Wire.read()<<8 | Wire.read();
79 int16_t GyroY=Wire.read()<<8 | Wire.read();
80 int16_t GyroZ=Wire.read()<<8 | Wire.read();
81 RateRoll=(float)GyroX/65.5;
82 RatePitch=(float)GyroY/65.5;
83 RateYaw=(float)GyroZ/65.5;

84 AccX=(float)AccXLSB/4096-0.05
0.05; Do not forget to put
85 AccY=(float)AccYLSB/4096+0.01
+0.01; your own accelerom-
86 AccZ=(float)AccZLSB/4096-0.11
-0.11; eter calibration values
here (project 14)
87 AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*
AccZ))*1/(3.142/180);

134
88 AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*
AccZ))*1/(3.142/180);
89 }
90 void pid_equation(float Error, float P , float I, float D, PID function (pro-
float PrevError, float PrevIterm) { ject 12)
91 float Pterm=P*Error;
92 float Iterm=PrevIterm+I*(Error+
PrevError)*0.004/2;
93 if (Iterm > 400) Iterm=400;
94 else if (Iterm <-400) Iterm=-400;
95 float Dterm=D*(Error-PrevError)/0.004;
96 float PIDOutput= Pterm+Iterm+Dterm;
97 if (PIDOutput>400) PIDOutput=400;
98 else if (PIDOutput <-400) PIDOutput=-400;
99 PIDReturn[0]=PIDOutput;
100 PIDReturn[1]=Error;
101 PIDReturn[2]=Iterm;
102 }
103 void reset_pid(void) {
104 PrevErrorRateRoll=0; PrevErrorRatePitch=0; PID reset function
PrevErrorRateYaw=0; (project 12)
105 PrevItermRateRoll=0; PrevItermRatePitch=0;
PrevItermRateYaw=0;

106 PrevErrorAngleRoll=0; PrevErrorAnglePitch=0; Reset the PID error

107 PrevItermAngleRoll=0; PrevItermAnglePitch=0; and integral values
108 } for the outer PID
loop as well
109 void setup() {
110 pinMode(5, OUTPUT); Visualize the setup
111 digitalWrite(5, HIGH); phase using the red
112 pinMode(13, OUTPUT); LED
113 digitalWrite(13, HIGH);

114 Wire.setClock(400000); Communication with

115 Wire.begin(); the gyroscope and
116 delay(250); calibration (project 4
117 Wire.beginTransmission(0x68); and 5)
118 Wire.write(0x6B);
119 Wire.write(0x00);
120 Wire.endTransmission();

135
Ca rbon aeronautics
 The flight controller: stabilize mode

121 for (RateCalibrationNumber=0;

RateCalibrationNumber<2000;
RateCalibrationNumber ++) {
122 gyro_signals();
123 RateCalibrationRoll+=RateRoll;
124 RateCalibrationPitch+=RatePitch;
125 RateCalibrationYaw+=RateYaw;
126 delay(1);
127 }
128 RateCalibrationRoll/=2000;
129 RateCalibrationPitch/=2000;
130 RateCalibrationYaw/=2000;

131 analogWriteFrequency(1, 250); Set the PWM fre-

132 analogWriteFrequency(2, 250); quency to 250 Hz
133 analogWriteFrequency(3, 250); and the resolution to
134 analogWriteFrequency(4, 250); 12 bit for all motors
135 analogWriteResolution(12); (project 8)

136 pinMode(6, OUTPUT); Show the end of the

137 digitalWrite(6, HIGH); setup process and
138 battery_voltage(); determine the initial
139 if (Voltage > 8.3) { digitalWrite(5, LOW); battery voltage per-
140 BatteryAtStart=BatteryDefault; } centage (project 9)
141 else if (Voltage < 7.5) {
142 BatteryAtStart=30/100*BatteryDefault ;}
143 else { digitalWrite(5, LOW);
144 BatteryAtStart=(82*Voltage-580)/100*
BatteryDefault; }

145 ReceiverInput.begin(14); SAFETY RELAT-

146 while (ReceiverValue[2] < 1020 || ED LINES: Avoid
ReceiverValue[2] > 1050) { accidental lift off af-
147 read_receiver(); ter the setup process
148 delay(4); (project 12)
149 }
150 LoopTimer=micros();
151 }

136
152 void loop() {
153 gyro_signals(); Measure the rotation
154 RateRoll-=RateCalibrationRoll; rates and subtract
155 RatePitch-=RateCalibrationPitch; the calibration values
156 RateYaw-=RateCalibrationYaw; (project 5)

157 kalman_1d(KalmanAngleRoll, Calculate the roll and

KalmanUncertaintyAngleRoll, RateRoll, AngleRoll); pitch angles through
158 KalmanAngleRoll=Kalman1DOutput[0]; the Kalman filter
KalmanUncertaintyAngleRoll=Kalman1DOutput[1]; (project 15)
159 kalman_1d(KalmanAnglePitch,
KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
160 KalmanAnglePitch=Kalman1DOutput[0];
KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

161 read_receiver();

162 DesiredAngleRoll=0.10*(ReceiverValue[0]-1500); Calculate the desired

163 DesiredAnglePitch=0.10*(ReceiverValue[1]-1500); angles from the re-
ceiver values
164 InputThrottle=ReceiverValue[2];
165 DesiredRateYaw=0.15*(ReceiverValue[3]-1500);

166 ErrorAngleRoll=DesiredAngleRoll- Calculate the differ-

KalmanAngleRoll; ence between the de-
167 ErrorAnglePitch=DesiredAnglePitch- sired and the actual
KalmanAnglePitch; roll and pitch angles

168 pid_equation(ErrorAngleRoll, PAngleRoll, Calculate the desired

IAngleRoll, DAngleRoll, PrevErrorAngleRoll, roll and pitch angles
PrevItermAngleRoll); through the outer
169 DesiredRateRoll=PIDReturn[0]; loop PID controller
PrevErrorAngleRoll=PIDReturn[1];
PrevItermAngleRoll=PIDReturn[2];
170 pid_equation(ErrorAnglePitch, PAnglePitch,
IAnglePitch, DAnglePitch, PrevErrorAnglePitch,
PrevItermAnglePitch);
171 DesiredRatePitch=PIDReturn[0];
PrevErrorAnglePitch=PIDReturn[1];
PrevItermAnglePitch=PIDReturn[2];

137
Ca rbon aeronautics
 The flight controller: stabilize mode

172 ErrorRateRoll=DesiredRateRoll-RateRoll; Calculate the differ-

173 ErrorRatePitch=DesiredRatePitch-RatePitch; ence between the de-
174 ErrorRateYaw=DesiredRateYaw-RateYaw; sired and the actual
roll, pitch and yaw
175 pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, rotation rates. Use
DRateRoll, PrevErrorRateRoll, these for the PID
PrevItermRateRoll); controller of the in-
176 InputRoll=PIDReturn[0]; ner loop (project 12)
PrevErrorRateRoll=PIDReturn[1];
PrevItermRateRoll=PIDReturn[2];
177 pid_equation(ErrorRatePitch, PRatePitch,
IRatePitch, DRatePitch, PrevErrorRatePitch,
PrevItermRatePitch);
178 InputPitch=PIDReturn[0];
PrevErrorRatePitch=PIDReturn[1];
PrevItermRatePitch=PIDReturn[2];
179 pid_equation(ErrorRateYaw, PRateYaw,
IRateYaw, DRateYaw, PrevErrorRateYaw,
PrevItermRateYaw);
180 InputYaw=PIDReturn[0];
PrevErrorRateYaw=PIDReturn[1];
PrevItermRateYaw=PIDReturn[2];

181 if (InputThrottle > 1800) InputThrottle = 1800; Limit the throttle val-
ue to 80% (project
12)

173 MotorInput1= 1.024*(InputThrottle-InputRoll Use the quadcopter

-InputPitch-InputYaw); dynamics equations
174 MotorInput2= 1.024*(InputThrottle-InputRoll (project 11)
+InputPitch+InputYaw);
175 MotorInput3= 1.024*(InputThrottle+InputRoll
+InputPitch-InputYaw);
176 MotorInput4= 1.024*(InputThrottle+InputRoll
-InputPitch+InputYaw);

182 if (MotorInput1 > 2000)MotorInput1 = 1999; Limit the maximal

183 if (MotorInput2 > 2000)MotorInput2 = 1999; power commands
184 if (MotorInput3 > 2000)MotorInput3 = 1999; sent to the motors
185 if (MotorInput4 > 2000)MotorInput4 = 1999; (project 12)

138
186 int ThrottleIdle=1180; Keep the quadcopter
187 if (MotorInput1 < ThrottleIdle) MotorInput1 = motors running at
ThrottleIdle; minimally 18% pow-
188 if (MotorInput2 < ThrottleIdle) MotorInput2 = er during flight
ThrottleIdle;
189 if (MotorInput3 < ThrottleIdle) MotorInput3 =
ThrottleIdle;
190 if (MotorInput4 < ThrottleIdle) MotorInput4 =
ThrottleIdle;

191 int ThrottleCutOff=1000; SAFETY RELAT-

192 if (ReceiverValue[2]<1050) { ED LINES: make
193 MotorInput1=ThrottleCutOff; sure you are able to
194 MotorInput2=ThrottleCutOff; turn off the motors
195 MotorInput3=ThrottleCutOff;
196 MotorInput4=ThrottleCutOff;
197 reset_pid();
198 }
199 analogWrite(1,MotorInput1); Sent the commands
200 analogWrite(2,MotorInput2); to the motors
201 analogWrite(3,MotorInput3);
202 analogWrite(4,MotorInput4);

203 battery_voltage(); Keep track of battery

204 CurrentConsumed=Current*1000*0.004/3600+ level (project 9)
CurrentConsumed;
205 BatteryRemaining=(BatteryAtStart-
CurrentConsumed)/BatteryDefault*100;
206 if (BatteryRemaining<=30) digitalWrite(5, HIGH);
207 else digitalWrite(5, LOW);

208 while (micros() - LoopTimer < 4000); Finish the 250 Hz

209 LoopTimer=micros(); control loop
210 }

Start-up and flying your quadcopter

To start and fly your quadcopter with the new stabilize-mode controller, follow the
same steps as you did with your rate-mode controller. You should notice that fly-
ing the quadcopter with your new flight controller is much easier than with the old
one. To further simplify flying, a third and final flight controller will be developed
in the next part to give you a better control over the altitude of the quadcopter.
139
Ca rbon aeronautics
Part III: velocity mode
stabilization mode is a major step to re-
duce the pilot’s effort necessary to keep the
quadcopter in the air. But there is one fea-
ture which would make flying even more
easy; being able to hold your altitude auto-
matically.

in this third part, you will explore the pos-

sibilities of a barometric sensor and adapt
your flight controller accordingly.

this flight controller will be optimized for

indoor navigation only; outdoors the need
for such a controller is limited, because you
generally stay a lot higher above ground.
 Project 17
Measuring altitude

+ - a b c d e f g h i j + -
1 1
2 2
3 3
4 4
5 5
6 6
7 7
8 8
9 9
10 10
VCC
11 11
GND
12 12
SCL
13 13
SDA
14 14
CSB
15 15
SDC
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
20 20
21 21
22 22
23 23
24 24
Teensy
25 25
2 3 4

26 26
14 11
27 27 15 10
28 28 16 9
3V

29 29 17 8
0

30 5V 30 18 7
G

+ - a b c d e + -
f g h i j
19 6
20 5
21 4
22 3
2
BMP-280 VCC 3V
GND 0
SCL 5V G
SDA
CSB
SDC

142
Us e a barometer to measure the altitude
To help you holding your quadcopter at a constant altitude, you first need to be
able to measure the altitude. You will use a barometric sensor for the altitude
measurements: the BMP-280.

A barometric sensor is a sensor that measures the atmospheric pressure. Because the
pressure decreases with increasing altitude, the relation between both can be used to
measure the altitude. One of the advantages of a barometric sensor is its ability to
detect very small pressure changes, making it a suitable measurement for your flight
controller. The relation between the atmospheric pressure and the altitude is given
through the barometric formula, which assumes in its standard form a constant tem-
perature of 15°C and a standard pressure at sea level of 1013.25 hPa:
   1 
pressure 5.255
altitude = 44330 · 1 −
1013.25
Where the altitude is given in meter and the pressure in hPa. Off course, the tem-
perature when flying your drone AccisZ,inot always
= −Acc 15°C and the pressure at sea level also
X · sin(θpitch )
differs from 1013.25 hPa, depending on the weather. However, since you are only
interested in the relative change of altitude between startup and a certain position,
both the actual temperature and· cos(θ
AccZ,i = AccZ,1 pressure atAcc
pitch ) = seaY level does
· sin(ϕ roll ) ·not matter
cos(θ pitch ) for your flight

controller. The relation between the altitude and pressure as given in the equation is
plotted below.
AccZ,i = AccZ,1 · cos(θpitch ) = AccZ · cos(ϕroll ) · cos(θpitch )
Altitude [m]

AccZ,i = −AccX ·sin(θpitch )+AccY ·sin(ϕroll )·cos(θpitch )+AccZ ·cos(ϕroll )·cos(θpitch )

12 000

8000
standard pressure
1013.25 hPa
4000

0
200 400 600 800 1000
Atmospheric pressure [hPa]
Now you understand this essential piece of theory, let’s connect the BMP-280 pres-
sure sensor to your Teensy. Normally you already installed the sensor on your quad-
copter: you can either choose to test directly on the printed circuit board of your
quadcopter, or you can connect the sensor separately using your breadboard.
143
 Measuring altitude

As with your MPU-6050, the SCL and SDA pins are connected to pins 19 and 18 of
the Teensy respectively to be able to communicate through the I2C protocol. Because
the BMP-280 sensor has a different address than the MPU-6050 sensor, you can use
the same Teensy pins for communication with both sensors. Connect the ground of
the sensor to the ground of the sensor. Be very carefull to connect the VCC pin
of the sensor to the 3V output on your Teensy, not the 5V output! The BMP-280
sensor is only 3V tolerant so powering it with 5V might damage it. When you’re all
wired up, let’s start programming.

Coding
Each individual sensor is calibrated beforehand by the manufacturer. The calibration
values are stored on the sensor’s memory in the form of twelve trimming parameters;
three for the temperature and nine for the pressure. They are all 16-bit signed or
unsigned integers (see datasheet BMP-280). The names of the variables used in this
part of the code correspond to the names in the datasheet of the BMP-280 sensor.

Define the altitude measured by the barometer as a global variable, together with the
altitude at startup. To have a steady value for the altitude at startup, the average of
a large integer number of altitude readouts will be taken (RateCalibrationNumber).

The I2C address for the BMP-280 is 0x76. The pressure and temperature data is read
by starting a burst read from the 6 registers 0xF7 to 0xFC; the measurement of the
raw temperature and pressure is spread out over three registers each. Request 6 bytes
to read the registers; the data comes in unsigned 32 bit format.

The three registers for the temperature and three for the pressure are combined to
form the raw, uncompensated and uncalibrated pressure (adc_P) and temperature
(adc_T). The msb register contains bits 19 to 12, the LSB register contains bits 11 to
4 and the xlsb register contains bits 3 to 0 of the raw measurements.

144
1 #include <Wire.h>

2 uint16_t dig_T1, dig_P1; Define the pressure

3 int16_t dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5; sensor calibration
4 int16_t dig_P6, dig_P7, dig_P8, dig_P9; values

5 float AltitudeBarometer, AltitudeBarometerStartUp; Define the altitude

6 int RateCalibrationNumber; variables

7 void barometer_signals(void){

8 Wire.beginTransmission(0x76); Make connection

9 Wire.write(0xF7); with the pressure
10 Wire.endTransmission(); sensor and read the
11 Wire.requestFrom(0x76,6); raw uncombined
12 uint32_t press_msb = Wire.read(); pressure and temper-
13 uint32_t press_lsb = Wire.read(); ature measurements
14 uint32_t press_xlsb = Wire.read();
15 uint32_t temp_msb = Wire.read();
16 uint32_t temp_lsb = Wire.read();
17 uint32_t temp_xlsb = Wire.read();

18 unsigned long int adc_P = (press_msb << 12) | ( Construct the raw
press_lsb << 4) | (press_xlsb >>4); temperature and
19 unsigned long int adc_T = (temp_msb << 12) | ( pressure measure-
temp_lsb << 4) | (temp_xlsb >>4); ments

145
Ca rbon aeronautics
 Measuring altitude

To calculate the compensated and calibrated pressure, first the fine resolution temper-
ature value t_fine needs to be determined from the raw temperature values and the
trimming parameters. These calculations are entirely given by the manufacturer in the
datasheet of the BMP-280 and are therefore not further explained here.

The compensated and calibrated pressure p (in Pa) is calculated with these lines from
the raw pressure values and the trimming parameters. Once again, these lines are en-
tirely given by the manufacturer in the datasheet of the BMP-280 and are therefore
not further explained here.

Convert the pressure in Pa to the pressure in hPa and calculate the altitude from the
standard pressure to the barometric formula. Multiply by 100 to convert from meter
to centimetre. This marks the end of the barometric function; continue with the setup
part of the code.

146
20 signed long int var1, var2; Construct the fine
21 var1 = ((((adc_T >> 3) - ((signed long int )dig_T1 resolution tempera-
<<1)))* ((signed long int )dig_T2)) >> 11; ture value
22 var2 = (((((adc_T >> 4) - ((signed long int )dig_T1
)) * ((adc_T>>4) - ((signed long int )dig_T1)))
>> 12) * ((signed long int )dig_T3)) >> 14;
23 signed long int t_fine = var1 + var2;

24 unsigned long int p; Construct the com-

25 var1 = (((signed long int )t_fine)>>1) - (signed pensated and cali-
long int )64000; brated pressure p
26 var2 = (((var1>>2) * (var1>>2)) >> 11) * ((signed
long int )dig_P6);
27 var2 = var2 + ((var1*((signed long int )dig_P5))
<<1);
28 var2 = (var2>>2)+(((signed long int )dig_P4)
<<16);
29 var1 = (((dig_P3 * (((var1>>2)*(var1>>2)) >> 13
))>>3)+((((signed long int )dig_P2) *
var1)>>1))>>18;
30 var1 = ((((32768+var1))*((signed long int )dig_P1))
>>15);
31 if (var1 == 0) { p=0;}
32 p = (((unsigned long int )(((signed long int )
1048576)-adc_P)-(var2>>12)))*3125;
33 if(p<0x80000000){ p = (p << 1) / ((unsigned
long int ) var1);}
34 else { p = (p / (unsigned long int )var1) * 2; }
35 var1 = (((signed long int )dig_P9) * ((signed long
int ) (((p>>3) * (p>>3))>>13)))>>12;
36 var2 = (((signed long int )(p>>2)) *
((signed long int )dig_P8))>>13;
37 p = (unsigned long int)((signed long int )p +
((var1 + var2+ dig_P7) >> 4));

38 double pressure=(double)p/100; Calculate the altitude

39 AltitudeBarometer=44330*(1-pow(pressure
/1013.25, 1/5.255))*100;
40 }

147
Ca rbon aeronautics
 Measuring altitude

In the setup phase, you configure the BMP-280 in such a manner that the sensor is
optimized for indoor navigation. The datasheet recommends to set the power mode
of the sensor in normal mode, with an oversampling setting for the pressure (osrs_p)
of x16 and the similar setting for the temperature (osrs_t) of x2. According to the
datasheet, these settings correspond to osrs_t[2:0] bits of 010, osrs_p[2:0] bits of 101
and mode [1:0] bits of 11 in the data acquisition control register 0xF4, which has a
layout that is visualised in the table:
Register Register
Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0
(Hex) (Decimal)
F4 244 osrs_t[2:0] osrs_p[2:0] mode[1:0]

Binary representation 0 1 0 1 0 1 1 1
Converting the resulting binary representation of 01010111 to a hexadecimal value
gives an address of 0x57.

The configuration register 0xF5, with a layout shown in the second table, sets the
standby time t_sb[2:0], the internal IIR filter filter[2:0] and the SPI interface spi3w_
en[0]. For indoor navigation, the manufacturer recommends to set the IIR filter co-
efficient to 16 (101). As you do not use the SPI interface and the standby time is only
helpful to reduce the power the device needs (which is anyway much smaller than the
power the motors need), these remain on their default values (0):
Register Register
Bit7 Bit6 Bit5 Bit4 Bit3 Bit2 Bit1 Bit0
(Hex) (Decimal)
F5 245 t_sb[2:0] filter[2:0] read only spi3w_en[0]

Binary representation 0 0 0 1 0 1 0 0
Converting the resulting binary representation of 00010100 to a hexadecimal value
gives an address of 0x14.

Import the twelve calibration (e.g. trimming) parameters from the sensor’s memory.
As they are stored in two’s complement, you need to foresee 2x12 or 24 variables. The
i variable will be used to indicate the trimming parameters in the subsequent while
loop during import.

148
41 void setup() {
42 Serial.begin(57600);
43 pinMode(13, OUTPUT);
44 digitalWrite(13, HIGH);
45 Wire.setClock(400000);
46 Wire.begin();
47 delay(250);

48 Wire.beginTransmission(0x76); Optimize the barom-

49 Wire.write(0xF4); eter for indoor navi-
50 Wire.write(0x57); gation
51 Wire.endTransmission();

52 Wire.beginTransmission(0x76); Setup the configura-

53 Wire.write(0xF5); tion register
54 Wire.write(0x14);
55 Wire.endTransmission();

56 uint8_t data[24], i=0; Import the calibra-

tion parameters

149
Ca rbon aeronautics
 Measuring altitude

The register address of the first trimming parameter is 0x88 according to the data-
sheet. Request 24 bytes such that you can pull the information from the 24 registers
0x88 to 0x9E.

Rearrange the trimming parameters, that are split in their two’s complement values,
such that they are readable in one single parameter. You need to carry out this step
for all twelve parameters.

Before you will start your quadcopter, you need the altitude level from which you take
off. Take the average of 2000 iterations to get a steady altitude reference level.

Now it is finally time to read the barometer in the loop part. Call the function and
subtract the average startup altitude to get the altitude variation in flight.

150
57 Wire.beginTransmission(0x76);
58 Wire.write(0x88);
59 Wire.endTransmission();
60 Wire.requestFrom(0x76,24);
61 while(Wire.available()){
62 data[i] = Wire.read();
63 i++;
64 }
65 dig_T1 = (data[1] << 8) | data[0];
66 dig_T2 = (data[3] << 8) | data[2];
67 dig_T3 = (data[5] << 8) | data[4];
68 dig_P1 = (data[7] << 8) | data[6];
69 dig_P2 = (data[9] << 8) | data[8];
70 dig_P3 = (data[11]<< 8) | data[10];
71 dig_P4 = (data[13]<< 8) | data[12];
72 dig_P5 = (data[15]<< 8) | data[14];
73 dig_P6 = (data[17]<< 8) | data[16];
74 dig_P7 = (data[19]<< 8) | data[18];
75 dig_P8 = (data[21]<< 8) | data[20];
76 dig_P9 = (data[23]<< 8) | data[22]; delay(250);

77 for (RateCalibrationNumber=0; Calculate the altitude

RateCalibrationNumber<2000; reference level
RateCalibrationNumber ++) {
78 barometer_signals();
79 AltitudeBarometerStartUp+=
AltitudeBarometer;
80 delay(1);
81 }
82 AltitudeBarometerStartUp/=2000;
83 }

84 void loop() { Read the barometer

85 barometer_signals(); and print the altitudes
86 AltitudeBarometer-=AltitudeBarometerStartUp;
87 Serial.print("Altitude [cm]: ");
88 Serial.println(AltitudeBarometer);
89 delay(50);
90 }

151
Ca rbon aeronautics
 Measuring altitude

Testing the barometric sensor

Test the barometric sensor by moving your breadboard or quadcopter up and
down. You will notice that the readings do not change very fast, are not very con-
stant over a longer time and are overall not very accurate. This is illustrated by the
figure below for changes in altitude between 50 cm and -30 cm. The reason for this
poor performance are rapid pressure changes in the atmosphere, for example due
to wind gusts or opening/closing windows when flying indoors.

The ‘jumpy’ performance of the sensor readings also mean that if you only use a
barometer for your vertical velocity PID, its performance will not be very good. By
now you have probably guessed already how you can solve this issue; use Kalman
filter with another, complimentary measurement. In this case, the additional meas-
urement will be the vertical velocity, obtained through the accelerometer.

Altitude [cm]
barometric sensor
100
real altitude

50

-50

-100
0 5 10 15 20 25 30
Time [s]

152
153
Ca rbon aeronautics
 Accy,i

AccX,i

Accz,i

Project 18 cos θpitch =

Accz,1
Pitch around the inertial y axis
with angle θpitch

Measuring vertical velocity

-Acc
Acc
X,i
Accz,i Accy,i
z,1
θpitch
Accz,i

Accy,i
AccX,i -Accz,i
θpitch sin θpitch =
AccX

-Accz,i
AccX,i AccX

Accz,i z,1
Acc
cos θpitch = Pitch aroundcos
theφinertial
= y axis Roll around the new X axis
Accz,1 roll
with angle θpitch Accz
with angle φroll
-AccX,i
Accz,i Accy,i Accz,1 Accz,i Accy Accz,1 Accz,1
Accz,1 sin φroll =
θpitch
Accy,i Accy
-Accy,i φroll φroll

Accz
AccX,i -Accz,i
θpitch sin θpitch =
AccX
AccX,i
-Accz,i
AccX
AccX
Accz,1
cos φroll = Roll around the new X axis
Accz
with angle φroll

Accz,1 Accz,i Accy Accz,1 Accz,1

sin φroll =
Accy,i Accy
-Accy,i φroll φroll

Accz
Accz,i = -AccX . sin θpitch
AccX,i
Accz,i = Accz,1 . cos θpitch = Accy . sin φroll . cos θpitch
AccX
Accz,i = Accz,1 . cos θpitch = Accz . cos φroll . cos θpitch

154
Me asure vertical velocity with an accelerometer
The results from measuring the altitude using your barometer are not suffi-
ciently accurate. Fortunately, your quadcopter hovers not only when the alti-
tude stays the same, but also when the vertical velocity is equal to zero. Meas-
uring this vertical velocity is surprisingly easy using your accelerometer.

Imagine that there exist three inertial directions for the acceleration of your quad-
copter: AccX,i, AccY,i and AccZ,i. These are always aligned respectively horizontally and
vertically to the surface of the earth and are coloured in red in the figure to the left.
If your accelerometer or quadcopter turns, these red inertial axes keep pointing in
the same direction. This means that AccZ,i will always be perpendicular to the earth’s
surface and is hence suitable for measuring the vertical velocity after integration.

When your quadcopter and its accelerometer rolls and pitches, it does not measure
the acceleration in its inertial axes anymore, but it measures along the axes that are
defined on the accelerometer itself. These are the acceleration vectors AccX, AccY
and AccZ. All three vectors will have a component that can be related back to the
acceleration along the inertial Z axis AccZ,i. Using basic trigonometry and the roll φrol
and pitch θpitch angles, you can calculate thetotal acceleration
 1 
along the inertial Z axis
AccZ,i by adding thealtitude
components of the pressure Acc
acceleration 5.255
, Acc and AccZ along this
= 44330 · 1 −  pressure  5.255
 1 X 
Y
axis. This is visualized on the
altitude figure· to
= 44330 1 the 1013.25
left. The acceleration
−  pressure 1  in the X direction
1 − 1013.25
5.255
altitude = 44330
gives the following component in the· inertial Z axis:
 1013.25  1 
pressure 5.255
altitude
Acc =Z,i
44330 · 1X
= −Acc − · sin(θpitch )
1013.25
AccZ,i = −AccX · sin(θpitch )
The acceleration in the YAccandZ,iZ=direction of the
−AccX · sin(θ )
pitchaccelerometer give the following
componentsAcc
in Z,i
the=inertial Z
AccZ,1Acc axis:
Z,i =
· cos(θ pitch ) = X
−Acc Acc · sin(θ pitch )roll ) · cos(θpitch )
Y · sin(ϕ
AccZ,i = AccZ,1 · cos(θpitch ) = AccY · sin(ϕroll ) · cos(θpitch )
AccZ,i = AccZ,1 · cos(θpitch ) = AccY · sin(ϕroll ) · cos(θpitch )
Acc
AccZ,i = Acc Z,1 ·· cos(θ
Z,i = AccZ,1 cos(θpitch ) = Acc Z ·· cos(ϕ
pitch ) = AccY
) ·· cos(θ
roll)
sin(ϕroll )
pitch)
cos(θpitch
AccZ,i = AccZ,1 · cos(θpitch ) = AccZ · cos(ϕroll ) · cos(θpitch )
AccZ,i = AccZ,1 · cos(θpitch ) = AccZ · cos(ϕroll ) · cos(θpitch )
This means
Acc thatZ,ithe
Z,i = −Acc
Acc = total acceleration
AccZ,1
X ·sin(θpitch
·)+Acc )in
=the
Accinertial
Y ·sin(ϕroll )·cos(θ
cos(θpitch Zroll
Z · cos(ϕ axis
pitch
) · can
)+Acc be
cos(θ calculated
Z ·cos(ϕ
pitch bypitch
) roll )·cos(θ adding
)
all Acc
separate components:
Z,i = −AccX ·sin(θpitch )+AccY ·sin(ϕroll )·cos(θpitch )+AccZ ·cos(ϕroll )·cos(θpitch )
AccZ,i = −AccX ·sin(θpitch )+AccY ·sin(ϕroll )·cos(θpitch )+AccZ ·cos(ϕroll )·cos(θpitch )
AccZ,i = −AccX ·sin(θpitch )+AccY ·sin(ϕroll )·cos(θpitch )+AccZ ·cos(ϕroll )·cos(θpitch )

Let's test this mathematical representation for the acceleration in the inertial Z axis
with the MPU-6050 and your Teensy.

155
 Measuring vertical velocity

Coding
To run this code, you once again need to connect your accelerometer to your Teensy.
You can test this part either on a breadboard or directly on the printed circuit board
of your quadcopter.

Define two additional variables for this part: the acceleration in the inertial Z axis
AccZ,i and the velocity in the inertial Z direction, which will be obtained by integrating
AccZ,i.

Teensy
14 11
MPU-6050 VCC 15 10
GND 16 9
SCL 17 8
SDA 18 7
XDA 19 6
XCL 20 5
ADO 21 4
INT 22 3
2
3V
+ - a b c d e f g h i j + - 0
1 1
5V G
2 2
3 3
VCC
4 4
GND
5 5
SCL
6 6
SDA
7 XDA
7
8 XCL
8
Y
9 ADO
9
10 10
INT
X

11 11
12 12
13 13
14 14
15 15
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
20 20
21 21
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e + -
f g h i j

156
1 #include <Wire.h> Initialize the gyro and
2 float RateRoll, RatePitch, RateYaw; accelerometer varia-
3 float AngleRoll, AnglePitch; bles (project 14)
4 float AccX, AccY, AccZ;

5 float AccZInertial; Define the accelera-

6 float VelocityVertical; tion and velocity var-
iables

7 float LoopTimer;
8 void gyro_signals(void) { Define the gyro/ac-
9 Wire.beginTransmission(0x68); celerometer function
10 Wire.write(0x1A); (project 14)
11 Wire.write(0x05);
12 Wire.endTransmission();
13 Wire.beginTransmission(0x68);
14 Wire.write(0x1C);
15 Wire.write(0x10);
16 Wire.endTransmission();
17 Wire.beginTransmission(0x68);
18 Wire.write(0x3B);
19 Wire.endTransmission();
20 Wire.requestFrom(0x68,6);
21 int16_t AccXLSB = Wire.read() << 8 |
Wire.read();
22 int16_t AccYLSB = Wire.read() << 8 |
Wire.read();
23 int16_t AccZLSB = Wire.read() << 8 |
Wire.read();
24 Wire.beginTransmission(0x68);
25 Wire.write(0x1B);
26 Wire.write(0x8);
27 Wire.endTransmission();
28 Wire.beginTransmission(0x68);
29 Wire.write(0x43);
30 Wire.endTransmission();
31 Wire.requestFrom(0x68,6);
32 int16_t GyroX=Wire.read()<<8 | Wire.read();
33 int16_t GyroY=Wire.read()<<8 | Wire.read();
34 int16_t GyroZ=Wire.read()<<8 | Wire.read();

157
Ca rbon aeronautics
 Measuring vertical velocity

35 RateRoll=(float)GyroX/65.5;
36 RatePitch=(float)GyroY/65.5;
37 RateYaw=(float)GyroZ/65.5;

38 AccX=(float)AccXLSB/4096-0.05
0.05; Do not forget to put
39 AccY=(float)AccYLSB/4096+0.01
+0.01; your own accelerom-
40 AccZ=(float)AccZLSB/4096-0.11
-0.11; eter calibration values
here (project 14)
41 AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*
AccZ))*1/(3.142/180);
42 AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*
AccZ))*1/(3.142/180);
43 }

The acceleration in the inertial Z axis is calculated through the formula you have
developed on the first page of this project. Do not forget to transform the roll and
pitch angles from degrees to radians by multiplying them with �/180, as the sines and
cosines functions in Arduino only accept radians. The resulting acceleration AccZIn-
ertial has the same units as AccX, AccY and AccZ, namely the gravitational constant
equivalent g. Be mindful that for the flight controller, the angles will come from the
one-dimensional Kalman filter to eliminate the effect of the quadcopter vibrations.

The acceleration in the inertial Z axis is equal to 1 g even when the accelerometer is
stationary because of the gravitation. Therefore you need to subtract 1 g from the ac-
celeration in order to obtain the values for the vertical acceleration of the quadcopter.
Because 1 g is equal to 9.81 m/s², you can multiply the acceleration with this constant
to get the values in m/s². The unit m/s² is too large to be practical so multiply the
value with 100 cm/m to get the acceleration in cm/s².

To obtain the vertical velocity, perform an integration by adding the previous velocity
to the acceleration multiplied with the length of one loop, 0.004 seconds. You now
have the velocity in cm/s.

Print out the value for the vertical velocity to be able to test your code.

158
44 void setup() {
45 Serial.begin(57600);
46 pinMode(13, OUTPUT);
47 digitalWrite(13, HIGH);
48 Wire.setClock(400000); Communication with
49 Wire.begin(); the gyroscope and
50 delay(250); calibration (project 4
51 Wire.beginTransmission(0x68); and 5)
52 Wire.write(0x6B);
53 Wire.write(0x00);
54 Wire.endTransmission();
55 }
56 void loop() {
57 gyro_signals();
58 AccZInertial=-sin(AnglePitch*(3.142/180))*AccX Calculate the acceler-
+cos(AnglePitch*(3.142/180))*sin(AngleRoll* ation in the inertial Z
(3.142/180))* AccY+cos(AnglePitch*(3.142/180))* axis
cos(AngleRoll*(3.142/180))*AccZ;

59 AccZInertial=(AccZInertial-1)*9.81*100; Convert the accelera-

tion to cm/s²

60 VelocityVertical=VelocityVertical Calculate and print

+AccZInertial*0.004; the vertical velocity

61 Serial.print("Vertical velocity [cm/s]: ");

62 Serial.println(VelocityVertical);

63 while (micros() - LoopTimer < 4000);

64 LoopTimer=micros();
65 }

159
Ca rbon aeronautics
 Measuring vertical velocity

Testing the vertical velocity code

When testing the code on the previous pages, you will notice that the calculated
vertical velocity changes linearly with time even when the accelerometer/quad-
copter is not moving. The rate of change depends on how well you performed
your accelerometer calibration as this is once again an example of accumulation
of small integration errors. This issue is represented in the figure below, where
the vertical velocity calculated through accelerometer integration is visualized, as
opposed to the real vertical velocity. It is clear that the error on the vertical velocity
becomes so large after a couple of seconds, that this method of measuring and
calculating the vertical velocity gives not sufficiently satisfactory results to use in
your control system. You will use a second Kalman filter to combine the altitude
and vertical velocity measurements in order to obtain an accurate value for the
vertical velocity.

Vertical velocity [cm/s]

accelerometer integration
150
100
50 real velocity

0
-50

-100
0 5 10 15
Time [s]

160
161
Ca rbon aeronautics
 Project 19
The Kalman filter - two dimensions
+ - a b c d e f g h i j + -
1 1
VCC
2 2
GND
3 3
SCL
4 4
SDA
5 5
XDA
6 6
XCL
Y
7 7
ADO
8 8
INT
X

9 9
10 10
VCC
11 11
GND
12 12
SCL
13 13
SDA
14 14
CSB
15 15
SDC
16 16
17 17
18 18
22 21 20 19 18 17 16 15 14

5 6 7 8 9 10 11

19 19
20 20
21 21
22 22
23 23
24 24
25 25
2 3 4

26 26
27 27
28 28
3V

29 29
0

30 5V 30
G

+ - a b c d e f g h i j
+ - Teensy
14 11
MPU-6050 VCC 15 10
GND 16 9
SCL 17 8
SDA 18 7
XDA 19 6
XCL 20 5
ADO 21 4
INT 22 3
2
BMP-280 VCC 3V
GND 0
SCL 5V G
SDA
CSB
SDC

162
 De termine the vertical velocity accurately
Just as you did when measuring the roll and pitch angles, you will use another
Kalman filter to combine the accelerometer and barometer measurements in
order to obtain the vertical velocity. Because the state of your system contains
two variables - the vertical velocity and the altitude - this Kalman filter will be
two dimensional.

The approach you will follow to construct the two dimensional Kalman filter is very
similar as the one dimensional case. However, you will now try to do it in a more
structured way. The state of our system in this case consists of both the vertical
velocity Velocitykalman and the altitude Altitudekalman. Put both variables in the state
vector S:  
Altitudekalman
S=
V elocitykalman

Knowing that the measurement you will  use to predict  the vertical velocity and alti-
Altitudekalman
tude is the acceleration in the
V elocitykalman (k)z =
direction,
=
VS elocity Acc
kalman (k − 1)
Z,inertial
V elocitykalman
, you
+ T sjust need
· Acc to integrate
Z,inertial (k) once to
obtain the velocity in the z direction:
 
Altitude elocity(k)
V kalman (k) = VAltitude
= Altitude elocity(k−1)+T (k s−·V1) + Ts kalman
elocity +0.5·Ts(k)
· AccZ,inertial 2
·AccZ,inertial (k)
kalman
S =  kalman kalman
kalman
V elocitykalman
Altitudekalman
S=
This gives a fully similar  equation
 Vas the
elocity
 one you obtained when
 kalman  integrating
 the roll
Altitudekalman
Altitude (k)= Altitude
(k) 1 Tkalman (k−1)+T
Altitude (k − 1)
s ·V elocity 0.5 · Ts2s2·AccZ,inertial (k)
+0.5·T
rate to getVVthe roll
elocity angle
kalman
in
(k) ==
project
V
s
elocity 15.· Integrating
(k − 1) +
kalman
Tthe· above
Acc +
kalman
equation
(k) a· second time
AccZ,inertial (k)
elocitykalman (k)
kalman 0 1 V elocitykalman (k − 1)
kalman s Z,inertial Ts
gives you the altitude:
V elocity kalman (k) = V elocitykalman (k − 1) + Ts · AccZ,inertial (k)
       
Altitudekalman (k) 1 TsAltitudeAltitude   (k − 1) 0.5 · Ts2
(k) = = S = (k−1)+T
· kalman 0
kalman
+
+0.5·T 2 · Acc Z,inertial (k)
(k)
V elocitykalman (k)
Altitude kalman Altitude 0 1 VS(k
kalman V=elocity
elocity0)s = kalman (k − 1)
·V
kalman
elocity
0
kalman ·Acc
s Ts Z,inertial
2
Altitudekalman (k) = Altitudekalman (k−1)+Ts ·V elocitykalman +0.5·Ts ·AccZ,inertial (k)
Both equations can be
 summarized
   in state space
 0 matrix
 
  form
 as:2 
elocity(k) 1 T
kalman (k)
=  = Vselocity (k −01) (k0+−T1) 0.5 · Ts (k)
==0)0)== (k
AltitudeV kalman Altitude s · AccZ,inertial
· S(k 0 0− 1) +  Ts  · AccZ,inertial (k)
kalman
0 1  PV (k
kalman
 V elocitykalman (k)
Altitudekalman (k) 1 Ts
elocity
Altitudekalman (k − 1)
kalman 0 0.5 · Ts2
= · + · AccZ,inertial (k)
V elocitykalman (k) 0 1 V elocitykalman (k − 1) Ts
  
2
Altitudekalman (k) = Altitudekalman (k−1)+T 0 sT·V·010 02
elocity kalman +0.5·Ts ·AccZ,inertial (k)
F S(kP= Q(k0)
==G =0)· G
=
0 0 0 G U
This corresponds to the general equation 0 for the state prediction S(k)=F.S(k-
S(k = 0) =
     0   
1)+G.U(k). The
Altitude uncertainty
(k) 1 on
Ts thisAltitude
prediction
 kalman  (k is2 −calculated
1) 0.5through
· Ts2 P(k)=F.P(k-1)
kalman
= · = 0
(k) − and 0
T
10 + (k)startup · AccZ,inertial (k)
FT+Q.VBecause you(k)
elocitykalman will set 0both
P1 (kbarometer
Altitude the Q G
altitude
= V0)
· G
=  kalman
elocity · (k
Altitude 1) at
velocity
−kalman Ts to zero, the initial
0 0 
prediction for S at iteration k=0 0 0
P (kis=equal
0) = to: 
0 0
0Altitude
AltitudeS(k
= G=· G
Q barometer 0)(k)
T= − 2
· 100 kalman (k)

Q = G · GT · 102
  
AltitudeP (k)
(k =−
 0  0 Altitude
0)−(k)1 −0 Altitude
=Altitude (k) (k)
Altitude
Altitudekalman (k)
barometer · (k)
kalmankalman
0 0 kalman kalman (k)
barometer V elocity
163
Altitudebarometer (k) − Altitudekalman (k)
 
kalman (k)
    Altitude
(k) T 10 02 ·
1
Altitude Q =(k) −
GI−·= · 10 kalman (k)
GAltitude
0 1 V elocitykalman (k)
Altitudekalman
barometer
 V elocitykalman (k) = 1V elocity (k − 1) +(k Ts−· Acc 0.5 (k)
Altitude kalman (k) Ts Altitude 1) Z,inertial · T2
kalman
Altitude kalman (k) = =
Altitude · (k−1)+Tkalman
s ·V+elocity + +0.5·Ts2s·Acc· Z,inertial (k) (k)
AccZ,inertial
V elocity
V elocity (k)(k) = V0 elocity
kalman
kalman 1kalman (k −kalman
V elocity
kalman 1) T(ks ·−Acc
1) Z,inertial (k)
kalman
Ts

The ·V Kalman filter - two2 dimensions

Altitude
 kalman (k) = Altitude  (k−1)+T
 kalman   +0.5·T
elocitykalman  s ·Acc  Z,inertial (k)
0 (k − 1)
s 
Altitudekalman (k) 1 Ts Altitude
Altitude 0.5 · T 2
= Skalman
Altitudekalman (k) = Altitude =1 S(k · = 0)kalman
(k−1)+T = elocitykalman+
kalman
s ·V 0
2
+0.5·Ts ·AccZ,inertial
s
(k)
· AccZ,inertial (k)
Because this initial
V elocitykalman (k)
prediction 0for SV is V100%
elocity
elocity kalman (k − the
accurate,
kalman
1) initialT suncertainty on the
       
prediction P can
Altitude be (k)
set to zero
1 Tas
s well:
Altitudekalman (k − 1) 0.5 · Ts2
 =  +  · AccZ,inertial (k)
kalman
  ·    
V elocitykalman
Altitude kalman (k)
(k) 10 T 1 P (k V elocitykalman
Altitude kalman0(k−−1)1)
00 (k 0.5 ·TTs s2
V elocitykalman= (k) = V selocity ==0)
0)=
(k= 1) + (k)
0 −s1) + Z,inertial · AccZ,inertial (k)
S(k
· kalman − T · Acc
V elocitykalman (k) 0 1 00 (k
V elocitykalman Ts
 
To be able to calculate to prediction uncertainty, 0 you still need the process uncertain-
S(k = 0) =  
ty. Let’s take a standard deviation of
Altitudekalman (k) = Altitudekalman = G · G 00· 10
10 cm/s²
(k(k−1)+T
0 on02 the accelerometer
2 values; together
==0)0)== s ·V elocitykalman +0.5·Ts ·AccZ,inertial (k)
T
PQ
S(k
with the control matrix G and because Q is0 0essentially 0 the variance of the process
 
uncertainty, you get:   0 0

Altitudekalman (k)
 
(k Altitude
= 0) =
1 TsPbarometer
  
0 2(k −kalman
1) (k)0.5 · Ts2

=Altitude ·Q = G (k)· G 0Altitude
0kalman
− T
·010 + · AccZ,inertial (k)
V elocitykalman (k) 0 1P (k =V 0) = kalman
elocity 0 0 (k − 1) Ts
The observation matrix H links the state with the
Q = G · GT · 10 2 measurement M; the error between
both will be multiplied byAltitude
the Kalman
Altitude (k)T−
barometergain.
(k) −M0 is in this
Altitude (k) the altitude measured by
case
kalman (k)
kalman
S(k=G= ·0)G= · 10
barometer
Q Altitude
2
the barometer, meaning that you can write the 0 error between the measurement and
the state as: Altitudebarometer (k)− Altitude 
kalman (k)

   Altitudekalman (k)
Altitude
Altitude (k) − (k)1 −00Altitude
0· elocity (k)
P (k =(k) (k)kalman (k)
kalmanbarometer
0) −
= AltitudeVkalman
Altitude kalman
barometer
0 0
Writing this more generally in barometer
Altitude matrix (k)form gives
  an
− Altitude
 1  0 Altitude
observation
kalman (k) matrix H equal to [1
kalman (k)
0] and the measurement vector
Altitude
Altitude M
(k)equal
(k)
I =
−to
1 Altitude
0 2
Q = G · G 0· 101 kalman
− ·
Altitude
T (k):
kalman
barometer kalman V elocitykalman (k)
 
 Altitudekalman (k)
Altitudekalman (k) − 1 0 · 
 1 0 Altitude
V elocitykalman
kalman (k)
(k)
Altitude
Altitude (k) − (k)
I 1 =
−0 Altitude
· (k)
M H 0 1 V elocitykalman(k)
kalman barometer kalman

 
With the observation matrix H, calculate 1 0
I = the intermediate matrix L(k)=H.P(k).H +R.
T

10 01
R is the uncertainty onAltitude barometer (k)
the barometer kalman (k)
I =altitude measurement;
− Altitude let’s take a standard devi-
0 1
ation of 30 cm yielding R=30². From here onward, the Kalman gain can easily be cal-
culated with the formula K=P(k).HT.L(k)  andAltitude
-1  the update of the prediction through
kalman (k)

S(k)=S(k)+K.(M-H.S). kalman (k)
AltitudeFinally 1 0 · on the predicted
the−uncertainty V elocitykalman (k) state is updated using
the equation P(k)=(I-K.F)P(k). In this case, I is the 2x2 identity matrix:
 
1 0
I=
0 1

And this is all there is to it, you now have a two-dimensional Kalman filter! Let’s try
to implement your new filter in Arduino. For this part you need both the MPU-6050
gyro and the BMP-280 barometer, so either test the code directly on the printed cir-
cuit board of you quadcopter, or connect the sensor and Teensy separately using your
breadboard as visualized on the previous page. Be careful to connect the Vcc of
the barometer to the 3.3V power source of your Teensy and not the 5V power
source to avoid damaging the sensor.

164
 Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)
Anglekalman (k) = Anglekalman (k) + Gainkalman · (Angle(k) − Anglekalman )

Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)

U ncertainty
Anglekalman (k)(k)
angle = U ncertainty
= Angle kalman (k − 1) (k
angle + T−s 1) + Ts2 · 42
· Rate(k)
U ncertaintyangle (k)
Two-dimensional
Gain =
(k) = Angle
form1)of
kalman (k − + T+
the Kalman filter 
angle (k) s ·3Rate(k)
Anglekalmankalman U ncertainty 2
2 2 Altitudekalman
U ncertainty (k) = U ncertainty (k − 1) + T · 4
Anglekalman (k) 1.
= Predict
angle the current state
Angle
U ncertainty (k) =
kalman (k)U+ Gainkalman
ncertainty of
angle the system:
· (Angle(k)
angle (k − 1) + Ts · 4
−s2 Angle
2 kalman ) V elocity  kalman
Altitude

angle Altitude
Anglekalman (k) = Anglekalman (k − 1) + Ts · Rate(k)
kalman
  kalman
U ncertaintyangle (k) = (1 − Gainkalman ) · U ncertainty2angle2(k)  Altitude VVkalman
elocity
elocity

U ncertaintyangle (k) = U ncertaintyangle (k − 1) + TS=state s ·4 vector
kalman
kalman
Altitude  kalman
Anglekalman (k) = Anglekalman (k) +UGain ncertainty · (Angle(k)
(k) − AnglekalmanV) elocity 1
V elocity T
 kalman
kalman
s
Anglekalman (k) = Gain kalman=
kalman angle
(k) + Gainkalman · (Angle(k) kalman )
 
Angle −2 Angle 0Altitude
1 1 kalman
angle(k) (k −+1) 32+ TF=state transition matrix  1 T
kalman 2
U ncertaintyangle (k) = U ncertaintyangle s ·4
Ts s
S(k) = F · S(k − 1) + G · U (k) 
Anglekalman (k) = Anglekalman (k) + Gainkalman · (Angle(k) − Anglekalman )  1 Ts 00kalman
V elocity 11
U ncertaintyangle (k) 1  T0s 1 2 
Gainkalman = U ncertaintyangle (k) 2 G=control matrix 0.5 · T
U ncertainty Gain (k) = = (1 U−ncertainty
Gainkalmanangle ) · U(k) + 32
ncertainty angle (k) 0 1 s  
anglekalman
Anglekalman (k) = Angle (k) + Gain (Angle(k)
T (k) + 3
· − Angle ) Ts 10.5T·s T 2 2
kalman U ncertainty
P (k) = F · PU(k − 1) · F
kalmanangle
+ Q(k)
kalman 0.5 · T ss
Gainkalman =
ncertainty angle
U=input variable


(Acc
0.5 ·T 02  )T1T ss
U ncertaintyangle (k) + 32 0.5 · TT 2Z,inertial
s
 
angle (k) = (1
2. Calculate Gainkalman ) · U
− uncertainty angle (k)
S(k) = Fthe of(k) the prediction: 0 0
U ncertainty ncertainty ss
U ncertainty angle (k) = (1 − U
· S(k
Gain − 1) + G ) ·
ncertaintyangle (k)
kalman · U
U ncertainty angle (k) P (k =
T 0) =
0.50· Ts02 00 00
s  
L(k) =
Gainkalman =H · P (k) · H Tangle +R (k) + 32 P=prediction
U ncertainty
U ncertaintyangle (k) = (1 − Gainkalman ) · U ncertaintyangle (k)  uncertainty
P (k(k = 
= 0)
vector
0)= =

0s 0 00 00
P T
Altitude 
S(k) = F · S(k − 1) + GT· U (k) P (k =kalman
0) = 0 0
(k) = (k 1) + PV(kelocity
= 0) 02 0
S(k) = F · S(k − 1) + G · U (k) G ·=
P F · P − · F Q
GT0· 10
kalman
0 
U ncertaintyangle (k) = (1H− T Gain
kalman ) T· U ncertainty angle (k)

P (k)=· F · S(k=−P1)
K =S(k) (k)+· G −1 T T 0 202
H · U· L(k)(k) Q=process uncertainty
P (k =G G0)· G
· =
G · 10
· 10
L(k)   0 0
P (k) = F · P (k − 1) · TF TT + Q 1G T·sGT · 102
3. Calculate
L(k) the
= H Kalman
P (k)
P (k) = F · P (k − 1) · F + Q
· · H gain+ R from the uncertainties G 0· G 1 ·110predictions
on the
T 2 and
0
S(k) = F · S(k − 1) + G · U (k)
measurements:  
S(k)P= = F+· K · (M (k)· F−TH+· Q
(k)S(k) P (k − 1) S(k))
 G ·GT11· 10 002
 
L(k) =HHT · P (k) · H T + T
R L= Intermediate 0.5 · T12 0 
matrix
K = PL(k)(k) · = H · = P (k) · H· H+
P (k) T R
· L(k)−1 1 0s1 0
F · P (k − 1) · F T + Q
P (k) = L(k) K=Kalman gain Ts 0 1   
P (k) ==H (I ·−PK (k)· F ) ·TP+(k) 1 0 00
L(k) ·H R H=Observation matrix  1 1
H TT
K = P (k) · H = P (k) · H TT · L(k)−1 R=Measurement  1 0 00 (30
uncertainty
11 2
)
= P=(k)
S(k) S(k) + K=· (M (k)· T−
P (k) H H· ·L(k) S(k)) −1 1 00 0 1 0
K · L(k)
L(k) = (k) + P (k = 0) =
H
L(k) · P · H R 0 1 0 0 
= P (k)the
HT
= P (k) · state
H T · L(k) −1 1 0
4. K
Update · predicted
L(k) of the system with the measurement 0 1
of the
S(k)
state = S(k)
through +theK · (M
Kalman (k) − H
gain: · S(k))
P (k)
S(k) = S(k)H = (I − K · F )
+TK · (M (k) − TH · S(k))
· P (k)
G · GT · 102
K = P (k) · = P (k) · H · L(k)−1
L(k)
S(k) = S(k) + K · (M (k) − H · S(k)) M=measurement vector (AltitudeKalman)
P (k) = (I − K · F ) · P (k)  
P (k) = (I − K · F ) · P (k) 1 0
S(k) = S(k) + K · (M (k) − H · S(k))
5. Update the uncertainty of the predicted state:
P (k) = (I − K · F ) · P (k)
 
P (k) = (I − K · F ) · P (k) I=unity matrix 1 0
0 1

165
Ca rbon aeronautics
 The Kalman filter - two dimensions

Coding

Use a dedicated library to be able to implement and calculate with matrices: the Ba-
sicLinearAlgebra library. Make sure you import it by clicking on Sketch → include
library → manage libraries because this is not a standard library. A namespace called
BLA is used to define the matrices; for each matrix you need to declare the size; H=[1
0] for example is a 1x2 matrix while the state space S is a 2x1 matrix. Do not forget to
define the altitude and vertical velocity that will be predicted with the Kalman filter.

Construct the function that will hold the two-dimensional Kalman filter. The acceler-
ation value that will come from the accelerometer AccZInertial has to be transformed
to the 1x1 matrix Acc. The prediction for the state space is calculated according to
the formula seen in the theory of this project, together with the uncertainty on the
prediction. Calculate the transpose of matrix F (FT) by placing a ~ in front of the
matrix. For the calculation of the Kalman gain, the matrix L should be inverted (L-1)
which you can do with the function Invert. The measurement matrix M consists only
of the measured altitude with the barometer. Now everything is ready to calculate the
updated state vector S; extract the altitude (in cm) from the first position in the vector
and the vertical velocity (in cm/s) from the second position. Do not forget to update
the uncertainty on the prediction.

166
1 #include <Wire.h> Include all variables
2 float RateRoll, RatePitch, RateYaw; for the gyro and ac-
3 float AngleRoll, AnglePitch; celerometer (project
4 float AccX, AccY, AccZ; 14)
5 float AccZInertial;
6 float LoopTimer;
7 uint16_t dig_T1, dig_P1; Include all variables
8 int16_t dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5; for the barometer
9 int16_t dig_P6, dig_P7, dig_P8, dig_P9; (project 17)
10 float AltitudeBarometer, AltitudeBarometerStartUp;
11 int RateCalibrationNumber;

12 #include <BasicLinearAlgebra.h> Define the matrices

13 using namespace BLA; for the two-dimen-
14 float AltitudeKalman, VelocityVerticalKalman; sional Kalman filter
15 BLA::Matrix<2,2> F; BLA::Matrix<2,1> G;
16 BLA::Matrix<2,2> P; BLA::Matrix<2,2> Q;
17 BLA::Matrix<2,1> S; BLA::Matrix<1,2> H;
18 BLA::Matrix<2,2> I; BLA::Matrix<1,1> Acc;
19 BLA::Matrix<2,1> K; BLA::Matrix<1,1> R;
20 BLA::Matrix<1,1> L; BLA::Matrix<1,1> M;

21 void kalman_2d(void){ Create the function

22 Acc = {AccZInertial}; that holds the two
23 S=F*S+G*Acc; dimensional Kalman
24 P=F*P*~F+Q; filter
25 L=H*P*~H+R;
26 K=P*~H*Invert(L);
27 M={AltitudeBarometer};
28 S=S+K*(M-H*S);
29 AltitudeKalman=S(0,0);
30 VelocityVerticalKalman=S(1,0);
31 P=(I-K*H)*P;
32 }

33 void barometer_signals(void){ Calculate the altitude

34 Wire.beginTransmission(0x76); in cm from the baro-
35 Wire.write(0xF7); metric measurement
36 Wire.endTransmission(); (project 17)
37 Wire.requestFrom(0x76,6);
38 uint32_t press_msb = Wire.read();

167
Ca rbon aeronautics
 The Kalman filter - two dimensions

39 uint32_t press_lsb = Wire.read();

40 uint32_t press_xlsb = Wire.read();
41 uint32_t temp_msb = Wire.read();
42 uint32_t temp_lsb = Wire.read();
43 uint32_t temp_xlsb = Wire.read();
44 unsigned long int adc_P = (press_msb << 12) | (
press_lsb << 4) | (press_xlsb >>4);
45 unsigned long int adc_T = (temp_msb << 12) | (
temp_lsb << 4) | (temp_xlsb >>4);
46 signed long int var1, var2;
47 var1 = ((((adc_T >> 3) - ((signed long int )dig_T1
<<1)))* ((signed long int )dig_T2)) >> 11;
48 var2 = (((((adc_T >> 4) - ((signed long int )dig_T1
)) * ((adc_T>>4) - ((signed long int )dig_T1)))
>> 12) * ((signed long int )dig_T3)) >> 14;
49 signed long int t_fine = var1 + var2;
50 unsigned long int p;
51 var1 = (((signed long int )t_fine)>>1) - (signed
long int )64000;
52 var2 = (((var1>>2) * (var1>>2)) >> 11) * ((signed
long int )dig_P6);
53 var2 = var2 + ((var1*((signed long int )dig_P5))
<<1);
54 var2 = (var2>>2)+(((signed long int )dig_P4)
<<16);
55 var1 = (((dig_P3 * (((var1>>2)*(var1>>2)) >> 13
))>>3)+((((signed long int )dig_P2) *
var1)>>1))>>18;
56 var1 = ((((32768+var1))*((signed long int )dig_P1))
>>15);
57 if (var1 == 0) { p=0;}
58 p = (((unsigned long int )(((signed long int )
1048576)-adc_P)-(var2>>12)))*3125;
59 if(p<0x80000000){ p = (p << 1) / ((unsigned
long int ) var1);}
60 else { p = (p / (unsigned long int )var1) * 2; }
61 var1 = (((signed long int )dig_P9) * ((signed long
int ) (((p>>3) * (p>>3))>>13)))>>12;
62 var2 = (((signed long int )(p>>2)) *
((signed long int )dig_P8))>>13;

168
63 p = (unsigned long int)((signed long int )p +
((var1 + var2+ dig_P7) >> 4));
64 double pressure=(double)p/100;
65 AltitudeBarometer=44330*(1-pow(pressure
/1013.25, 1/5.255))*100;
66 }
67 void gyro_signals(void) { Define the gyro/ac-
68 Wire.beginTransmission(0x68); celerometer function
69 Wire.write(0x1A); (project 14)
70 Wire.write(0x05);
71 Wire.endTransmission();
72 Wire.beginTransmission(0x68);
73 Wire.write(0x1C);
74 Wire.write(0x10);
75 Wire.endTransmission();
76 Wire.beginTransmission(0x68);
77 Wire.write(0x3B);
78 Wire.endTransmission();
79 Wire.requestFrom(0x68,6);
80 int16_t AccXLSB = Wire.read() << 8 |
Wire.read();
81 int16_t AccYLSB = Wire.read() << 8 |
Wire.read();
82 int16_t AccZLSB = Wire.read() << 8 |
Wire.read();
83 Wire.beginTransmission(0x68);
84 Wire.write(0x1B);
85 Wire.write(0x8);
86 Wire.endTransmission();
87 Wire.beginTransmission(0x68);
88 Wire.write(0x43);
89 Wire.endTransmission();
90 Wire.requestFrom(0x68,6);
91 int16_t GyroX=Wire.read()<<8 | Wire.read();
92 int16_t GyroY=Wire.read()<<8 | Wire.read();
93 int16_t GyroZ=Wire.read()<<8 | Wire.read();
94 RateRoll=(float)GyroX/65.5;
95 RatePitch=(float)GyroY/65.5; Do not forget to put
96 RateYaw=(float)GyroZ/65.5; your own accelerom-
97 AccX=(float)AccXLSB/4096-0.050.05; eter calibration values
98 AccY=(float)AccYLSB/4096+0.01
+0.01; here (project 14)

169
Ca rbon aeronautics
 The Kalman filter - two dimensions

99 AccZ=(float)AccZLSB/4096-0.11
-0.11;
100 AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*
AccZ))*1/(3.142/180);
101 AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*
AccZ))*1/(3.142/180);
102 }
103 void setup() {
104 Serial.begin(57600);
105 pinMode(13, OUTPUT);
106 digitalWrite(13, HIGH);
107 Wire.setClock(400000);
108 Wire.begin();
109 delay(250);
110 Wire.beginTransmission(0x68); Setup the MPU-6050
111 Wire.write(0x6B); (project 4)
112 Wire.write(0x00);
113 Wire.endTransmission();
114 Wire.beginTransmission(0x76); Setup the BMP-280
115 Wire.write(0xF4); (project 17)
116 Wire.write(0x57);
117 Wire.endTransmission();
118 Wire.beginTransmission(0x76);
119 Wire.write(0xF5);
120 Wire.write(0x14);
121 Wire.endTransmission();
122 uint8_t data[24], i=0;
123 Wire.beginTransmission(0x76);

In the final setup step, you need to initialize the matrices and vectors that you will
use in the two dimensional Kalman filter. Matrices F, G, H, I, Q and R stay constant
throughout all iterations and were already defined in the theory part at the beginning
of this project, with TS=0.004 s. For matrix P and vector S, you only need to set the
initial value, when k is equal to zero. As seen in the theory, you can set all elements of
these matrices to zero since you know the exact starting altitude and speed, namely 0
cm and 0 cm/s respectively.

170
124 Wire.write(0x88);
125 Wire.endTransmission();
126 Wire.requestFrom(0x76,24);
127 while(Wire.available()){
128 data[i] = Wire.read();
129 i++;
130 }
131 dig_T1 = (data[1] << 8) | data[0];
132 dig_T2 = (data[3] << 8) | data[2];
133 dig_T3 = (data[5] << 8) | data[4];
134 dig_P1 = (data[7] << 8) | data[6];
135 dig_P2 = (data[9] << 8) | data[8];
136 dig_P3 = (data[11]<< 8) | data[10];
137 dig_P4 = (data[13]<< 8) | data[12];
138 dig_P5 = (data[15]<< 8) | data[14];
139 dig_P6 = (data[17]<< 8) | data[16];
140 dig_P7 = (data[19]<< 8) | data[18];
141 dig_P8 = (data[21]<< 8) | data[20];
142 dig_P9 = (data[23]<< 8) | data[22]; delay(250);
143 for (RateCalibrationNumber=0; Calculate the altitude
RateCalibrationNumber<2000; reference level (pro-
RateCalibrationNumber ++) { ject 17)
144 barometer_signals();
145 AltitudeBarometerStartUp+=
146 AltitudeBarometer; delay(1);
147 }
148 AltitudeBarometerStartUp/=2000;

149 F = {1, 0.004, Define the Kalman

150 0, 1}; matrix values
151 G = {0.5*0.004*0.004,
152 0.004};
153 H = {1, 0};
154 I = {1, 0,
155 0, 1};
156 Q = G * ~G*10*10;
157 R = {30*30};
158 P = {0, 0,
159 0, 0};
160 S = {0,
161 0};

171
Ca rbon aeronautics
 The Kalman filter - two dimensions

Measure the acceleration in the inertial Z direction and the altitude for each iteration
of TS=0.004. Both measurements are subsequently used in the Kalman filter function
to calculate the Kalman altitude and velocity.

Print out the Kalman-filtered values for both the altitude and the velocity.

Testing the two-dimensional Kalman filter

When you test the 2D Kalman filter, you will notice that the values for both the alti-
tude and the vertical velocity are sometimes significantly off. This is due to changes
in the atmospheric pressure which is unfortunately inherent to the type of meas-
urement. However, when implementing the filter in the flight control controller and
testing the quadcopter, you will not really notice this when flying because you use a
velocity control and not an altitude control. Even if the measured velocity is slightly
wrong, you will be able to hover the quadcopter by adjusting the throttle stick on
your radiocontroller to match this velocity; this will happen intuitively when flying.

The evolution of the Kalman gain in time is given in the figure to the right. The
Kalman gain is zero initially, because you have set the initial uncertainty matrix P
to zero. After a few seconds, the Kalman gain reaches its steady state at 0.0033 for
the altitude calculations and 0.0013 for the vertical velocity. This means physically
that the Kalman filter relies heavily on the accelerometer integration and less on
the barometer measurements, so the latter is used mostly to ensure that the altitude
obtained from accelerometer integration does not diverges too far.

172
162 LoopTimer=micros();
163 }

164 void loop() { Calculate the Kalman

165 gyro_signals(); altitude and velocity
166 AccZInertial=-sin(AnglePitch*(3.142/180))*AccX
+cos(AnglePitch*(3.142/180))*sin(AngleRoll*
(3.142/180))* AccY+cos(AnglePitch*(3.142/180))*
cos(AngleRoll*(3.142/180))*AccZ;
167 AccZInertial=(AccZInertial-1)*9.81*100;
168 barometer_signals();
169 AltitudeBarometer-=AltitudeBarometerStartUp;
170 kalman_2d();

171 Serial.print("Altitude [cm]: "); Print the altitude and

172 Serial.print(AltitudeKalman); velocity
173 Serial.print(" Vertical velocity [cm/s]: ");
174 Serial.println(VelocityVerticalKalman);

175 while (micros() - LoopTimer < 4000);

176 LoopTimer=micros();
177 }

kalman gain

kalman gain altitude

0.003 (steady state ≈ 0.0033)

0.002

0.001 kalman gain velocity

(steady state ≈ 0.0013)

0
0 2 4 6 8
Time [s]

173
Ca rbon aeronautics
 Project 20
The flight controller: velocity mode

desired vertical velocity motor throttle

vertical velocity error Velocity command
+ motor power
- Controller

Kalman measured quadcopter

vertical velocity altitude altitude
Barometer

kalman filter
measured quadcopter
vertical velocity gyroscope and vertical velocity
Accelerometer

desiredVelocityVertical

desiredVelocity = 0.3 (ReceiverValue - 1500)

150 cm/s

0 cm/s

150 cm/s

1000 µs 1500 µs 2000 µs ReceiverValue[2] (=channel 3)

174
Ho ver your quadcopter with ease
The second flight controllers you programmed allowed you to stabilize your
quadcopter based on its angles, which makes flying a lot easier. By addition-
ally controlling the vertical velocity, the effort of flying will be reduced even
more.

The flight controller you will program during this project will allow you to control
the vertical velocity of your quadcopter, instead of the throttle. You can compare this
with your own car; with the accelerator pedal, you control the acceleration of your
car but not the speed. In order to stay at a constant speed, you almost continuously
have to adjust the pedal in order to remain at a more or less equal speed. This was
also true for your quadcopter when using the previous two flight controllers; you had
to adjust the throttle stick almost continuously in order to hover. The velocity control
that you will develop in this project is similar to the cruise control in your car; you set
the speed once and the motor of your car will adjust its power in order to keep going
at the required speed.

The control loop you will implement for the vertical velocity is a similar loop as you
used for the rate control in your first flight controller. There are only two noticeable
differences: the output for this controller is the throttle input command, instead of
the roll, pitch and yaw commands. Furthermore, the measurement of the vertical ve-
locity goes through the two dimensional Kalman filter as constructed in the previous
project. The velocity controller is once again a PID controller for which you can use
the already programmed function in Arduino. Good P, I and D parameters for the
velocity control are:

• PVelocity Vertical= 3.5

• IVelocity Vertical= 0.0015
• DVelocity Vertical=0.01

Finally, you also need to do decide how the vertical velocity input values correspond
with the receiver commands. A too steep correlation will lead to very sensitive con-
trols, while a more horizontal correlation will lead to insensitive controls. A good
balance for the controls is when the maximal and minimal receiver values correspond
with a vertical velocity of respectively ± 150 cm/s.

You are now ready to program your third and final flight controller, so let’s start!

175
 The flight controller: velocity mode

Coding
1 #include <Wire.h> Initialize the same
2 float RateRoll, RatePitch, RateYaw; variables that you al-
3 float RateCalibrationRoll, RateCalibrationPitch, ready needed for rate
RateCalibrationYaw; mode (project 12)
4 int RateCalibrationNumber;
5 #include <PulsePosition.h>
6 PulsePositionInput ReceiverInput(RISING);
7 float ReceiverValue[]={0, 0, 0, 0, 0, 0, 0, 0};
8 int ChannelNumber=0;
9 float Voltage, Current, BatteryRemaining, BatteryAtStart;
10 float CurrentConsumed=0;
11 float BatteryDefault=1300;
12 uint32_t LoopTimer;
13 float DesiredRateRoll, DesiredRatePitch,
DesiredRateYaw;
14 float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;
15 float InputRoll, InputThrottle, InputPitch, InputYaw;
16 float PrevErrorRateRoll, PrevErrorRatePitch,
PrevErrorRateYaw;
17 float PrevItermRateRoll, PrevItermRatePitch,
PrevItermRateYaw;
18 float PIDReturn[]={0, 0, 0};
19 float PRateRoll=0.6; float PRatePitch=PRateRoll;
float PRateYaw=2;
20 float IRateRoll=3.5; float IRatePitch=IRateRoll;
float IRateYaw=12;
21 float DRateRoll=0.03; float DRatePitch=DRateRoll;
float DRateYaw=0;
22 float MotorInput1, MotorInput2, MotorInput3,
MotorInput4;
23 float AccX, AccY, AccZ; Initialize the accel-
24 float AngleRoll, AnglePitch; erometer variables
(project 14)

25 float KalmanAngleRoll=0, Define the Kalman

KalmanUncertaintyAngleRoll=2*2; variables (project 15)
26 float KalmanAnglePitch=0,
KalmanUncertaintyAnglePitch=2*2;
27 float Kalman1DOutput[]={0,0};

176
28 float DesiredAngleRoll, DesiredAnglePitch; Define the values
29 float ErrorAngleRoll, ErrorAnglePitch; necessary for the out-
30 float PrevErrorAngleRoll, PrevErrorAnglePitch; er loop PID control-
31 float PrevItermAngleRoll, PrevItermAnglePitch; ler, including the P,
32 float PAngleRoll=2; float PAnglePitch=PAngleRoll; I and D parameters
33 float IAngleRoll=0; float IAnglePitch=IAngleRoll; (project 16)
34 float DAngleRoll=0; float DAnglePitch=DAngleRoll;
35 uint16_t dig_T1, dig_P1; Define the variables
36 int16_t dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5; that you need for
37 int16_t dig_P6, dig_P7, dig_P8, dig_P9; the two dimensional
38 float AltitudeBarometer, AltitudeBarometerStartUp; Kalman filter and ba-
39 float AccZInertial; rometer (project 19)
40 #include <BasicLinearAlgebra.h>
41 using namespace BLA;
42 float AltitudeKalman, VelocityVerticalKalman;
43 BLA::Matrix<2,2> F; BLA::Matrix<2,1> G;
44 BLA::Matrix<2,2> P; BLA::Matrix<2,2> Q;
45 BLA::Matrix<2,1> S; BLA::Matrix<1,2> H;
46 BLA::Matrix<2,2> I; BLA::Matrix<1,1> Acc;
47 BLA::Matrix<2,1> K; BLA::Matrix<1,1> R;
48 BLA::Matrix<1,1> L; BLA::Matrix<1,1> M;

49 float DesiredVelocityVertical, ErrorVelocityVertical; Define the values

50 float PVelocityVertical=3.5; necessary for the ve-
float IVelocityVertical=0.0015; locity PID controller,
float DVelocityVertical=0.01; including the P, I and
51 float PrevErrorVelocityVertical, D parameters
PrevItermVelocityVertical;

52 void kalman_2d(void){ Define the two di-

53 Acc = {AccZInertial}; mensional Kalman
54 S=F*S+G*Acc; filter (project 19)
55 P=F*P*~F+Q;
56 L=H*P*~H+R;
57 K=P*~H*Invert(L);
58 M = {AltitudeBarometer};
59 S=S+K*(M-H*S);
60 AltitudeKalman=S(0,0);
61 VelocityVerticalKalman=S(1,0);
62 P=(I-K*H)*P;
63 }

177
Ca rbon aeronautics
 The flight controller: velocity mode

64 void barometer_signals(void){ Calculate the altitude

65 Wire.beginTransmission(0x76); in cm from the baro-
66 Wire.write(0xF7); metric measurement
67 Wire.endTransmission(); (project 17)
68 Wire.requestFrom(0x76,6);
69 uint32_t press_msb = Wire.read();
70 uint32_t press_lsb = Wire.read();
71 uint32_t press_xlsb = Wire.read();
72 uint32_t temp_msb = Wire.read();
73 uint32_t temp_lsb = Wire.read();
74 uint32_t temp_xlsb = Wire.read();
75 unsigned long int adc_P = (press_msb << 12) | (
press_lsb << 4) | (press_xlsb >>4);
76 unsigned long int adc_T = (temp_msb << 12) | (
temp_lsb << 4) | (temp_xlsb >>4);
77 signed long int var1, var2;
78 var1 = ((((adc_T >> 3) - ((signed long int )dig_T1
<<1)))* ((signed long int )dig_T2)) >> 11;
79 var2 = (((((adc_T >> 4) - ((signed long int )dig_T1
)) * ((adc_T>>4) - ((signed long int )dig_T1)))
>> 12) * ((signed long int )dig_T3)) >> 14;
80 signed long int t_fine = var1 + var2;
81 unsigned long int p;
82 var1 = (((signed long int )t_fine)>>1) - (signed
long int )64000;
83 var2 = (((var1>>2) * (var1>>2)) >> 11) * ((signed
long int )dig_P6);
84 var2 = var2 + ((var1*((signed long int )dig_P5))
<<1);
85 var2 = (var2>>2)+(((signed long int )dig_P4)
<<16);
86 var1 = (((dig_P3 * (((var1>>2)*(var1>>2)) >> 13
))>>3)+((((signed long int )dig_P2) *
var1)>>1))>>18;
87 var1 = ((((32768+var1))*((signed long int )dig_P1))
>>15);
88 if (var1 == 0) { p=0;}
89 p = (((unsigned long int )(((signed long int )
1048576)-adc_P)-(var2>>12)))*3125;
90 if(p<0x80000000){ p = (p << 1) / ((unsigned
long int ) var1);}

178
91 else { p = (p / (unsigned long int )var1) * 2; }
92 var1 = (((signed long int )dig_P9) * ((signed long
int ) (((p>>3) * (p>>3))>>13)))>>12;
93 var2 = (((signed long int )(p>>2)) *
((signed long int )dig_P8))>>13;
94 p = (unsigned long int)((signed long int )p +
((var1 + var2+ dig_P7) >> 4));
95 double pressure=(double)p/100;
96 AltitudeBarometer=44330*(1-pow(pressure
/1013.25, 1/5.255))*100;
97 }
98 void kalman_1d(float KalmanState, Define the 1D Kal-
float KalmanUncertainty, float KalmanInput, man filter function
float KalmanMeasurement) { (project 15)
99 KalmanState=KalmanState+0.004*KalmanInput;
100 KalmanUncertainty=KalmanUncertainty + 0.004
* 0.004 * 4 * 4;
101 float KalmanGain=KalmanUncertainty * 1/
(1*KalmanUncertainty + 3 * 3);
102 KalmanState=KalmanState+KalmanGain * (
KalmanMeasurement-KalmanState);
103 KalmanUncertainty=(1-KalmanGain) *
KalmanUncertainty;
104 Kalman1DOutput[0]=KalmanState;
Kalman1DOutput[1]=KalmanUncertainty;
105 }
106 void battery_voltage(void) { Battery voltage func-
107 Voltage=(float)analogRead(15)/62; tion (project 9)
108 Current=(float)analogRead(21)*0.089;
109 }
110 void read_receiver(void){ Receiver function
111 ChannelNumber = ReceiverInput.available(); (project 7)
112 if (ChannelNumber > 0) {
113 for (int i=1; i<=ChannelNumber;i++){
114 ReceiverValue[i-1]=ReceiverInput.read(i);
115 }
116 }
117 }
118 void gyro_signals(void) { Gyro and accelerom-
119 Wire.beginTransmission(0x68); eter function (project
120 Wire.write(0x1A); 14)

179
Ca rbon aeronautics
 The flight controller: velocity mode

121 Wire.write(0x05);
122 Wire.endTransmission();
123 Wire.beginTransmission(0x68);
124 Wire.write(0x1C);
125 Wire.write(0x10);
126 Wire.endTransmission();
127 Wire.beginTransmission(0x68);
128 Wire.write(0x3B);
129 Wire.endTransmission();
130 Wire.requestFrom(0x68,6);
131 int16_t AccXLSB = Wire.read() << 8 |
Wire.read();
132 int16_t AccYLSB = Wire.read() << 8 |
Wire.read();
133 int16_t AccZLSB = Wire.read() << 8 |
Wire.read();
134 Wire.beginTransmission(0x68);
135 Wire.write(0x1B);
136 Wire.write(0x8);
137 Wire.endTransmission();
138 Wire.beginTransmission(0x68);
139 Wire.write(0x43);
140 Wire.endTransmission();
141 Wire.requestFrom(0x68,6);
142 int16_t GyroX=Wire.read()<<8 | Wire.read();
143 int16_t GyroY=Wire.read()<<8 | Wire.read();
144 int16_t GyroZ=Wire.read()<<8 | Wire.read();
145 RateRoll=(float)GyroX/65.5;
146 RatePitch=(float)GyroY/65.5;
147 RateYaw=(float)GyroZ/65.5;

148 AccX=(float)AccXLSB/4096-0.05
0.05; Do not forget to put
149 AccY=(float)AccYLSB/4096+0.01
+0.01; your own accelerom-
150 AccZ=(float)AccZLSB/4096-0.11
-0.11; eter calibration values
here (project 14)

151 AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*
AccZ))*1/(3.142/180);
152 AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*
AccZ))*1/(3.142/180);
153 }

180
154 void pid_equation(float Error, float P , float I, float D, PID function (pro-
float PrevError, float PrevIterm) { ject 12)
155 float Pterm=P*Error;
156 float Iterm=PrevIterm+I*(Error+
PrevError)*0.004/2;
157 if (Iterm > 400) Iterm=400;
158 else if (Iterm <-400) Iterm=-400;
159 float Dterm=D*(Error-PrevError)/0.004;
160 float PIDOutput= Pterm+Iterm+Dterm;
161 if (PIDOutput>400) PIDOutput=400;
162 else if (PIDOutput <-400) PIDOutput=-400;
163 PIDReturn[0]=PIDOutput;
164 PIDReturn[1]=Error;
165 PIDReturn[2]=Iterm;
166 }
167 void reset_pid(void) { PID reset function
168 PrevErrorRateRoll=0; PrevErrorRatePitch=0; (project 12)
PrevErrorRateYaw=0;
169 PrevItermRateRoll=0; PrevItermRatePitch=0;
PrevItermRateYaw=0;
170 PrevErrorAngleRoll=0; PrevErrorAnglePitch=0;
171 PrevItermAngleRoll=0; PrevItermAnglePitch=0;

172 PrevErrorVelocityVertical=0; Reset the PID error

PrevItermVelocityVertical=0; and integral values
173 } for the vertical veloc-
ity controller loop as
well
174 void setup() {
175 pinMode(5, OUTPUT); Visualize the setup
176 digitalWrite(5, HIGH); phase using the red
177 pinMode(13, OUTPUT); LED
178 digitalWrite(13, HIGH);
179 Wire.setClock(400000);
180 Wire.begin();
181 delay(250);
182 Wire.beginTransmission(0x68); Setup the MPU-6050
183 Wire.write(0x6B); (project 4)
184 Wire.write(0x00);
185 Wire.endTransmission();

181
Ca rbon aeronautics
 The flight controller: velocity mode

186 Wire.beginTransmission(0x76); Setup the BMP-280

187 Wire.write(0xF4); (project 17)
188 Wire.write(0x57);
189 Wire.endTransmission();
190 Wire.beginTransmission(0x76);
191 Wire.write(0xF5);
192 Wire.write(0x14);
193 Wire.endTransmission();
194 uint8_t data[24], i=0;
195 Wire.beginTransmission(0x76);
196 Wire.write(0x88);
197 Wire.endTransmission();
198 Wire.requestFrom(0x76,24);
199 while(Wire.available()){
200 data[i] = Wire.read();
201 i++;
202 }
203 dig_T1 = (data[1] << 8) | data[0];
204 dig_T2 = (data[3] << 8) | data[2];
205 dig_T3 = (data[5] << 8) | data[4];
206 dig_P1 = (data[7] << 8) | data[6];
207 dig_P2 = (data[9] << 8) | data[8];
208 dig_P3 = (data[11]<< 8) | data[10];
209 dig_P4 = (data[13]<< 8) | data[12];
210 dig_P5 = (data[15]<< 8) | data[14];
211 dig_P6 = (data[17]<< 8) | data[16];
212 dig_P7 = (data[19]<< 8) | data[18];
213 dig_P8 = (data[21]<< 8) | data[20];
214 dig_P9 = (data[23]<< 8) | data[22]; delay(250);
215 for (RateCalibrationNumber=0;
RateCalibrationNumber<2000;
RateCalibrationNumber ++) {
216 gyro_signals();
217 RateCalibrationRoll+=RateRoll;
218 RateCalibrationPitch+=RatePitch;
219 RateCalibrationYaw+=RateYaw;
220 barometer_signals(); Calculate the altitude
221 AltitudeBarometerStartUp+= reference level (pro-
222 AltitudeBarometer; delay(1); ject 17)
223 }
224 RateCalibrationRoll/=2000;

182
225 RateCalibrationPitch/=2000;
226 RateCalibrationYaw/=2000;
227 AltitudeBarometerStartUp/=2000;
228 F = {1, 0.004, Setup the matrices
229 0, 1}; for the two-dimen-
230 G = {0.5*0.004*0.004, sional Kalman filter
231 0.004}; (project 19)
232 H = {1, 0};
233 I = {1, 0,
234 0, 1};
235 Q = G * ~G*10*10;
236 R = {30*30};
237 P = {0, 0,
238 0, 0};
239 S = {0,
240 0};
241 analogWriteFrequency(1, 250); Set the PWM fre-
242 analogWriteFrequency(2, 250); quency to 250 Hz
243 analogWriteFrequency(3, 250); and the resolution to
244 analogWriteFrequency(4, 250); 12 bit for all motors
245 analogWriteResolution(12); (project 8)

246 pinMode(6, OUTPUT); Show the end of the

247 digitalWrite(6, HIGH); setup process and
248 battery_voltage(); determine the initial
249 if (Voltage > 8.3) { digitalWrite(5, LOW); battery voltage per-
250 BatteryAtStart=BatteryDefault; } centage (project 9)
251 else if (Voltage < 7.5) {
252 BatteryAtStart=30/100*BatteryDefault ;}
253 else { digitalWrite(5, LOW);
254 BatteryAtStart=(82*Voltage-580)/100*
BatteryDefault; }

255 ReceiverInput.begin(14); SAFETY RELAT-

256 while (ReceiverValue[2] < 1020 || ED LINES: Avoid
ReceiverValue[2] > 1050) { accidental lift off af-
257 read_receiver(); ter the setup process
258 delay(4); (project 12)
259 }
260 LoopTimer=micros();
261 }

183
Ca rbon aeronautics
 The flight controller: velocity mode

262 void loop() {

263 gyro_signals();
264 RateRoll-=RateCalibrationRoll; Measure the rotation
265 RatePitch-=RateCalibrationPitch; rates and subtract
266 RateYaw-=RateCalibrationYaw; the calibration values
(project 5)
267 kalman_1d(KalmanAngleRoll,
KalmanUncertaintyAngleRoll, RateRoll, AngleRoll); Calculate the roll and
268 KalmanAngleRoll=Kalman1DOutput[0]; pitch angles through
KalmanUncertaintyAngleRoll=Kalman1DOutput[1]; the Kalman filter
269 kalman_1d(KalmanAnglePitch, (project 15)
KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
270 KalmanAnglePitch=Kalman1DOutput[0];
KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

271 AccZInertial=-sin(AnglePitch*(3.142/180))*AccX Calculate the vertical

+cos(AnglePitch*(3.142/180))*sin(AngleRoll* acceleration and the
(3.142/180))* AccY+cos(AnglePitch*(3.142/180))* altitude (project 18)
cos(AngleRoll*(3.142/180))*AccZ;
272 AccZInertial=(AccZInertial-1)*9.81*100;
273 barometer_signals();
274 AltitudeBarometer-=AltitudeBarometerStartUp;
275 kalman_2d();

276 read_receiver();

277 DesiredAngleRoll=0.10*(ReceiverValue[0]-1500); Calculate the desired

278 DesiredAnglePitch=0.10*(ReceiverValue[1]-1500); angles from the re-
279 DesiredRateYaw=0.15*(ReceiverValue[3]-1500); ceiver (project 16)

280 DesiredVelocityVertical=0.3*(ReceiverValue[2]- Calculate the desired

1500); velocity from the re-
281 ErrorVelocityVertical=DesiredVelocityVertical- ceiver and start the
VelocityVerticalKalman; PID loop for the
282 pid_equation(ErrorVelocityVertical, throttle
PVelocityVertical, IVelocityVertical,
DVelocityVertical, PrevErrorVelocityVertical,
PrevItermVelocityVertical);

184
283 InputThrottle=1500+PIDReturn[0]; Because the zero ve-
PrevErrorVelocityVertical=PIDReturn[1]; locity point and thus
PrevItermVelocityVertical=PIDReturn[2]; the hover point will
be around the point
where the throttle
stick is in the middle
(1500 µs), add this
value to the PID out-
put

284 ErrorAngleRoll=DesiredAngleRoll- Calculate the differ-

KalmanAngleRoll; ence between the de-
285 ErrorAnglePitch=DesiredAnglePitch- sired and the actual
KalmanAnglePitch; roll and pitch angles
(project 16)

286 pid_equation(ErrorAngleRoll, PAngleRoll, Calculate the desired

IAngleRoll, DAngleRoll, PrevErrorAngleRoll, roll and pitch rota-
PrevItermAngleRoll); tion rates through the
287 DesiredRateRoll=PIDReturn[0]; outer loop PID con-
PrevErrorAngleRoll=PIDReturn[1]; troller (project 16)
PrevItermAngleRoll=PIDReturn[2];
288 pid_equation(ErrorAnglePitch, PAnglePitch,
IAnglePitch, DAnglePitch, PrevErrorAnglePitch,
PrevItermAnglePitch);
289 DesiredRatePitch=PIDReturn[0];
PrevErrorAnglePitch=PIDReturn[1];
PrevItermAnglePitch=PIDReturn[2];

290 ErrorRateRoll=DesiredRateRoll-RateRoll; Calculate the differ-

291 ErrorRatePitch=DesiredRatePitch-RatePitch; ence between the de-
292 ErrorRateYaw=DesiredRateYaw-RateYaw; sired and the actual
pitch, roll and yaw
293 pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, rotation rates. Use
DRateRoll, PrevErrorRateRoll, these for the PID
PrevItermRateRoll); contoller of the inner
294 InputRoll=PIDReturn[0]; loop (project 12)
PrevErrorRateRoll=PIDReturn[1];
PrevItermRateRoll=PIDReturn[2];

185
Ca rbon aeronautics
 The flight controller: velocity mode

295 pid_equation(ErrorRatePitch, PRatePitch,

IRatePitch, DRatePitch, PrevErrorRatePitch,
PrevItermRatePitch);
296 InputPitch=PIDReturn[0];
PrevErrorRatePitch=PIDReturn[1];
PrevItermRatePitch=PIDReturn[2];
297 pid_equation(ErrorRateYaw, PRateYaw,
IRateYaw, DRateYaw, PrevErrorRateYaw,
PrevItermRateYaw);
298 InputYaw=PIDReturn[0];
PrevErrorRateYaw=PIDReturn[1];
PrevItermRateYaw=PIDReturn[2];

299 if (InputThrottle > 1800) InputThrottle = 1800; Limit the throttle val-
ue to 80% (project
12)

300 MotorInput1= 1.024*(InputThrottle-InputPitch- Use the quadcopter

InputRoll-InputYaw); dynamics equations
301 MotorInput2= 1.024*(InputThrottle+InputPitch- (project 11)
InputRoll+InputYaw);
302 MotorInput3= 1.024*(InputThrottle+InputPitch+
InputRoll-InputYaw);
303 MotorInput4= 1.024*(InputThrottle-InputPitch+
InputRoll+InputYaw);

304 if (MotorInput1 > 2000)MotorInput1 = 1999; Limit the maximal

305 if (MotorInput2 > 2000)MotorInput2 = 1999; power commands
306 if (MotorInput3 > 2000)MotorInput3 = 1999; sent to the motors
307 if (MotorInput4 > 2000)MotorInput4 = 1999; (project 12)

308 int ThrottleIdle=1180; Keep the quadcop-

309 if (MotorInput1 < ThrottleIdle) MotorInput1 = ter minimally at 18%
ThrottleIdle; power during flight
310 if (MotorInput2 < ThrottleIdle) MotorInput2 =
ThrottleIdle;
311 if (MotorInput3 < ThrottleIdle) MotorInput3 =
ThrottleIdle;
312 if (MotorInput4 < ThrottleIdle) MotorInput4 =
ThrottleIdle;

186
313 int ThrottleCutOff=1000; SAFETY RELAT-
314 if (ReceiverValue[2]<1050) { ED LINES: stop the
315 MotorInput1=ThrottleCutOff; motors when throttle
316 MotorInput2=ThrottleCutOff; stick is fully down
317 MotorInput3=ThrottleCutOff;
318 MotorInput4=ThrottleCutOff;
319 reset_pid();
320 }
321 analogWrite(1,MotorInput1); Sent the commands
322 analogWrite(2,MotorInput2); to the motors
323 analogWrite(3,MotorInput3);
324 analogWrite(4,MotorInput4);

325 battery_voltage(); Keep track of battery

326 CurrentConsumed=Current*1000*0.004/3600+ level (project 9)
CurrentConsumed;
327 BatteryRemaining=(BatteryAtStart-
CurrentConsumed)/BatteryDefault*100;
328 if (BatteryRemaining<=30) digitalWrite(5, HIGH);
329 else digitalWrite(5, LOW);

330 while (micros() - LoopTimer < 4000); Finish the 250 Hz

331 LoopTimer=micros(); control loop
332 }

Start-up and flying your quadcopter

To start and fly your quadcopter with the new vertical velocity-mode flight con-
troller, follow the same steps as with your rate-mode controller and your stabi-
lize-mode controller. Remember that this controller is meant for flying indoors.
You should notice that flying the quadcopter with your new flight controller is
even easier as the controller facilitates the hovering. Congratulations, you have
successfully reached the end of the practical part of this manual!

187
Ca rbon aeronautics
 Part IV: quadcopter
design and simulation
in this final part, you will learn how to de-
sign and simulate your quadcopter in order
to fully understand its control.

first, you will characterize the main com-

ponents; the power train, sensors and the
controller coded in your Teensy. This ena-
bles you to describe your quadcopter math-
ematically and optimize the PID values you
use in the controller.

at the end of this part, you will be fully

equipped to design, build and operate a to-
tally new quadcopter yourself.
 Project 21
Motor and sensor simulation

desired rotation rate motor power motor thrust

rotation rate + error PId command motor and torque quadcopter
- controller dynamics dynamics

measured quadcopter
rotation rate sensor rotation rate
dynamics

Your two bladed propeller generates a less thrust than the three bladed 3035 propel-
ler, although it is driven by a motor that turns less fast (4500 kV instead of 5000 kV).
The reason is simple; the three bladed propeller ‘catches’ more air under its surface,
generating more thrust but also requiring the motor to overcome more air resistance;
this means that the current consumption increases as well.

The thrust and current relation with the motor power evolves almost linearly; this
means that you can simply perform linear regression in order to estimate the throt-
tle-thrust and throttle-current relationship. For the GEPRC 1105 5000 kV motor and
3018 propeller combination, these relationships are:
thrust [g] = 160 · throttle [0 : 1]
current [A] = 4.4 · throttle [0 : 1] + 0.132

Where the throttle is a value between 0 (0%) and 1 (100%). The measurements from
which these linear regressions
rpm are constructed39were performed at a constant battery
000 rounds/min
= 650 Hz
V. V · 7.8 V = 39 000 rpm or
voltage of 7.8 5000 60 s/min
190
output(time) = (1 − e− τ ) · input(time)
t
Le arn to characterize your motors and sensors
Understanding how to characterize the motors and sensors you use to fly is
critical for the design and simulation of your quadcopter. The static and dy-
namic aspects of both components will be investigated thoroughly during this
theoretical project.

When starting to think about the design and simulation of your quadcopter, it is
useful to take a step back and look to the basic loop of the rate-mode controller, visu-
alized in the figure to the left. You start with a desired rotation rate, given by the posi-
tion of the sticks of your radiocontroller. The desired rotation rate is fed to your PID
controller and gives a motor power command. The power command is subsequently
converted to a certain amount of thrust generated by each individual motor, which
in turn will affect the movement (e.g. rotation rate) of your quadcopter. The rotation
rate is then measured with your sensor, compared to the desired rotation rate and fed
back into your PID controller to start the second loop. In this project, you will char-
acterize two important components of this control loop: the motors and the sensors.
Static motor modelling
When modelling your brushless motors, you are primarily interested in the relation
between the throttle and the thrust provided by your motor-propeller combination,
as well as the current consumption. A general rule in quadcopter design says
that the maximal thrust generated by your four motors should be equal to two
times the quadcopter weight, to ensure enough flexibility during flight. This
means that one motor should generate about half the thrust necessary to hover the
quadcopter. Your quadcopter weights around 250 gram (g), which means that the
maximal thrust of one motor should be equal to at least 125 g.

The relation between the thrust and throttle can be measured with a thrust bench; you
can create this device yourself by mounting your motor on a load cell connected to a
microchip (for example the HX711), amplifying the load cell signals and connecting
this in turn to your Teensy. When you have a lab power supply, you can also measure
the current that goes to the motors during testing. The datasheet of the motor man-
ufacturer usually gives the maximal thrust and current for a given type of propeller.
The thrust and current data for the motors used in this project are visualised on the
next pages: the relation between the motor thrust and current is displayed each time
with respect to the throttle level. To highlight the importance of the propeller used,
your two bladed propeller (Gemfan 3018) is compared to a three bladed propeller
(Gemfan 3035).

191
 Motor and sensor simulation

This is important because a higher/lower voltage will off course generate a higher/
lower thrust and current. For a quadcopter that weighs about 250 g, the motors need
to provide a thrust equal to 4 x 62.5 g when hovering. Using the formula from the
previous page, this corresponds to a power level for one motor of 62.5 / 160 = 0.4
or 40%. The current consumption in this case will be equal to 4.4 . 0.4 + 0.132 = 1.9
Ampere (A) for each motor, or 7.4 A for all motors together. This means that a 1300
mAh (=1.3 Ah) battery will be able to keep your quadcopter 1.3 Ah / 7.8 A = 0.17
h or 10 minutes in the air. In reality this value will be lower, as you do not only hover
but also move the quadcopter around and accelerate, consuming more energy than
during static hovering. The above calculation gives you nonetheless a good upper
limit of the flight time.

By dividing the thrust with the consumed current, the motor efficiency (in g/A) can
also be determined as displayed on the figure to the right. Two bladed propellers will
be slightly more efficient than three bladed propellers and the larger the propeller, the
more efficient as well.
Dynamic motor modelling
Besides the generated thrust and the consumed current of your motor, another im-
portant parameter is necessary for modelling; how fast does your motor accelerate
when increasing the motor power? This is important because your PID controller
generates a new command every 0.004 seconds, but your motor generally needs more
time to accelerate/decelerate up to this desired value. The easiest way to measure this
(usually very short) time delay is by recording the sound; both your motor and your
propeller generate noise which changes in amplitude and frequency when their speed
increases or decreases. You can configure your Teensy in such a way that it gives a
step increase for one of the motor power commands and record the sound with your
cellphone during this step increase. The evolution of the sound wave amplitude is
visualised in the figure below:
sound wave amplitude motor power command
1500 µs (50%)

0.4

motor power command

-0.4 1150 µs (11.5%)

0 0.1 0.2 0.3 Time [s]

192
 Thrust

200 g
three bladed 3035 propeller
gEPRC 1206 motor (4500 kV)
150 g

100 g
two bladed 3018 propeller
62.5 g
gEPRC 1105 motor (5000 kV)
50 g

1000 µs 1200 µs 1400 1600 1800 2000 Throttle

0% 20% 40% 60% 80% 100%
Current

6A

4A

2A

1000 µs 1200 µs 1400 1600 1800 2000 Throttle

0% 20% 40% 60% 80% 100%
motor efficiency
40 g/A

30 g/A

20 g/A

10 g/A

1000 µs 1200 µs 1400 1600 1800 2000 Throttle

0% 20% 40% 60% 80% 100%

193
Ca rbon aeronautics
 Motor and sensor simulation

At 0.11 seconds, the motor power command changes from 1150 µs or 11.5% to
1500 µs or 50%. Immediately the amplitude of the sound coming from the motor/
propeller increases. It takes around 0.1 to 0.2 seconds before the amplitude reaches a
new steady state. To characterize the time delay more accurately, you will not look at
the amplitude change, but at the frequency change of the sound during acceleration.

Let’s now take a fast Fourier transform to transform the sound signal from the time
domain to the frequency domain. By doing this, you will lose the information on the
time, so let’s do it first for the part at which the motor power turns stationary at 1150
thrust [g] = 160 · throttle [0 : 1]
µs or 11.5% throttle. You know that your motor has a 5000 kV rating. Since kV is the
equivalent for rpm/V andcurrent [A]was
the test = 4.4 · throttle
done with [0 : 1] + 0.132
a constant battery voltage of 7.8 V,
your motor frequency at full throttle (100 %) will be equal to:
rpm 39 000 rounds/min
5000 · 7.8 V = 39 000 rpm or = 650 Hz
V 60 s/min

Since your propeller has two blades, the frequency of the sound emitted by the pro-
pellers will be equal to 2 xoutput(time) = (1Hz ) · input(time)
− τt
650 Hz = 1300 − eat full throttle and without losses. When
not at full throttle, the ESC will lower the average voltage in order for the motor
to spin slower. At 11.5% motor power,  the
 fast  Fourier transform of the emitted
1 1
sound gives the frequencyoutput(s)
spectrum= displayed
− on 1 the · input(s)
figure below. The first peak in
s s+
the spectrum is due to the sound emitted by1 theτ motor; it is situated at 280 Hz for
this low motor power, meaning that the s + τ −turnss
output(s) = motor 
1
at 280 Hz x 60 s/min = 16 800
 · input(s)
rpm. The second and largest peak is exactly equal
s · s + τ to 2 x 280 Hz or 560 Hz: this is the

two-bladed propeller frequency. Further in the 1 spectrum,

input(s)some small additional peaks
output(s) = ·
are recorded at equal intervals; these are the
τ · spropeller
+1 sexcitation frequencies.

sound wave magnitude 1

(motor power: 1150 µs) output(s) = · input(s)
propeller frequency τ ·s+1
(560 Hz)
ωc
output(s) =
propeller excitation · input(s)
motor frequency s + ωcfrequencies
(280 Hz) (n x 560 Hz with n=2,3,...)

2 · π · 10
output(s) = · input(s)
s + 2 · π · 10

0 500 1000 1500 2000 2500 frequency [Hz]

A second fast Fourier transform is used to visualize the frequency after the motor
power increase, at 1500 µs or 50% power. The first peak coming from the sound of
the motor is now situated at 400 Hz, meaning that the motor turns at 400 Hz x 60 s/
min = 24 000 rpm. The propeller frequency is now equal to 2 x 400 Hz or 800 Hz.

194
In the higher frequency ranges, you find once again the propeller excitation frequen-
cies.
sound wave magnitude
(motor power: 1500 µs)
propeller frequency
(800 Hz)

propeller excitation frequencies

motor frequency (n x 800 Hz with n=2,3,...)
(400 Hz)

0 500 1000 1500 2000 2500 frequency [Hz]

This means that purely by recording the sound emitted by the motor, our fast Fourier
transform reveals that the motor frequency increases from 280 Hz at 11.5% throttle
to 400 Hz at 50% throttle. However, with this transformation all information with
regard to time is lost. This can be solved with the so-called spectrogram; with a spec-
trogram, the frequencies are displayed for each chosen time interval allowing you to
follow the change in frequency over time. The spectrogram for the acceleration of
our motor (so frequency range up to 500 Hz) is given in the picture below. You can
nicely follow the frequency increase from 280 Hz at a motor power of 11.5% to 400
Hz at a motor power of 50%. The time it takes for the motor to accelerate up to 400
Hz is equal to 0.09 seconds. This key piece of information will enable you to model
the transfer function for the motor.

frequency step time ≈ 0.09 s

400 Hz

300 Hz
280 Hz
200 Hz

100 Hz

0
0 0.1 0.2 0.3 0.4 Time [s]

195
Ca rbon aeronautics
 Motor and sensor simulation

Transfer function for the motor response

You will now describe the time response of the motor compared to the command
you have given to the motor mathematically. This is done using a transfer function.
When looking at the behaviour of the motor visualized on the spectrogram, it resem-
bles a first order response to a step input. Consider a general example in which a step
input command of 1 is given at time = 0 seconds to your motor. Mathematically, this
thrust [g] = 160 · throttle [0 : 1]
can be described as:
current [A] = 4.4 · throttle [0 : 1] + 0.132
input(time)= 0 if time < 0
input(time)= 1 if time > 0
rpm 39 000 rounds/min
5000 · 7.8 V = 39 000 rpm or = 650 Hz
V 60 s/min
Describe now the first order response of a motor using one time-related parameter τ:

output(time) = (1 − e− τ ) · input(time)
t

thrust [g] = 160 · throttle [0 : 1]

As visualised on the figures to the
current [A] right
= 4.4 with different step-response lengths, τ is de-
1 · throttle 1 [0 : 1] + 0.132
fined as the time at whichoutput(s)
the motor = output − reaches 95% of the desired value divided
· input(s)
+1
thrust [g] =s 160 ·sthrottle [0 : 1]
by 3. For our motor, this gives a time parameter ττof τ=0.09 seconds/3=0.03 seconds.
rpm current
For further calculations, [A] = 4.4
your transfer s· throttle
+ τ1 −
function [0 :rounds/min
39needs
s000 1]to+ 0.132
be written in the frequency
5000 V = 39 000=rpmor 1  · input(s)
· 7.8 output(s) = 650 Hz
domain, not the timeVdomain. This means that you
s · s + τ 60
needs/min
the Laplace transform of the
above equation. The Laplace transform of 11is easyinput(s) and equal to 1/s. In this notation,
rpm 39 000 rounds/min
5000
s is not the unit · 7.8
of time, butVoutput(s)
=
a 39 000=
complex rpm or
number
+−1
· with
s the form=s=650σ+i.ω.
Hz You also
V output(time) = (1 · τ ) · 60 s/min
input(time)
t
τ − s e
know that the Laplace transform of e is equal to 1/(s+a) by convention, so trans-
-a t

forming the above equation to the Laplace domain 1 t gives you:

output(s)=
output(time) = (1 − input(s)
1 τ · s e+11) · input(time)
 − τ ·
output(s) = − 1 · input(s)
s s+ τ
 1 
output(s) 1 s + ωτc −
1 s · input(s)
output(s) = = =−s+ ωc11·input(s)
output(s) · input(s)
s s · ss++τ τ
s + 11 − s input(s)
output(s)== 2 ·πτ · 10· · input(s)
output(s)
output(s) = sτ· · ss + 11 · input(s)
s
s + 2 ·+ π ·τ 10
And input(s)/s represents the unit step input1 considered
input(s) in this example. In reality,
you can have any input thatoutput(s)
you want,= giving
output(s) = τ · s1+ 1the· final transfer function for a first
s
· input(s)
order response: τ ·s+1

1
output(s) = ωc · input(s)
output(s) =τ · s + 1· input(s)
s + ωc
With τ being equal to 0.03 seconds for yourωcmotor. This transfer function is a good
approximation for the dynamicsoutput(s)
of your=2 · π · 10· input(s)
output(s) = motor.
s + ωc · input(s)
s + 2 · π · 10

2 · π · 10
output(s) = · input(s)
s + 2 · π · 10 196
 motor command
Input(Time)
1

0.5

0
0 1 2 3 4 5 6
Time [s]
motor command
motor response
Input(Time)
1
1-e-3/τ=0.95
1-e-2/τ=0.86
output(Time)
1-e-1/τ=0.63
0.5

0
0 1 2 3 4 5 6
Time [s]
Time @ 95% settled 3s
τ= = =1
3 3
motor command
motor response
Input(Time)
1
1-e-6/τ=0.95
1-e-4/τ=0.86

1-e-2/τ=0.63 output(Time)
0.5

0
0 1 2 3 4 5 6
Time [s]
Time @ 95% settled 6s
τ= = =2
3 3

197
Ca rbon aeronautics
 rpm current [A] = 4.4 · throttle [0 :rounds/min
39 000 1] + 0.132
5000 · 7.8 V = 39 000 rpm or = 650 Hz
V 60 s/min
Motor and sensor simulation
rpm 39t 000 rounds/min
5000 · 7.8 V = 39 000=rpm
output(time) (1 −ore− τ ) · 60
input(time) = 650 Hz
Dynamic sensorVmodelling s/min

The state of your quadcopter (absolute  angle

 tand
 angular rates) is measured by a
output(time) = 1 (1 − e−1 τ ) · input(time)
separate measurement system; the MPU-6050
output(s) = − sensor. This measurement system also
· input(s)
s + τ1
has a dynamic response, as it does nots respond instantaneously to the demands of
1
your Teensy to provide some s +
data on the quadcopter
− s  state. However, the sampling
output(s) =1  τ 11  · input(s)
output(s) =
time of your MPU-6050 is equal to 1 kHz ss+ · input(s)
ss · or 1000
+τ τ1 Hz, which is much faster than the
−
speed of your control loop (250 Hz). In addition, you also configured a 10 Hz low
output(s)and s + 1τ1 − s· input(s)
pass filter for both the accelerometer
output(s) == τthe·s + gyroscope s in your code. With this filter,
11  · input(s)
s· s+ τ
you are able to filter out unwanted high-frequency motor vibrations. Hence to model
the time response of your MPU-6050 accurately, 1 input(s)
you only need to model its low-pass
output(s) = 1 ·
output(s)The
filter as its has the lowest frequency. · s + 1 · input(s)
= τ transfer s
function of any low-pass filter in
τ ·s+1
the frequency domain is equal to:
output(s) ω1c input(s)
output(s)== τ · s + 1· ·input(s)
s+ω c

where ωc =2.π.fc with fc being output(s)

the cutoff=2frequency
ω of the filter, 10 Hz in this case.
· π c· 10· input(s)
output(s) =
This means that the frequency domain stransfer
s + · input(s)
ωcfunction for your sensor becomes:
+ 2 · π · 10

2 · π · 10
output(s) = · input(s)
s + 2 · π · 10

Now what does this practically mean in the time domain? Well, the transfer function
can be transformed to 1/(1/(2.π.10)s+1), which gives a first order step response τ of
1/ 2.π.10 or 0.016 seconds. With this response, the same reasoning holds as with the
motor response; any inputs that occur faster than 3.τ = 3. 0.016 seconds will be signif-
icantly attenuated. An input of 0.016 seconds (=τ) for example, is attenuated to 63%
of its value as 1-e-0.016/0.016=0,63. A fast vibration with a response of 0.001 seconds
gets almost fully attenuated: 1-e-0.001/0.016 = 0.06 or 6%. This is why configuring a 10
Hz low-pass filter in your MPU-6050 sensor was sufficient to filter out the high-fre-
quency vibrations coming from your motors.

198
199
Ca rbon aeronautics
 Project 22
Quadcopter dynamics simulation

desired rotation rate motor power motor thrust

rotation rate + error PId command motor and torque quadcopter
- controller dynamics dynamics

measured quadcopter
rotation rate sensor rotation rate
dynamics

motor 4 z
X motor 4 X
y
y Roll around the z
motor 3 X axis
motor 1
motor 1

motor 2 motor 3

motor 2

2d representation 2d representation

Thrustmotor 3+4

Thrustmotor 3+4 z z
Roll around the
Thrustmotor 1+2 X axis y θRoll
Torquex
y Torquex Thrustmotor 1+2

distancemotor,x= 8 cm distancemotor,x= 8 cm

InputThrottle InputRoll
output motor 1 = 25% power = 50% - 25%
output motor 2 = 25% power = 50% - 25%
output motor 3 = 75% power = 50% + 25%
output motor 4 = 75% power = 50% + 25%

200
De scribe how the quadcopter moves in space
A mathematical discussion on quadcopter dynamics is essentially an analysis
of its roll, pitch, yaw and throttle reactions. As you already saw when pro-
gramming your the flight controller, these movements can all be calculated
separately; this means that all four movements can be described independent
of each other. While it is not fully true that these movements are independent
from each other, it will prove to be a very good approximation.

The previous project ended with the mathematical description of the motors and
sensor dynamics using transfer functions. In this project, you will try to find the trans-
fer function for the roll, pitch and yaw rotation rates, and also for the vertical velocity.
These four transfer functions will form the "quadcopter dynamics". All movements
of the quadcopter are essentially generated by the thrust and torque developed by
your motors, in combination with the gravitation force that acts on it during flight.
You will describe the roll motion of the quadcopter to derive the roll dynamics and
proceed subsequently with the other movements.

Roll and pitch motion

Let’s first try to describe the roll motion of the quadcopter in mathematical terms.
To rotate the quadcopter in the roll direction, you need to apply a torque around the
stationary x-axis: Torquex. The resulting angular acceleration Accelerationroll in the roll
direction depends on this applied torque, but also on the distribution of mass of the
object. This mass distribution is described by the moment of inertia Ix, which results
in the following relation:
Torque [N · m]
Accelerationroll [rad/s] =
Ix [kg · m2 ]
Where N is the unit Newton (equivalent to kg.m²/s²)
Torque [N ·and m] rad the angle unit radians.
Accelerationroll [rad/s] = [N · m] 2180
Torque
In order to convert the angular ◦ acceleration
2
Accelerationroll [ /s ] = x Ifrom
x [kg · radians/s²
m· ] to °/s², the formula
needs to be multiplied with 180/π (°/rad): Ix [kg · m2 ] π
Torquex [N · m] 180
Accelerationroll [◦ /s2 ] = ·
Torquex [N ·m] = Thrustmotor 3+4 [N ]·distanceImotor,x x [kg [m]−Thrust
· m2 ] π motor 1+2 [N ]·distancemotor,x [m]
Torque [N · m]
Accelerationroll [rad/s] =
Ix [kg · m2 ]
Torque around
Torque the x axis
x [N ·m] = Thrustmotor 3+4 [N ]·distancemotor,x [m]−Thrustmotor 1+2 [N ]·distancemotor,x [m]
thrust [g] = 160 · throttle [0 : 1]
◦ 2 Torquex [N · m] 180
The torque around Acceleration roll [ depends
the x axis /s ] = on the combination
· of the thrust from all four
Ix [kg · m2 ] π
motors and the distance ofthrust each
 [g]motor
= 160 to the x axis:
· throttle [0 : 1]
inputroll [0 : 1000] 9.81 [m/s2 ]
thrust [N ] = 160 · [g] ·
1000
Torquex [N ·m] = Thrustmotor 3+4 [N ]·distancemotor,x [m]−Thrustmotor 1000 [g/kg] [N ]·distance
1+2 motor,x [m]
 
input [0 : 1000] 9.81
9.81 [m/s
[m/s ] ]
2 2
thrust [N[N
thrust ] =] = 160 ·· (inputroll [0 : 1000]) [g][g]
0.160 roll · ·
1000 1000
1000 [g/kg]
[g/kg]
201
thrust [g] = 160 · throttle [0 : 1] 9.81 [m/s2 ]
thrust [N ] = 0.160 · (inputroll [0 : 1000]) [g] · 2 ]
9.81 [m/s
Torquex [N ·m] = 4·(0.160·inputroll [0 : 1000]) [g]· 1000 [g/kg]
·distance motor,x [m]
  1000 [g/kg]
2
 Torquex [N · m] 180
Accelerationroll [◦ /s2 ] = ·
Ix [kg · m2 ] π
Torque [N · m]
Accelerationroll [rad/s] =
Ix [kg · m2 ]
Quadcopter dynamics simulation
Torquex [N ·m] = Thrustmotor 3+4 [N ]·distancemotor,x [m]−Thrustmotor 1+2 [N ]·distancemotor,x [m]
You already determinedAcceleration
the thrust◦ of2[rad/s] Torque
oneTorque
motor [N · 180
(in· m]
[N gram m]g):
= x
Accelerationroll [ roll
/s ] = ·
[kg
Ix [kg I·xm 2 ] · m2 ]π
thrust [g] = 160 · throttle [0 : 1]

This can be xtransformed tomotor

N (or kg.m²/s²) Torque x [N · m] 180the result
by multiplying with 9.81 m/
Torque [N ·m] = Thrust
Acceleration 3+4 [N ◦ ]·distance
[ /s ] =
roll
2
motor,x [m]−Thrust · motor 1+2 [N ]·distancemotor,x [m]
s² (the gravitational constant) divided input by 1000
[0 : x [kg
g/kg.
I1000] ·m2 ]Moreover,
π[m/s2you
9.81 ] do not input a
roll
thrust [N ] = 160 · [g] ·
motor power between 0 and 1 but rather1000 a valueTorque between m]0 [g/kg]
[N ·1000 µs and 1000 µs through
Accelerationroll [rad/s] =
the parameter [N ·m]roll=(although
Torquexinput thrust
Thrust in3+4
[g]
motor the
= 160
[N PID loop
· throttle
]·distance itx [kg
I[0
motor,x :is1]restricted
] [m/s
· m29.81
[m]−Thrust to2 ]a 1+2
motor maximal valuemotor,x
[N ]·distance of [m]
thrust [N ] = 0.160 · (inputroll [0 : 1000]) [g] ·
400 µs). The formula transforms to: 1000 [g/kg]
 Torque [N · m] 180 2
Accelerationroll [◦ /s2 ] [0
input = 1000] x
thrust [N ] = 160 thrust
· [g] =roll 160 ·: throttle [0m:·21]
Ix [kg 9.81
·[g]
9.81· [m/s ]
][m/s2 ]π
1000
Torquex [N ·m] = 4·(0.160·inputroll [0 : 1000]) [g]· 1000 [g/kg]
·distancemotor,x [m]
1000 [g/kg]
9.81 [m/s 2
]
thrust
Torquex [N ·m] = [NThrust
] = 0.160 · (input

roll [0[0: :1000])
[N ]·distance
[g] ·
motor,x [m]−Thrust 2 1+2 [N ]·distancemotor,x [m]
motor 3+4 input roll 1000] 9.81
1000 [g/kg][m/s motor ]
thrust [N ] = 160 ·  
Torque[g][N 2· · m] 2

Ix [kg · m2 ] = roll mass
Acceleration [rad/s] i [kg]
1000 = · distancemass,x 1000 [m[g/kg]
]
The value of inputroll is equal fori=1 all four motors, Ix [kg
but· m has2]
a positive sign for motors
9.81 [m/s 9.81 [m/s2 ]
· ] ·distance
2
3 and Torque thrust
4 and xa[Nnegative
·m] [N ] =thrust
sign 0.160
for
= 4·(0.160·input ·roll(input
motors
[g] [0=:160 ·[0throttle
1rolland
1000]) :[g]·
1000])
2. This [0 [g] means
: 1] that the torque
[m] formula
1000 [g/kg] 1000 [g/kg] motor,x
can be rewrittenIx as:
= 4 · massmotor · distance 2
Torque
+ 4 · [N
mass · m]
ESC · 180
distance 2
Accelerationroll [◦ /s2motor,x ]= x
· ESC,x
Ix [kg  · m2 ] π
 
9.81 [m/s ]
2 
Ix [kg 2 inputroll [0 ·: distance
1000] 2 9.812 [m/s ] 2
Torquex [N thrust
·m] = [N ] =] =160 · mass
4·(0.160·input
·m [kg]
roll [0i : 1000]) [g]· [g] · [m ]·distancemotor,x [m]
mass,x
◦ 2
i=1 180 1000 4 · 0.160 1000· input [g/kg]
10009.81 [g/kg]
roll · 1000 · 0.08
TorquexAcceleration roll [ motor
[N ·m] = Thrust /s ] 3+4
= [N ]·distance· motor,x [m]−Thrust [N ]·distancemotor,x [m]
π 4 · 0.008 · 0.08 +9.81 2 4 · 0.007
[m/s ·20.04
motor
] 1+22
Moment of inertia thrustaround the
[N ] = 0.160 x· (input
axis
 roll [0 : 1000]) [g]2 · 2
Ix = 4 · mass · m· ]distance
Ix [kgmotor 2
=
2
mass
motor,x + 4 ·· distance
i [kg] massESCmass,x 1000
· distance [g/kg]
2
[m ] ESC,x
i=1
2
The moment of inertia Ix Acceleration
inthrust [g] = [ /s· throttle
roll160
turn describes ] = 115
the · input
[0
weight : 1]distribution of the quadcopter
◦
roll
9.81 [m/s9.81 2
]
and the Torque
distance of
x [N ·m] all
= components
4·(0.160·input
Ix = 4 · massmotor 180 to
[0
2 :4the
1000])
· stationary
0.160 [g]·
· input x · axis. Using
0.08
·distance
· 2 the [m]
motor,x two-dimen-
[◦ /s2 ]· distance · motor,x + 4 · mass · distance
roll
Acceleration = 1000ESC[g/kg]
roll 1000
ESC,x
sional approximation ofrollthedquadcopter,π the moment
4 · 0.008 · 0.082 + 4of· 0.007
inertia can
· 0.042 be calculated as:

(Rate input roll [0=

[◦ /s]) : 1000]
115 · input 9.81 [m/s ] 2
thrust [N ] =dt160 ·Roll [g] ·roll
2

1000 2 10009.81 [g/kg]

Ix [kg · m ] = mass [kg] distance
180◦ 2i 4 · 0.160 · input [m2 ]
roll · 1000 · 0.08
· mass,x
Acceleration
Acceleration 2
=roll [ /s
roll [ /s ] i=1
◦
· ] = 115 · input roll9.81 [m/s2 ]
π 4roll· [0
thrust [N ] = 0.160 · (input 0.008 · 0.08[g]
: 1000]) 2+
· 4 · 0.007 · 0.042
1000 [g/kg]
Where massi is the mass of component2 i, and distancemass,i the distance between com-
Ix = 4 · massdmotor · distancemotor,x + 4 · massESC · distance2ESC,x
ponent i and the x-axis. Most weight
Acceleration
(RateRoll [roll
◦ of
/s])
◦ the
[ /s 2
= ]115 quadcopter
= 115 · inputroll 2
· input is situated very close to the
9.81roll[m/s ]
x-axis: the battery
Torque and
x [N ·m] alldtthe on-board
= 4·(0.160·input roll [0 : electronics
1000]) [g]· are stacked along
·distance the[m]x-axis. Be-
motor,x
1000 [g/kg]
cause this gives a very small ◦distance,
2 180their4contribution
· 0.160 · inputroll to· I1000 is ·negligible.
9.81
0.08 The weight
Accelerationroll d[ /s ] = · x
of the frame is very small meaning (Rate that

Rollπ [ ◦ this
4
/s]) · = contribution
0.008
115 ··0.08
input2 + 4 is
· also
0.007  ·negligible.
0.04 2 Therefore,
dt2 ] = 2roll
the only componentsIxwith [kg · m massi [kg]
a non-negligible · distance
weight and [m2 ]
sufficiently
mass,x far from the x-axis
i=1
are the four motors and the ESCs. The ◦formula
2
Accelerationroll [ /s ] = 115 · input reduces to:
roll

Ix = 4 · massmotor · distance2motor,x + 4 · massESC · distance2ESC,x

Knowing that the mass of deach (Ratemotor ◦ (including propeller and bolts) is 8 g with a
Roll [ /s]) = 115 · input
dt and the ESC weighs 7 groll
distance of 8 cm to the x axis, 180 4 · 0.160 · inputwith a distance
9.81
roll · 1000 · 0.08 of 4 cm to the
Accelerationroll [◦ /s2 ] = ·
x axis, you now have everything to calculate
π 4 · 0.008 the ·angular
0.08 + 4roll
2 acceleration.
· 0.007 · 0.042

Accelerationroll [◦ /s2 ] = 115 · inputroll

202
d
(RateRoll [◦ /s]) = 115 · inputroll
dt
 Torquex [N ·m] = 4·(0.160·inputroll [0 : 1000]) [g]· 9.81 [m/s22] ·distancemotor,x [m]
roll[0 : 1000]) [g]· 1000 [g/kg] ·distancemotor,x
Torquexx[N ·m] = 4·(0.160·inputroll motor,x[m]
1000 [g/kg]
 
Ix [kg · m2 ] =  massi [kg] · distance2mass,x
2 [m222]
Ixx [kg · m22] = i=1 massii [kg] · distance2mass,x
mass,x[m ]
Roll and pitch dynamics i=1
i=1

Ix = 4 · massmotor · distance22motor,x
2 + 4 · massESC · distance22ESC,x
2
Introducing the = 4 · massmotor
Ixx formulas · distance
for the
motor torque and+the
motor,x
motor,x 4 · moment
massESC
ESC · distance
of inertia in the angular roll
ESC,x
ESC,x

acceleration formula gives:

9.81
180 4 · 0.160 · inputroll · 1000 · 0.08
Accelerationroll [◦◦◦/s222] = 180 · 4 · 0.160 · input 9.81
9.81
roll · 1000 · 0.08 2
Accelerationroll
roll [ /s ] = π · 4 · 0.008 · 0.082 + 4
roll · 0.007
1000 · 0.04
π 4 · 0.008 · 0.0822 + 4 · 0.007 · 0.0422
Which results in a very simple formula for the angular acceleration in the time do-
main: Accelerationroll [◦◦◦/s222] = 115 · inputroll
Accelerationroll
roll [ /s ] = 115 · inputroll
roll

The angular roll acceleration can be replaced by the derivative of the roll rate, giving:
d
d (RateRoll [◦◦/s]) = 115 · inputroll
◦
dt (RateRoll
Roll [ /s]) = 115 · inputroll
roll
dt
For the simulation of the PID controller, you want to have your equations in the
frequency domain rather than the time domain. You can transform the time domain
to the frequency domain by performing a Laplace transform to both sides of the
equation:
s · Rateroll [s] = 115 · inputroll [s]

In this notation, s is not the unit of time, but a complex number with the form s=
σ+i.ω. This gives you finallyRate
therollrelation115between the quadcopter roll rate and the
[s] = · inputroll [s]
Ratecontroller,
input command given by thes ·PID roll [s] = 115 ·which
s inputrollis[s]called your quadcopter transfer
function:
115
Ratepitch [s] = 115 · inputpitch [s]
Rateroll [s] = s · inputroll [s]
s
Torquez [N · m] 180
Accelerationyaw [◦ /s2 ] 115
= ·
Ratepitch [s] = Iz [kgpitch
· input 2]
· m[s] π
sX
motor 4 motor 1
Torquez [N ·m] = −Torquemotor 1 +Torque motor 2 −Torque
Torque motor 3 +Torquemotor 4
z [N · m] 180
Accelerationyaw [◦ /s2 ] = ·
ESC 4 Iz [kg · mESC
2] 1π
 
N ·m
Torquemotor [N · m] = KT · Current [A]
Torquez [N ·m] = −Torquemotor 1 +Torquemotor
A 2 −Torquemotor 3 +Torquemotor 4

60 60 N ·m
KTY= = N ·m= 0.0019
2 · π [N
Torquemotor 2 ·K
· K·v m] = π T· 5000 · Current
A[A]
A

inputyaw [0 : distance
1000] motor,x= 8 cm
Current
mass = 7[A]
g = 4.4
60 · 60 +N0.132
·m
K =ECS = 1000= 0.0019
2 · π · Kv 2 · π · 5000
T
A
ESC 3  ESC 2 
inputyaw [0 : 1000]
Torquez [N · m] = 0.0019input
· 4 · yaw
4.4[0· : 1000]
Current [A] = 4.4 · 1000
+ 0.132
1000 massmotor= 8 g
  distanceECS,x
= 4 cm 
Iz [kg3· m2 ] = mass [kg] · distance2 [m2 ] + distance 2 
motor[m
2
2 ]
motor yaw [0 : 1000]
i mass,i,xinput mass,i,y
Torque
i=1 [N · m] = 0.0019 · 4 · 4.4 ·
z
1000
203
   
2 2 2 2
Iz = 4·massmotor distance
· Ca
 rbonmotor,x+ distancemotor,y +4·massECS · distanceESC,x
aeronautics  + distanceESC,y
Iz [kg · m2 ] = massi [kg] · distance2mass,i,x [m2 ] + distance2mass,i,y [m2 ]
i=1
 s · Rateroll [s] = 115 · inputroll [s]
Quadcopter dynamics simulation

You can construct a fully similar reasoning115 for the pitch dynamics, eventually giving
Rateroll [s] = · inputroll [s]
the following formula: s

s · Rateroll [s] =115

115 · inputroll [s]
Ratepitch [s] = · inputpitch [s]
s

115
Yaw motion Rateroll [s] =
Accelerationyaw [◦ /s2 ] =s
· inputzroll
Torque [N[s]· m] 180
·
Iz [kg · m2 ] π
The same basic equation for the angular 115 acceleration in the yaw direction holds as for
the roll and pitch direction, Rate
with the
pitch [s]only · inputpitchbeing
= difference [s] the torque and moment of
Torquez [N ·m] = −Torquemotor 1 +Torque s motor 2 −Torque motor 3 +Torquemotor 4
inertia, which should be calculated around the stationary z axis:
s · Rateroll [s] = 115 · inputroll [s]
Torque
 [N · m] 180
Accelerationyaw [◦ /s2 ] = N · mz ·
Torquemotor [N · m] = KT Iz [kg · m · Current
2] π[A]
A
s · Rateroll [s] =115 115 · inputroll [s]
TorqueTorque
around[Nthe z axis Rateroll [s]+Torque
·m] = −Torque
=
s
· inputroll [s]
z 60motor 1 60 motor 2 −TorqueNmotor · m3 +Torquemotor 4
KT = s · Rate =[s] =115 115 = 0.0019
· input
2 ·Rate
π · Kvroll 2 · π · 5000 roll [s] A
The torque around the z axis roll [s] =
is equal to115 the· input
sum roll of[s]all four motor torques; the mo-
Ratepitch [s] = s · N input
· m
 [s]
pitch
tor torque is opposite to the
Torque rotation input
motor [N · m] =
direction
KT yaw [0 : of
s each motor.
· Current [A] When the motor is
Current [A] =roll4.4 115 A 1000] + 0.132
spinning in a clockwise direction,
Rate the
[s] ·=force115 ·1000of
input the air pushed against the propellers
roll [s]
s · Rate
Rate [s][s]== s115 · input
· input
Torque roll [s]
z [N 180torque around the z axis
pitchroll pitch
creates a torque in theAcceleration
counter-clockwise ◦ 2 s direction. The · m]total
60 yaw [ /s ] =60 Iz [kg · m2 ] N· · πm 
is then equal to: KT = = =input0.0019 [0 : 1000]
Torquez [N 2·Rate · π ·=K0.0019 2 · 115
π 5000
4 ·· Torque
yaw A
m] pitch [s]
v
=2 · 115 ·4.4
input z [N[s]
· pitch [s]
· m] 180
Rate
Accelerationyaw [s]
◦ =
roll[ /s ] = s · input roll 2
1000·
Torquez [N ·m] = −Torquemotor 1 +Torque s Iz [kg−Torque · m ] π +Torquemotor 4
motor 2
inputyaw [0 : 1000] motor 3
Current
 [A] = 4.4 · + 0.132 
Iz [kg · m2 ] =Acceleration
massi [kg] · [◦distance Torque
21000 [N 2 · m] 180 2 2
yaw /s ] 115
2
= mass,i,x [m z ] + distance
· mass,i,y [m ]
Torque
The torque forz [N ·m] =
each motor
−Torque
i=1 is related
Rate 1 +Torque
pitch [s]
motor =to the·motor motor
Iinput
N z [kg
·m · mcurrent
2 −Torque

pitch [s]
2 ] motor 3 +Torque
πthrough the
motortorque
4 coef-
Torquemotor [N · m] = KsT  · Current [A] 
ficient KT:  A input yaw [0 : 1000]  
Torque z [N · m] 2= 0.0019 · 4 · 4.4 2 ·
· distance2ESC,x + distance2ESC,y
z = 4·mass
ITorque motor · distancemotor,x + distance  +4·mass 1000
z [N ·m] = −Torquemotor 1◦+Torque
motor,y ECS+Torque
Torque
N
motor · m2 [N
−Torque · m] 180
motor 3 motor 4
Torque
Acceleration
motor60 [Nyaw [ /s=2 ] K
· m] =60 z · Current · [A]
· m2 ] N · πm
T
K T = = IzA[kg = 0.0019
  2·π·K  2 · π · 5000 A 2 
2
Iz [kg · m2 ] = ◦ 2mass180 i [kg] · distance
v
0.0019
mass,i,x
N · m [m  2· ]4+· distance
4.4
1000
· input
mass,i,y
yaw
[m2 ]
Acceleration
This torque coefficient
yaw [i=1
TorqueKT] motor
/s =
can60 be· 4·estimated
[N = K·60 using the 2 )kV
· Current rating
[A] of the motor, which
Torquez [N ·m] = =
KT−Torque πmotor ·m]
0.008
1=+Torque
T(0.08 2 + 0.05
A= 0.0019
2 −Torque
4 ·· m
+Nmotor 0.007 · (0.042 + 0.052 )
3 +Torquemotor 4
yaw [0 : 1000]
motor
is in our case equal toCurrent
5000

2 kV
· π ·or
K
[A] = 4.4 ·
v5000 2 · rpm/V:
input
π · 5000 
A
+ 0.132  
Iz = 4·massmotor · distance2motor,x + distance 1000
2
+4·massECS · distance2ESC,x + distance2ESC,y
60 60 motor,y N ·m
KT = = inputyaw N[0· = m 0.0019
: 1000]
Torque
Current motorπ [N
2 ·[A] ·=Kv·4.4 m] ·=2 ·K π T· 5000 · Current A[A] 
+ 0.132
1000 A inputyaw [0 : 1000]
Torquez [N · m] 180 = 0.0019 · 4 · 4.4 0.0019
· · 4 · 4.4
· inputyaw
Acceleration
You already determined ◦ 2
yaw [ /sthat] = the ·current consumed
π 4 · 0.008 input· yaw [0
(0.08 2+ : 1000]
0.05
by2 )1000
1000
one
+ 4 ·m
motor is described by the
0.007· (0.042 + 0.052 )
Current 60 = 4.4 ·
[A] 60 
+[0N0.132
:· 1000]
formula below, which Torque is already
= corrected
= for1000 input
= 0.0019
input between 0 and 1000
 µs instead
z [N 2 ·=K0.0019 4 ·· 5000
4.4 ·
K · m]
yaw
 
2 ·· π yaw
 T
I [kg · m2 ] =
of a throttle
z between 0 and 1:i mass· π [kg] v
· distance 2
mass,i,x [m2 ] +1000 A 2
distance
mass,i,y [m2 ]
i=1  
inputyaw [0 : 1000]
Torque [N

· m] = 0.0019 · 4 · yaw
 input
24.4[0· : 1000] 
Iz [kg · m2 ] = Current
z mass[A] = 4.4
i [kg] · distance
· mass,i,x + 0.132 2mass,i,y
[m2 ] +1000
distance  [m2 ]2 
Iz = 4·massmotori=1 2
· distancemotor,x + distance 1000
2
motor,y +4·mass ECS · distance 2
ESC,x + distanceESC,y

    
Iz [kg · m2 ] =  massi [kg] 2 · distance2mass,i,x
2 [m2 ] + distance
[0 : ECS
1000]
2
[m2 ] 
Iz = 4·massTorque· distance
motori=1 [N · m] + distance
= 0.0019 · 4 · 4.4
input+4·mass
yaw4.4 distance2ESC,x + distance2ESC,y
·mass,i,y
◦z 2 180
motor,x 0.0019
motor,y
· · 4 ·1000
1000
· inputyaw
Accelerationyaw [ /s ] = ·
π 4 · 0.008 · (0.082 + 0.052 ) + 4 · 0.007 · (0.042 + 0.052 )
   
2 2 4.4 ECS · distance2 2042ESC,y
Iz = 4·mass2motor · distance
 motor,x+ distance
180 motor,y 2·+4·mass
2 0.0019 4 · 1000 · input
2 yaw 2 ESC,x
+ distance
Iz [kg · m ]yaw
Acceleration = [◦ /s2mass
] = i [kg]
· · distancemass,i,x [m ] + distance mass,i,y [m ]
i=1 π 4 · 0.008 · (0.082 + 0.052 ) + 4 · 0.007 · (0.042 + 0.052 )
4.4
180 0.0019· 4 · · input
 yaw
[◦ /s2 ] =
 1000 
Acceleration 2 · 2 2 2
 motor 4 z X z
X motor 1

y yaw around the

motor 3 z axis
motor 1

motor 2
motor 2
motor 4
s · Rateroll [s] = 115 · inputroll [s] y
motor 3

2d representation 115 2d representation

Rateroll [s] = · inputroll [s]
s
s · Rateroll [s] = 115 · inputroll [s] Torquemotor 2
Torquemotor 4 115
X Ratepitch [s] = · inputpitch [s]
Torquemotor 2 s Torquemotor 2
115 X
Rateroll [s]yaw
= around
· input
the roll [s]
Torquez [N · m] 180
zsaxis
Accelerationyaw [◦ /s2 ] = · Torquez
y Torquez Iz [kg · m2 ] π
115
Rate
Torque
pitch [s]2 =
motor · inputpitch [s]
s
Torquez [N ·m] = −Torquemotor 1 +Torquemotor 2 −Torquemotor 3 +Torquemotor 4
Torquemotor 4 y
Torquemotor 3 Torquez [N · m] 180
Accelerationyaw [◦ /s2 ] =  ·
Iz ·[kg
N m · m2 ]

π
Torquemotor [N · m] = KT · Current [A] Torquemotor 3
A
InputThrottle Inputyaw
Torquez [N ·m] = −Torquemotor 1 +Torquemotor 2 −Torquemotor 3 +Torquemotor 4
60 1 = 25% power
output motor 60 = 50% - 25%
N ·m
= motor 2 ==
KToutput 75% power = 50% = 0.0019+ 25%
2 · π · K 2 · π · 5000
output motor 3 = 25% power= 50% 
v - 25%
A
output motor 4 = 75% power =N 50% ·m + 25%
Torquemotor [N · m] = KT · Current [A]
inputyaw [0 A : 1000]
The value of inputyaw Current
is equal[A]for=all4.4four
· motors,
1000
but has a positive sign for motors
+ 0.132 2
and 4 and a negative sign for motors60 1 and 60 3. The torque N formula
·m results in:
KT = = = 0.0019
2 · π · Kv 2 · π · 5000
 A 
inputyaw [0 : 1000]
Torquez [N · m] = 0.0019 · 4 · 4.4 ·
1000
inputyaw [0 : 1000]
Current [A] = 4.4 · + 0.132
Moment of inertia around
 the z axis 1000 
Iz [kg · m2 ] = massi [kg] · distance2mass,i,x [m2 ] + distance2mass,i,y [m2 ]
i=1  
The moment of Torqueinertia [NI describes the weight input yaw [0 : 1000]
distribution of the quadcopter and
z z · m] = 0.0019 · 4 · 4.4 ·  1000 representation,
 
its distance
Iz = 4·massmotor · distancemotor,x + distancemotor,y +4·massECS · distance2ESC,x this
to the stationary z axis.
2 In the two-dimensional
2 gives:2ESC,y
+ distance
  
Iz [kg · m2 ] = massi [kg] · distance2mass,i,x [m2 ] + distance2mass,i,y [m2 ]
4.4
i=1 180 0.0019 · 4 · 1000 · inputyaw
Accelerationyaw [◦ /s2 ] = ·
π 4 · 0.008 · (0.08 + 0.05 ) + 4 · 0.007 · (0.042 + 0.052 )
2 2
Notice that not only the  distance of the mass to the  x axis matters, but also the dis- 
= 4·massmotor · distance2motor,x + distance2motor,y +4·massECS · distance2ESC,x + distance2ESC,y
tance ofIzthe mass to the y axis. This is different compared to Ix , as the two-dimen-
sional representation in that case meant that distancemass,i,z is approximated as being
zero. Once again only ◦the2 components
180 with a0.0019
considerable4.4
· 4 · 1000 massyawthat are far enough
· input
Accelerationyaw [ /s ] = ·
from the quadcopter X and Y πaxes 4 · are the
0.008 motors
· (0.08 and
2 + 0.05 the4 ·ESCs.
2) + 0.007 · (0.042 + 0.052 )

205
Ca rbon aeronautics
 KT = = = 0.0019 
2·π·K 5000 yaw [0 : 1000]A
2 · π · input
Torquez [N · m] = 0.0019 ·v 4 · 4.4 ·
1000
inputyaw Quadcopter
[0 : 1000] dynamics simulation
  Current [A]  = 4.4 · + 0.132 
2
Iz [kg · m ] = 2
massi [kg] · distancemass,i,x [m 1000
2 2
] + distancemass,i,y [m2 ]
This gives the following
i=1 formula:
 
Torque inputyaw [0 : 1000]
z [N
2 · m] = 0.0019 ·24 · 4.4 ·
 
Iz = 4·massmotor · distance motor,x + distancemotor,y +4·massECS · distance2ESC,x
1000 + distance2ESC,y

Where theIzdistance ofthe

[kg · m2 ] =

motors
mass and

the ESCs
i [kg] · distance
2 from
[m 2 the Y axis
4.4 ] + distance
2 are[mequal
2
]

to 5 cm.
180 0.0019 · 4 · 1000 · inputyaw mass,i,y
mass,i,x
Accelerationyaw [◦ /s2 ] =i=1 ·
π 4 · 0.008 · (0.082 + 0.052 ) + 4 · 0.007 · (0.042 + 0.052 )
Yaw dynamics    
Iz = 4·massmotor · distance2motor,x + distance2motor,y +4·massECS · distance2ESC,x + distance2ESC,y
Introducing both formulas in the angular yaw acceleration formula gives:
4.4
180 0.0019 · 4 · 1000 · inputyaw
Accelerationyaw [◦ /s2 ] = ·
π 4 · 0.008 · (0.08 + 0.05 ) + 4 · 0.007 · (0.042 + 0.052 )
2 2

Which results again in a very simple formula for the angular acceleration in the time
domain:
Accelerationyaw [◦ /s2 ] = 4.8 · inputyaw

Doing a similar Laplace Acceleration

transformation[◦ /s
as before together with the integration of the
4.82 ] = 4.8 · inputyaw
yaw
RateYawfunction
acceleration results in the transfer [s] = · inputyawyaw
[s] dynamics:
s for the

4.8
RateYaw [s] =2 · inputyaw [s] 2
massquadcopter [kg]·Accelerationz [m/s ] s= thrustmotor 1+2+3+4 [N ]−massquadcopter [kg]·9.81 m/s

motor 4 z X
massquadcopter [kg]·Accelerationz [m/s2 ] = thrustmotor 1+2+3+49.81[N ]−massquadcopter [kg]·9.81 m/s2
thrustmotor 1+2+3+4 [N ] = 4·(0.160·inputthrottle [0 : 1000])· = 0.0063·inputthrottle [0 : 1000]
motor 4 z X y 1000
motor 3

y 9.81
thrustmotor 1+2+3+4 [N ] = 4·(0.160·input
2 throttle [0 : 1000])·
Increase altitude = 0.0063·inputthrottle [0 : 1000]
motor 3 motor 1 [m/s
0.250 [kg] · Acceleration ] =the0.0063
along z z axis · input 1000 [0
throttle : 1000]
motor 1
Accelerationz [m/s2 ] = 0.025 · inputthrottle
motor 2
0.250 [kg] · Accelerationz [m/s2 ] = 0.0063 · inputthrottle [0 : 1000] motor 2
Accelerationz [m/s2 ] 2= 0.025 · inputthrottle
Accelerationz [cm/s ] = 2.5 · inputthrottle
2d representation

2d representation
Accelerationz [cm/s2 ]2.5
= 2.5 · inputThrust motor 3+4
throttle
z Thrustmotor 1+2
VelocityVertical [s] = · inputthrottle [s]
s
Thrustmotor 3+4 z Thrustmotor 1+2
Increase altitude y
2.5
along the
VelocityVertical [s] = z·axis
inputthrottle [s]
y s
Weightquadcopter

Weightquadcopter InputThrottle

output motor 1 = 75% power = 75%

output motor 2 = 75% power = 75%
output motor 3 = 75% power = 75%
output motor 4 = 75% power = 75%
206
 motor 4 X motor 1

ESC 4 ESC 1
massECS= 7 g

distanceECS,y= distancemotor,y= 5 cm
distancemotor,x= 8 cm

ESC 3 ESC 2

massmotor= 8 g
motor 3 distanceECS,x= 4 cm motor 2

Vertical velocity dynamics

Accelerationyaw [◦ /s2 ] = 4.8 · inputyaw
The mathematical Acceleration
description of[◦ /sthe2 ] =
quadcopter dynamics along the stationary Z
4.8 · inputyaw
yaw
axis is rather intuitive; the resulting acceleration along this axis is equal to the thrust
4.8
delivered by the motorsRate Yaw [s]the
minus = gravity· input yaw [s] on the quadcopter mass. Write this
acting
s
force balance as: 4.8
Rate [s] = Yaw · input [s]
yaw
s
massquadcopter [kg]·Accelerationz [m/s ] = thrustmotor 1+2+3+4 [N ]−massquadcopter [kg]·9.81 m/s2
2

2
Acceleration
massquadcopter [kg]·Acceleration 2
z [m/s ] =yaw
[◦ /smotor
thrust ] = 1+2+3+4
4.8 · input
[N ]−mass
yaw quadcopter [kg]·9.81 m/s
2

And the thrust of all fourAcceleration

motors was already
◦ 2 calculated
yaw [ /s ] = 4.8 · input
previously as:
9.81 yaw
thrustmotor 1+2+3+4 [N ] = 4·(0.160·inputthrottle [0 : 1000])· = 0.0063·inputthrottle [0 : 1000]
4.8 1000
Rate [s] = input9.81[s]
thrustmotor 1+2+3+4 [N ] = 4·(0.160·input
Yaw
throttle [0 s: 1000])· yaw = 0.0063·inputthrottle [0 : 1000]
·
4.8 1000
RateYaw
2 [s] = · input [s]
0.250 [kg] · Accelerationz [m/s ] = 0.0063 s · inputyaw throttle [0 : 1000]
In the flight controller, you programmed input 2
Inputthrottle as InputThrottle=1500+P- 2
22
mass[kg]
0.250
IDReturn[0], because
quadcopter [kg]·Acceleration
·Acceleration
Acceleration
the zerozz[m/s
[m/s
velocity=[m/s
]]z= and] ·=thus
0.025
0.0063 · thrust
input
input the hover
motor
throttle [N ]−mass
[0 : 1000]
1+2+3+4point at the[kg]·9.81
will bequadcopter point m/s
throttle

where the throttle

mass stick is in the
[kg]·Acceleration
Acceleration
quadcopter middle
z [m/s
2 [m/s (1500
2
z] = 0.025 µs). throttle
] =· thrust
input This means[N
motor 1+2+3+4 that if you
]−mass take this
quadcopter into m/s2
[kg]·9.81
account, the throttle stick at 1500 µs2 and gravity acting on the quadcopter mass cancel
9.81
thrustmotorAcceleration
1+2+3+4 [N ] = [cm/s ] = 2.5 · input
each other out. Knowing thatz 4·(0.160·input
the quadcopter [0 : 1000])·
throttle
mass
throttle is equal = 0.0063·inputthrottle [0 : 1000]
1000to 250 g, the dynamic
2 9.81
equationthrust
of the
motor [N ] =along
Acceleration
acceleration
1+2+3+4 z4·(0.160·input
[cm/sthe ] =z2.5
axis · input [0throttle
becomes:
throttle : 1000])·
1000
= 0.0063·inputthrottle [0 : 1000]
2.5 2
0.250 [kg] · Acceleration
Velocity Vertical [s] = z [m/s ] = 0.0063
· input · inputthrottle [0 : 1000]
throttle [s]
s
0.250Velocity Acceleration
[kg] · Acceleration 2.5
[m/s2 2 ] = 0.025 · input
z [m/s
Vertical [s] =
z ] = 0.0063
· input throttle [s] throttle [0 : 1000]
throttle
s 2
When Acceleration [m/s ] =
coding the flight controller,z you did not measure0.025 · input the vertical velocity
in m/s
throttle

but rather in cm/s. Just multiply the equation

2 by 100 cm/m to get the correct units:
Accelerationz [cm/s ] = 2.5 · inputthrottle
Accelerationz [cm/s2 ] = 2.5 · inputthrottle
2.5
VelocityVertical [s] = · inputthrottle [s]
207 s
2.5
VelocityVertical [s] = · inputthrottle [s]
Ca rbon aeronautics s
 0.250 [kg] · Accelerationz [m/s2 ] = 0.0063 · inputthrottle [0 : 1000]
Accelerationz [m/s2 ] = 0.025 · inputthrottle
Quadcopter dynamics simulation

Doing the same Laplace transformation2as before together with the integration of the
Accelerationz [cm/s ] = 2.5 · inputthrottle
acceleration results in the transfer function of the vertical velocity dynamics:

2.5
VelocityVertical [s] = · inputthrottle [s]
s

motor quadcopter system

system input
dynamics dynamics output
motor thrust
InputYaw and torque RateYaw
1 4.8
0.03 . s + 1 s
motor thrust
InputRoll/InputPitch and torque RateRoll/RatePitch
1 115
0.03 . s + 1 s
motor thrust
InputThrottle and torque VelocityVertical
1 2.5
0.03 . s + 1 s

Roll/Pitch/yaw rate [°/s]

Vertical velocity [cm/s]

yaw
3 response
roll/pitch
response velocity
2 response

1
input command
0
0 0.5 1 1.5 2
Time [s]

208
Open loop system response
You have now characterized the quadcopter dynamics for the roll, pitch, yaw and ver-
tical velocity movements. Combined with the motor dynamics, you can simulate the
response of the system to a command in the time domain (usually done with dedicat-
ed software). This is called the open loop system response and is visualised in the dia-
gram to the left. The quadcopter dynamics for all movements are summarized below:
115
RateRoll (s) = · InputRoll (s)
s
115
RatePitch (s) = · InputPitch (s)
s
4.8
RateYaw (s) = · InputYaw (s)
s
2.5
VelocityVertical (s) = · InputThrottle (s)
s

The value of the nominator is the only part that changes for each of the quadcopter
dynamics. When you simulate an input step response in the time domain and evaluate
the so called open loop system response, the system output gives an indication of the
inherent (in)stability for all four movements. The results are visualized in the figure
to the left. You can see that the roll and pitch response happens much faster than
the velocity response. This depends off course on the value of the nominator in the
quadcopter dynamics transfer function: a large nominator (115) causes a much faster
response than a small nominator (2.5). The quadcopter dynamics are an example of
a meta-stable system, as control theory requires the poles of the system to be in the
left half plane (e.g. negative). Since the pole of the system lies in this case exactly in
the origin (s=0), it means that the system is meta-stable; the system will continuously
increase at the same rate in an open loop. The pole of the motor dynamics is equal to
0.03s+1=0 or s=-33 and thus very stable, meaning it does not have an impact on the
open loop system as a whole.

Physically, the behaviour of the open loop system means that you can control the
vertical velocity of the quadcopter manually and you do not necessarily need a control
loop. The first rate controller you developed did not have vertical velocity control
yet you were able to control the altitude of the quadcopter pretty well by constantly
adjusting the throttle. It is a different story with the roll, pitch and yaw rate; trying
to manually control these rates is practically impossible for the reaction times of a
human being. That is why the rate mode flight controller is the minimal controller you
need to fly your quadcopter. The mathematical development of this controller that
automatically stabilizes your quadcopter will be explained in the next project.

209
Ca rbon aeronautics
 motor input(k) = Pterm (k) + Iterm (k) + Dterm (k)

Ts
motor
motor input(k) = input(k) = Pterm
P ·Error(k)+I term(k) + Iterm (k) + Dterm (k)
(k−1)+I·(Error(k)+Error(k−1))· +
2
motor input(k) = Pterm (k) + I (k) + D (k)

Project 23
term term Error(k)
term term− Error(k − 1)
term
D·
Ts
Ts
motor input(k) = P ·Error(k)+Iterm (k−1)+I·(Error(k)+Error(k−1))· +
Quadcopter PID controller
motor input(k) = P ·Error(k)+I term 1+s· Ts
2 Error(k) − Error(k − 1) +
s (k−1)+I·(Error(k)+Error(k−1))·
T2ss
z = e−s·Tterm = 2
1 −Ds ·· Ts
2 Error(k) −T Error(k
s − 1)
D· motor thrust
DesiredRateRoll ErrorRateRoll Input Roll Tssand torque
+ PId controller 1 115
Pterm (k) = P · Error(k)
- C(s) 1 + s · Ts
0.03 . s + 1 s
z = e−s·Ts = 2
1 +
−s·Tss = 1
− T
s · Tss
motor quadcopter
z = e−s·T 22
1 − s · TT2ss dynamics roll dynamics
Pterm (z) = P · Error(z) 2

Pterm (k) = P · Error(k)

2 . π . 10
Pterm
term (k) = P ·sError(k)
measured roll + 2 . π . 10 quadcopter roll
Pterm (s) = P · Error(s)
rotation rate rotation rate
Pterm (z) = sensor dynamics
P · Error(z)
Pterm
term (z) = P · Error(z)
The integral
Itermterm
(k) = Iterm (k − 1) + I · (Error(k) + Error(k − 1)) ·
Ts
Pterm (s) = P · Error(s) 2
A more difficult transformation is the transformation of the integral term; the bilin-
Pterm
term (s) = P · Error(s)
ear transformation is used together with the property of the z-transform Ts that a(k-1)
Iterm (z) = z −1 · Iterm (z) + I ·-1(Error(z) + z −1 · Error(z)) ·Ts
in the discrete
I domain is
(k) = I equivalent to z .a(z) in the z-domain:
(k − 1) + I · (Error(k) + Error(k − 1)) · 2
term term
T2ss
term (k) = Iterm
Iterm term (k − 1) + I · (Error(k) + Error(k − 1)) ·
Ts 2
(1 − z −1 ) · Iterm (z) = I · (1 + z −1 ) · Error(z)) ·
z-transform
Iterm (z) = z −1 · Iterm (z) + I · (Error(z) + z −1 · Error(z)) 2 · Ts
T2
Iterm
term (z) = z
−1 · I
−1
term (z) + I · (Error(z) + z
term
−1 · Error(z)) · ss
−1

Ts 2
(1 − z −1 ) · Iterm (z) = I · (1 + z −1 ) · Error(z)) · Ts
(1 − z ) · Iterm (z) = I · (1 + z ) · Error(z)) ·
−1 −1 2
T2ss
(1 − z −1
−1) · I
term (z) = I · (1 + z ) · Error(z)) ·
term
−1
−1

z−1 z+1 Ts 2
· Iterm (z) = I · · Error(z) ·
(1 − z −1
z ) · Iterm (z) = I · (1 + z z −1 ) · Error(z)) 2 · Ts
2
(1 − z −1
−1) · I (z) = I T z+
T· s(1 + z −1 1) · Error(z)) · Tss
s · z + 1 · Error(z)
−1
II term (z)
term
term = I · 2
term (z) = I · 2 · z − 1 · Error(z)
z−1 2z+ z− 1 1 Ts
· Iterm (z) = I · transform
bilinear · Error(z) ·
z− z 1 z + z 1  T2s
· Iterm (z) = I · 1+s· Ts
· Error(z)
 · s
z term
Ts
1+s·
z
T 2s
1−s· TT2s + 1
+ 1 2
IIterm (s) = = II ·· Ts ·· 
term (s)  1−s· T22s  · Error(s)
Error(s)
22
1+s· s  ·
1+s· TT22s − 1
1−s· s − 1
1−s· T22s
Ts 11 + + 11 −
+ ss ·· TT2ss
+ − ss ·· TT2ss
IIterm (s) =
term (s) = II ·· Ts ·· 1 + s · TT2ss 2 · Error(s)
+ ss ·· TT2ss · Error(s)
22 1+s· 2 2
− 11 +
− 2
210
Ts 2
2
IIterm (s) =
= II ·· s ··
term (s)
T
22 22 ·· ss ·· T2s
Ts
2
11
De sign your quadcopter controller
During the previous project, you have characterized the quadcopter dynamics.
The goal of this characterization is the design of a mathematical controller
that is capable of stabilizing your quadcopter. The PID controller that you
used for your flight code will be designed in this project.

Your goal is once again to construct a transfer function for the controller that you
want to use. Remember that a PID controller consists of three terms:
motor input(k) = Pterm
term (k) + Iterm
term (k) + Dterm
term (k)

which, in the discrete time domain of your flight controller program (where each k is
a new iteration that takes Ts=0.004 s or 250 Hz), translates to:
motor input(k) = Pterm (k) + Iterm (k) + Dterm (k) Tss
motor input(k) = P ·Error(k)+Itermterm (k−1)+I·(Error(k)+Error(k−1))· +
 2
Error(k) − Error(k − 1)
D·
motor input(k) = Pterm (k) + Iterm (k) + Dterm T(k) Ts
motor
= input(k) = Pterm (k) + Iterm (k) + Dterm (k)
ss
motor input(k) P ·Error(k)+I term (k−1)+I·(Error(k)+Error(k−1))· +
Now you want to transform this discrete representation  to the s-domain. 2 Such a
transformation is only possible by first transforming Error(k) − Error(k − 1)
1 +Ds · TT2ss the discrete time representation
= = 2
−s·T
−s·Tss
to the frequency domain using the z-transform. Ts
1 − s · T2Next the z-domain willTsbe trans-
z e T s
motor input(k) = P ·Error(k)+Iterm (k−1)+I·(Error(k)+Error(k−1))· Ts +
s
2
formedmotor
to theinput(k)
s-domain
= Pusing the bilinear
·Error(k)+I or Tustin transformation:
term (k−1)+I·(Error(k)+Error(k−1))· 2 +
 2
Ts Error(k) − Error(k − 1) 

1 + s · 2 Error(k) − Error(k − 1)
Pzterm
=e
term (k)
−s·T=s P · Error(k)
= D
D·
·
1 − s · T2s Ts
Ts
The proportional term
Pterm (k)
(z) = 1 Error(k)
+ s · TT2ss
s P · Error(z)
−s·Ts = 1 + s · T2s
z = e−s·T
z =one
The easiest transformation is the e of = the1 proportional
−s· term, as it requires only the
1 − s · T22s
transformation of the in- and outputs to the z and s domains:
(s) = P · Error(z)
Pterm (k)
(z) Error(s)
P = P · Error(k)
Pterm
term (k) = P · Error(k)
z-transform
Pterm Ts
term (k) = Iterm
Iterm term (kP− 1) (s)
+ I=· (Error(k)
P · Error(s)
term (z) = P · Error(z)
+ Error(k − 1)) · s
Pterm (z) = P · Error(z) 2
bilinear transformation
TTs
Iterm (k)==zI−1
Iterm(z) −1 · I(k
term P−
term 1) (s)
(z) (Error(k)
+ II=· (Error(z) + zError(k
P · Error(s) − 1)) · · ss
−1 · Error(z))
−1
term Pterm
term
term (s) = P · Error(s) 22

As expected, the result −1 is just a constant P multiplied with theTsserror.

TTs s Let's continue
IIterm (z)
(1 ==zz−1
(k)− I )··II(k
−1
term−(z)
term (z)
1) ++ (Error(z)
−1 )+
=III···(Error(k)
(1 + z −1 z −1 · Error(z))
+· Error(z))
Error(k −· 1)) · ·Ts2
Iterm
with the integral (k) = Iterm
termterm. term (k − 1) + I · (Error(k) + Error(k − 1))
term
2 · 2
2

Ts
(1
Iterm (z)
(1 − −1) · I
−1
= zzz−1 · I term (z)
(z)+=
term (z)
(1
(1 +
+ zz −1 ))+·· Error(z))
=III· ··(Error(z)
−1
·· Ts · T
−1 · Error(z))
= z −1)··IIterm Error(z))
211 − z −1 Ts
Iterm (z) term (z) + I · (Error(z) + z 22 · 2s
· Error(z))
2

−1 1 z++ 1zz−1
z− Ts T Ts
(1 −
(1 − zz−1 )) ···IIIterm (z)
(z) =
(z) = (1
= III ···(1 + )) ·· Error(z))
Error(z))
· Error(z)
−1
· s ·· Tss
 2  1+s· Ts
1+s· TT22s − 1
T2ss + 1
 1+s· 
Ts  1−s·
s
2 + 1
Ts
1−s· T
1+s·
Iterm (s) = I · T 2s + 1  · Error(s)
1+s· T T22s
T
Ts ·  1−s·
1−s·
s
+ 1
Iterm (s)
(s) = =TIIs ·· T12ss+·· s 1+s·
 ·1−s·
T
+
T2
2s 1 − 
 Error(s)
Ts
ItermIIterm 1s ··· Error(s)
s T2s s

(s) = (s)I = · I ·· 22 ·  1+s· 1+s· 2 T − Quadcopter Error(s)

2 PID controller
· Error(s)
T 2
term
2 12 + s ·1−s·
1−s· T2ss
− 1
1+s·
Ts T
1−s·
2 T−2s 1
T22s
2
s
−
− +11 s · T2s
T 1 + s 1−s· · Ts + 2 1 − s · T
T2s Ts
Iterm (s) = II · T Tss · 11 + + 11 −
+ ss ·· TTTT22sss + −2ss ·· TT22ss · Error(s)
s (s) =
IIIterm (s) = = II term
·· T22s ·· 11 + + Is · T222ss + + s · 2s · Error(s)
term (s) · 1−
+s ss· T··2sTTT22ss ·· Error(s)
−
term (s) = I · 2 · 1 + ss ·· TT2s − − 211 ·+ Error(s)
2 1 + s · T2 s − 1 +2s · 22s
2 s

Iterm (s) = · T · 1 2
Iterm I
= (s) =Tss I · 2 Ts
IIIterm
term (s)(s) = II ·· T22s ·· 22s·· ss2·· TT2ss
term (s) = I · 2 · 2 2
2 2 1·· ss ·· T22s
The derivative term Iterm (s) = I · 11
IIIterm (s) =
term (s)
Error(k)
= I ·· 1s
(s) = II − · sError(k − 1)
D
The transformation of the (s) = D term
termderivative term from s
· the discrete to the s-domain is very
sT
s
similar to the work you have already carried out−for
Error(k) the integral
Error(k − 1) term:
Dterm (s) = D · Error(k) Error(k) − Error(k Error(k − − 1)
Dterm
D (s) =
term (s)
=D D ·· Error(k) −
Error(z) −zT TError(k
s − 1)
Error(z) 1)
D (s) =
−1
D · − ·
Dterm
term (z) = D · T s
T s
s
z-transform T−1 s
Error(z)
1 z − − 1z−1 · Error(z)
Dterm
Dterm (z) = = Error(z) Error(z)
(z)(z) · Error(z)
D · Error(z)
· · − − zz−1 ·· Error(z)
DDterm (z) =D
= D ·D · Error(z)
Ts z− zT T−1s · Error(z)
Dterm
term (z) = D ·
1 z − 1T
s
s
Dterm (z) = D · 11 · zz − − 11Ts· Error(z)
Dterm (z)
(z) = =D D ·· T1s ·· z T− z · Error(z)
T 1+s· zs 1 · Error(z)
 
DDterm
term (z) = D · Tss · T2z − ·1Error(z)
1 Ttransform
bilinear s 1−s· z
s
Dterm (s) = D · ·   2 Ts   · Error(s)
Ts  1+s· 1+s·Ts 
T2ss − 1
2
 1+s· T2s Ts 
1  1+s· 1−s·
1−s· T
2− 1
Dterm (s) = D · 11 · 1−s·  T22ss T−1 · Error(s)
T22s
1+s·
1−s·
+1 s ·· Error(s)
T
Dterm
D (s) =
(s) =D D1 ·· TT1s+·· s ·1−s· s T2s −
T1+s·
21+s· − 12ss  Error(s)
Ts
DtermDterm
(s) =
term (s)D=· D ·· Ts · s  2 T
1−s· T
1+s·
T2ss  · Error(s)
2
· Error(s)
Ts Ts 1+ 1+s·
1−s·
1−s·
T22s T
s T·22ss 2s
T

1 1 + s · T2s − 21Ts+ s · T2s

T1−s· 2s
s T Ts
Dterm (s) = D · 11 · 11 + +
1 s · 2Tss· s− −· 112T+ + ss ·· TT2s · Error(s)
D
D term (s)
D
(s) =
=
term (s)
D ·
· =
T1sD · 1· + ss· ··1T22+
· − s ·1T2+ s
·sError(s) Error(s)
· 22s ·· Error(s)
D
Dterm (s) = D · Tss ·
term T T 1 +
2
s 1 + s · T2s s · T s
· Error(s)
Ts 1 +
1 2 · s · T2 s · T2ss
Dterm
Dterm (s)(s)
== DD · 11· · 22s·· ss ·· TT22Tsss·sError(s)
· Error(s)
D
D (s) = D · 1
(s) = D · T s1·· + 12s+ · ssT··s 2 · Error(s)
+·ss 2·· 2TTT22ss ·· Error(s)
term T
Dterm
term (s) = D · Tss · 1 1+ Error(s)
T 1+ s 2s
ss sT·s 2· Error(s)
Dterm (s) = D ·s
Dterm (s)
D (s) = D D ·· 1 + s · T2s ·· Error(s) Error(s)
term (s) = D· · 11to+ + ss ·· TT2s · Error(s)
When taking into account DD that
term
term (s)
Ts=is=D
equal 0.004 · Error(s)
s seconds, the transport function for
1 1++s s· 0.002
· 2 2
the derivative term becomes: s
Dterm (s) = D · s · Error(s)
D
D (s) =
term (s) = D · 1 + s s
D · s· 0.002 ·· Error(s) Error(s)
Dterm
term (s) = D · 1 +
11 + s
s ·
· 0.002
0.002 · Error(s)
+ s · 0.002
Closed control loop response
During the previous projects you learned how to describe the quadcopter, motor
and sensor dynamics in a mathematical way, subsequently transforming them to the
frequency domain. Now that you can describe the PID controller as well, you are able
to construct the full control loop. The figure on the right shows the full quadcopter
rate control loop, with the P, I and D values that you used for your rate mode flight
controller. For the pitch and yaw rate loop, the figure would be exactly the same, only
the PID values and the quadcopter dynamics transfer function would change. You
can simulate the time-domain response of the full closed control loop with dedicated
software.
212
Let’s assume your desired roll rate changes from 0 to 30°/s and then back to 0 in
respectively one second. The resulting quadcopter roll rate for the closed control
loop is displayed on the figure below. At each step change, the quadcopter roll rate
overshoots the desired roll rate with almost 20°/s, but within half a second it is stable
again. When looking at the commands sent to the motor, they follow the desired roll
rate also closely. Notice that the commands saturate at ±400 µs; this was already pro-
grammed in the flight controller and can additionally be simulated as well.
RateRoll[°/s]

40 Quadcopter roll rate

20
DesiredRateRoll

-20
0 1 2 3 Time [s]
InputRoll[µs]

400

200

-200

-400
0 1 2 3 Time [s]

With the mathematical representation of the full control system, it is possible to op-
timize the response of the system by adjusting the PID values. In reality, the system
is first simulated using the mathematical representation, then the optimal PID values
are chosen given a desired system response. Because the mathematical representation
is an approximation of the real physical system, further tuning of the PID values will
always be necessary when testing your quadcopter.

PRateRoll= system g(s)

system input U(s) 0.6
motor thrust
ErrorRateRoll
+ InputRoll and torque
+ 1 IRateRoll= 1 115
+
- s 3.5 + 0.03 . s + 1 s
Desired RateRoll

s motor quadcopter
dRateRoll=
dynamics dynamics
1+0.002 . s 0.03
PId controller C(s) sensor H(s)
quadcopter
RateRoll 2 . π . 10 roll rate
s + 2 . π . 10 system output y(s)
sensor dynamics
213
Ca rbon aeronautics
 Project 24
Estimate the PID values

system input U(s) system g(s)

motor thrust
DesiredRateYaw ErrorRateYaw InputYaw
+ PId controller 1 and torque 4.8
- C(s) 0.03 . s + 1 s
motor quadcopter
dynamics roll dynamics
sensor H(s)
RateYaw 2 . π . 10 quadcopter yaw rotation rate
s + 2 . π . 10 system output y(s)
sensor dynamics

s-plane (s= σ+i.ω)

imaginary axis (i.ω)

left (stable) right (unstable)

half plane half plane

-63 -33 0 real axis (σ)

system poles

214
 Tu ne your controller for a smooth flight
You are now capable to mathematically simulate the full system and control
loop. The goal of this simulation is to 'tune' your PID controller in order to
get a stable flight. In essence, you need to determine the best values for the P,
I and D parameters.

Once you have the mathematical representation for the system and the sensor, a first
estimation of the PID values for the controller is usually calculated using dedicated
software. However, the optimal values can also be calculated by hand using the root
locus method. In this project you will derive an estimation of the PID values for the
yaw rate controller. A similar approach can be followed for the other controllers.

The closed loop transfer function of the full yaw rate controller can be mathematical-
ly constructed using the figure on the left: the output of the system Y(s) is related to
the system input U(s) through:

Y (s)
Y (s) = = C(s)
C(s) ·· G(s) G(s) ·· [U [U (s)
(s) − H(s) ·· Y
− H(s) Y (s)]
(s)]
Y (s) = C(s) · G(s) · [U (s) − H(s) · Y (s)]
Rewriting the equation such that the system output Y(s) is isolated gives:
Y (s) = C(s) · G(s) · [U (s) − H(s) · Y (s)]
G(s)
Y·(s) = · U (s)
Y (s) = C(s) · G(s) · [U (s) − H(s)Y Y (s)] G(s)
(s) = C(s)C(s) ·· 1 + C(s)G(s) H(s) · U (s)
YY(s) 1 + ·· G(s)
G(s) ·· H(s)
(s)==C(s) (s)
C(s)
C(s)· · G(s) · [U (s) − H(s) · Y· U (s)]
Y (s) = C(s) · G(s) · [U (s) − H(s) G(s)· Y (s)] 1 + C(s) · G(s) · H(s)
Y (s) = C(s) · · U (s)
The closed-loop
1 + C(s) poles
· G(s) s* of this system
· H(s) are∗ the zeros of the denominator:
Y (s) = C(s) ·
G(s)
· U (s) 11 +
+ C(sC(s∗ )) ·· G(s
∗
G(s∗ )) ·· H(s H(s∗ )) =
∗
= 00
1 + C(s) · G(s) · H(s)
Y (s) =1C(s) + C(s ) · G(s ) · H(s =) −1= 0 · U (s)
∗ ∗G(s) ∗
))· ·· G(s )) ·· H(s )) =
∗ ∗ ∗
G(s) C(s
C(s ∗ G(s ∗ H(s ∗ −1
Y (s) = C(s) · 1 + C(s∗ ) · G(s∗ ) · H(s·∗U) (s) = 0 ∗ 1 + C(s) · G(s) · H(s)
1 + C(s) · G(s) · H(s) C(s ) · G(s ∗
) · H(s ∗
) = −1
1 +G(s)
C(s∗∗)and
· G(sH(s)∗∗
) · H(s
are )determined
∗∗
= −10 by the physical system and· 10 cannot be chosen by you
11 ∗ 4.8
4.8 ∗·) =220·· π π · 10
) ) ) = 1design
+= =
C(s∗remains ) · G(s ) ·fixed
··H(s
1 + C(s ) · G(s ) · H(sthe
∗(considering
) =quadcopter ·· H(s) · that is). But you have full control
C(s ∗
· G(s ∗
· H(s ∗
−1G(s) H(s)
∗ ∗
0 G(s) 0.03 ·1 s 4.8
0.03 · ∗s + 1 · ∗s · s + 2 · π ·· 10
+ 1 s s 2
+ 2
· π· ·
π 10 10
over∗ the controller G(s)
C(s) to H(s) C(s=∗ )0.03
· stabilize ·the
G(s·system;)+· H(s
1 ) = s−1
strictly + speaking
2 10 it does not even have
C(s· )H(s)
∗ 1
· G(s=) · H(s ) = −1
∗ 4.8 2 · π · 10 s s · π ·
G(s) to be 0.03
a PID controller.
+ 1
· · Let’s first look at the characteristics of the physical system:
+ 2 10
1 · s 4.8 s 2 · π · 10
s · π ·
ss =
G(s) · H(s) = · · = 00
0.03 · s + 1 s s + 2 · π · 10 1s = 0 4.8 2 · π · 10
1 4.8 · 10 · H(s) =
2 · π G(s) · ·
G(s) · H(s) = · s =·0 0.03 · s + 1 s s + 2 · π · 10
0.03 · s + 1 s s + 2 · π · 10 −1
The system s = 0contains three poles: ss = = −1 = = −33 −33
0.03
s = 0.03
−1
s = =
0 −33
s =s 0= −1 = −33 0.03
−10.03 ss =
s= = −33 = −2 −2 ·· π π ·· 1010 = = −63
−63
0.03 s =s −2 −1
· π · 10 = −63
−1 = = −33
s = s = −2 = −33· π · 10 = −63 0.03
0.03
s = −2 · π · 10 = −63 =
C(s) = K · (s + b)
C(s) K · (s + b) with
with b b near
near zero
zero
C(s) =sK=· −2 (s + b) with
· π · 10 = −63 b near zero
= −2
sC(s) 215
=· K π ··10
(s =
+ b) −63 with b near zero
ss ++ bb
C(s) = K · (s + b) with b near zero C(s) =
C(s) =K K ·· s + sb
C(s) = KC(s) · (s + =b) Kwith· s b near zero
C(s) = K · (s + b) with b near s +zero b s
 s=0
s = 0G(s)
Y (s) = C(s) · s = 0· G(s) · H(s) · U (s)
1 + C(s)
−1 Estimate the PID values
s= = −33
0.03
−1
s= = −33 ∗
The pole at zero is the only one 1 +that
C(s∗makes −1 ∗the
) ·0.03
G(s system
) · H(s ) = meta-stable,
0 as the other poles
s= = −33
are situated in the stable left half-plane) ·−2 0.03
far from
G(s· π) ·· 10 ∗ the imaginary axis. To stabilize the
= )−63= −1
∗ ∗
C(ss= H(s
system, you will have to add a zero near the origin to cancel out the pole. This means
s = −2 · π · 10 = −63
that you need a controller of thestype: = −2 · π · 10 = −63
1 4.8 2 · π · 10
· H(s)
G(s) C(s) == K · (s + b) with · b near· zero
0.03 · s + 1 s s + 2 · π · 10
C(s) = K · (s + b) with b near zero
To make sure you do not C(s) have=a K steady-state
· (s + b) with error
b nearin thezeroresponse, you put another
pole at zero to finally obtain a controllersof the +PI type:
C(s) = = K 0·
s b
s+sb
C(s) = K ·
s+sb
C(s) = K ·
With b and K the parameters that you 1 will
−1calculate
4.8s using 2 · π · 10the root locus
s + b method. The
C(s) · G(s) · H(s) = s = 0.03 = · −33 · ·K ·
full open loop system now becomes: 0.03 ·1s + 1 4.8 s s+ 2 ·2π· ·π10· 10 s+sb
C(s) · G(s) · H(s) = · · ·K ·
0.03 ·1s + 1 4.8 s s+ 2 ·2π· ·π10· 10 s+sb
C(s) · G(s) · H(s) =  · ·  ·K ·
0.03 · s + 1
= −2 · π · 10−ln s= −63 s +
10 2 · π · 10 s
−ln s100
OS
 100 
This is a fourth orderξ =system,

−ln
because
OS


2the= highest
−ln
power
10
  of = 0.59
s in the denominator is
100OS  2
100 10
π + lnOS +
2   2 
s when you multiplyξ all
4 = fractions.
−ln 100  However,
100 2 =  πas b will
ln
−ln 100 10
 be
100 2 = 0.59
close to zero, it will help to
ξ= 2 + ln OS =b)with 2 + ln 10 = 0.59
cancel out the two poles atC(s)
πzero.= This
K 100 means
· (s 2+ πthat b the
near
 system
100zero
 2
will behave as a second
π 2 + lnof100OS 2 + ln 10
order system. The step response a generalπ second order system can be character-
100

ized by two parameters: the maximal overshoot and the settling time, which is defined
as the time at which the response goes s+b
C(s)to=within
K · 2% of the desired value. Let’s say that
you want the system to settle within 0.5 secondss with a maximal overshoot of 10%:
• overshoot (OS)=10%
1 4.8 2 · π · 10 s+b
• settling time G(s))=0.5
C(s)(t· settling · H(s)s = · · ·K ·
0.03 · s + 1 s s + 2 · π · 10 s
The damping ratio ζ for a second

order

system is defined
 
by:
OS 10
−ln 100
−ln 100
ξ=  2 =   2 = 0.59
10
π 2 + ln OS
100
π 2 + ln 100

You already defined the settling time as =the−ln(0.02)

tsettling time necessary to reach 2% (=0.02) of the
−ln(0.02)
ξ · ωn ratio through the formula:
desired response. This is related to the =damping
tsettling
ξ · ωn
−ln(0.02)
tsettling =
−ln(0.02)
ωn = ξ= · ω13
−ln(0.02)n rad/s
t 0.59 0.5 s = 13 rad/s
·=
−ln(0.02)
Where ωn is the natural frequency
ωn = of the second-order
settling
0.59 · 0.5 s ξ · ω n
system. Calculate the natural
frequency by inverting the above formula:
−ln(0.02)
ωn = = 13 rad/s
pdesired = 0.59
−ωn· ·0.5
−ln(0.02)ξ ±s i · ωn ·  1 − ξ 2
ωn == −ωn · ξ ± i=
pdesired · ω13
n · rad/s
1 − ξ2
0.59 · 0.5 s

The desired poles of your pfull system are then equal
desired = −ωn · ξ ± i · ωn ·
 to:2
1 − ξ2
pdesired = −ωn · ξ ± i · ωn ·  1 − ξ
pdesired = −ωn · ξ ± i · ωn · 1 − ξ 2

pdesired = −ωn · ξ ± i · ωn · 1 − ξ 2
pdesired = −7.8 ± i · 10.7 
pdesiredpdesired
= −ω= · 1 − ξ2
ξ ± i±· ωi n· 10.7
n ·−7.8

pdesired =−7.8 ± i · 10.7

 θpoles −  θzeros = 180◦ 216
pdesired
θpoles −= −7.8 ±i=
θzeros · 10.7
180◦
 
θpoles − θ = 180◦
2 + θ 3 + θ4 
θ + θ5zeros
− θ1 = 180◦
The b and K parameters will be determined using the angle and magnitude conditions
of the root locus method.
Response −ln(0.02)
tsettling =
ξ · ωn
−ln(0.02)
maximal overshoot tsettling = −ln(0.02)
tsettling = ξ · ωn
ξ · ωn
−ln(0.02)
ωn = = 13 rad/s
0.59 · 0.5 s
−ln(0.02)
ωn = −ln(0.02) = 13 rad/s
ωn = 0.59 · 0.5 s = 13 rad/s
0.59 · 0.5 s 
pdesired = −ωn · ξ ± i · ωn · 1 − ξ 2


pdesired = −ωn · ξ ± i · ωn · 1 − ξ 22
pdesired = −ωn · ξ ± i · ωn · 1−ξ

pdesired = −ωn ·settling
ξ ± i · ωtime
n· 1 − ξ2 Time
Angle condition p = −ωn · ξ ± i · ωn ·


1− ξ2
pdesired
desired = −ωn · ξ ± i · ωn · 1− ξ2
pdesired
The angle condition of the root locus = −7.8 i · 10.7
says ±that the sum of the angles of the open
loop poles minus the sum of the angles of the open
pdesired = −7.8 ± i · 10.7 loop zeros has to be equal to
pdesired = −7.8 ± i · 10.7
180°:  
θpoles − θzeros = 180
◦

 
four= 180◦ and you want to add a zero at
θ − θ
Remember that your system contained
poles − the =poles
◦
θpoles θzeros
zeros 180
b. The angle condition becomes:
θ2 + θ3 + θ4 + θ5 − θ1 = 180◦
θ2 + θ3 + θ4 + θ5 − θ1 = 180◦◦
θ2 + θ3 + θ4 + θ5 − θ1 = 180
Using basic trigonometry,
tan(180angles
the ◦
− θ2 ) = can10.7
be→ determined
θ2 = θ5 = 126 (see
◦ figure below):
7.8
10.7
tan(180◦ − θ2 ) = 10.7
◦ 10.7 → θ2 = θ5 = 126◦◦
tan(θ−3 )θ=2 ) = 7.8 → θ2 = θ5 =◦ 126
7.87.8 → θ3 = 23
tan(180
3310.7
−
tan(θ3 ) = 10.7 10.7 → θθ33 = 23◦◦◦
tan(θ43 )) =
tan(θ = 3333 − 7.8 → =
= 23
11
− 7.8
− → θ 4
6310.77.8
tan(θ4 ) = 10.7 → θ = 11◦
tan(θ4 ) = 63 − 7.8 → θ44 = 11◦
63 − 7.8
θ1 = 126◦ + 23◦ + 11◦ + 126◦ − 180◦ = 106◦
s-plane (s= σ+i.ω) θθ11 =
= 126◦ + 23◦ + 11◦ +imaginary axis
126◦ + 23◦ + 11◦ + 126◦◦ − 180◦◦ =
126 − 180 =
106(i.ω)
◦
106◦
10.7
tan(180◦ − θ1 ) =
left (stable) 7.8 −b
10.7
right (unstable)
half plane tan(180
desired
◦
pole = 10.7
◦ − θ1 ) 10.7
◦− θ1 ) = 7.8 − b half plane
tan(180 − 106 ) =
tan(180
◦
7.8 →bb = 4.7
−
7.8 −
10.7 b
tan(180◦◦ − 106◦◦ ) = 10.7 → b = 4.7
tan(180 − 106 ) = 7.8 − b → b = 10.7 4.7
L4 7.8 − b L =L
2 5
L3 L1 θ1 θ =θ
θ4 θ3 2 5

-63 -33 -7.8 b 0 real axis (σ)

system poles

217
Ca rbon aeronautics
 7.8
10.7
tan(θ3 ) = 10.7 → θ3 = 23◦
tan(θ3 ) = 33 − 7.8 → θ3 = 23◦
3310.7
− 7.8
tan(θ4 ) = 10.7 → θ4 = 11◦ Estimate the PID values
tan(θ4 ) = 63 − 7.8 → θ4 = 11◦
63 − 7.8
This gives for θ1:
θ1 = 126◦ + 23◦ + 11◦ + 126◦ − 180◦ = 106◦
θ1 = 126◦ + 23◦ + 11◦ + 126◦ − 180◦ = 106◦
From which you can calculate the value for parameter b, again using trigonometry and
the figure on the previous page: 10.7
◦
tan(180 − θ1 ) = 10.7
tan(180◦ − θ1 ) = 7.8 − b
7.8 − b
10.7
tan(180◦ − 106◦ ) = 10.7 → b = 4.7
tan(180 − 106 ) =
◦ ◦ 7.8 − b → b = 4.7
7.8 − b

Your PID controller becomes equal to C(s)=K(s-4.7)/s.

Magnitude condition
You will now determine the final parameter K using the magnitude condition of the
root locus:

Lpoles
Kglobal = 
Lzeros

 Lpoles
1 =

In which Kglobal comprises notK only
Kglobal =the
L parameter
poles 2 · π · 10K, but sthe
Lpoles
 4.8

+ b full amplification
global = 
global  L poles
C(s) · G(s) · H(s) = K
K global =
global = 
·L poles
zeros
·
poles ·K ·
throughout the open loop system:
0.03 · s + 1L
Kglobal szeros
zeross + 2 · π · 10
zeros
Lzeros
L
s
zeros
1
1 4.8 4.8· 2 2· π· π· 10 · 10 · K · s s++b b
= = s +11 1 · · s4.8
0.03
C(s) · G(s) · H(s)
4.8 · + 222 ··· π ··· 10
10 · K · sss + b
10 ·· s +
C(s) · G(s) · H(s)
= 0.03 ·11s0.03 4.8
+ 1 ·· 4.8 s ·· s + 22 ··22ππ·· ·π 10 +
C(s) ·· G(s) ·· H(s) s ππ · 10 ·· K s bbb
C(s) G(s) H(s) =
= 0.03 · s + 1 s s +
π· 10
· 10 K s+s
C(s) · G(s) · H(s) = 0.03
C(s) · G(s) · H(s)
0.03 ···11sss + + · ss · ss +
+ 111 4.8
· · 2·π · 10 · K ·
0.03 2+ · π22···10 ππ ·· 1010 · K s· + sssb
C(s) · G(s) · H(s) = 0.03 11
1 1 1·· 4.8 ··
4.8s s22 + ·· π ·· 10
10 · K · ss + b
= 0.03
4.8 ·· s +
22 ··222ππ··· ·π 10 +
π s bbb
s+ ·· ss· + · 10 ·· K
C(s) · G(s) · H(s) 0.03
C(s) ·· G(s) ·· H(s) =
Kglobal= = +
0.03
1 1
0.03
K 0.0311 ·· 4.8
·0.03 s· 4.8
+
· 10
10 K s+
C(s) G(s) H(s)
C(s) · G(s) · H(s) = s + 0.03 s +
0.03 s + π · 10 · K · s
10.03
s 1 ·
0.03
s s
· + 2 · π · 10 · K · s π · s
s + 0.03 0.03 ss ss + 2 · π · 10 =Kglobal s
Meaning that: 1
Kglobal = K · 11L ·· L 4.8··L2 · ·πL· 10
=K 4.83 · 24· π ·510
11 2 ·· 4.8
K ·· 0.03
K 10053=
global
global
= ·K =0.03 4.8L··· 222 ··· π π ··· 10
π 10
10
K K ·
Kglobal = K · 0.03 · 4.8 0.03
global
global
1
0.03
The magnitude condition of the root Llocus · L is · Lthen · L5 equal to:
√  10053 · K = L22 · L33 · L444 ·· L 5 √
7.8 + 10.7 · (33 10053
− 7.8) =+
L 22 ··2L 33L·· L 4 ·L L5557.8)2 + 10.72 · 7.82 + 10.72
=10.7
L2 · L·3L·11(63
2 2
10053 · K 2
= L 2 L 3 L 4
10053 ·K L4 · − L 5
K= 10053 ·· K
K= L
L11 21


10053 · (7.8 − L 4.7)
1 + 10.72
√ √
√
√ 7.822 + 10.722 ·  
(33 − 7.8) 2 + 10.72 · (63 − 7.8)2 + 10.72 · √7.82 + 10.72

22 + 10.722 · (63 − 7.8)22 + 10.722 · √
√ 7.8 2 + 10.7 2 ·  (33
K = √ 7.822 + 10.722 ·  (33 − 7.8)22 +
− 7.8) 10.7 2 ·  (63 − 7.8)2 + 10.72 · √ √7.8
7.8
22 + 10.722
2 + 10.72
K=
K 7.82 +
= 7.8 + 10.7
10.72 ·· (33 (33 − 7.8)2 +
−10053
7.8) ·+ 10.7
10.7
 (7.8
2·
· +4.7)
2s− (632 −
4.7
(63 −+7.8) 7.8)
10.7222+
2 + 10.72 ·
10.72 · 7.8 7.822 +
+ 10.7
10.722
K
K= = 10053
C(s) =
10053 ·· (7.8 2.4

(7.8 −· − 4.7) 4.7)
2
22 + + 10.7
10.7 22
10053 ·· (7.8 (7.8 − − 4.7)s4.7)22 + 10.722
This gives K=2.4 and now you10053 finally know 11.3
+ 10.7
the parameters of your PI controller:
C(s) = 2.4 + s + 4.7
C(s) = 2.4 · ss + + s4.7
4.7
C(s) =
= 2.4
2.4 · s +s 4.7
C(s)
C(s) = = 2.4 2.4 ··· s +ss4.7
C(s) 11.3
ss
C(s) = 2.4 + 11.3 11.3
C(s) =
= 2.4
2.4 +
+ 11.3
C(s)
C(s) = = 2.42.4 + + ss 11.3s
C(s)
ss
The PI values for the yaw rate you used in your flight controller are equal to 2 and 12,
which is very close to your calculated estimation of 2.4 and 11.2. In practice, you will
choose 2.4 and 11.2 as initial values when testing your flight controller then slightly
tune the parameters to further improve the handling of your quadcopter.
218
219
Ca rbon aeronautics
Part V: expanding your
horizon
congratulations, you reached the final part
of this book! By now, you have learned so
much about quadcopters that you are ready
to further develop, tweak and modify it
yourself!

what about adding a GPS system to devel-

op a return to home function, an ultrasonic
sensor enabling you to add obstacle avoid-
ance, a telemetry system or a camera to get
into the exciting world of FPV racing?

now that you know the basics, nothing is

beyond your reach. Have fun developing
your own projects!
This manual helps you to develop, program
and construct your own quadcopter with
the help of 24 small projects, explaining
the essentials on aeronautics, electronics
and embedded programming along the
way.

All components and code used in this man-

ual are fully hackable and adaptable, giving
you the opportunity to create your own
unique quadcopter.

Ca rbon aeronautics