Thesis CDU 41657 Hanna W

Modelling and Control of an
Unmanned Aerial Vehicle

A Thesis submitted in partial fulfilment of the requirements
for
the degree of Bachelor of Engineering
in
Electrical and Electronics Engineering
By
William Hanna
Student No. S210566
Supervisor:
Damien Hill
Friso De Boer
Thesis coordinator:
Kamal Debnath
School of Engineering & Information Technology
Faculty of Engineering, Health, Science and the Environment
Charles Darwin University
Darwin
May 2014
ABSTRACT
Keywords: Quadcopter, Quadrotor, Multi-rotor, Unmanned Aerial Vehicle (UAV), Control
Systems, Mathematical Modelling, Crazyflie
Technological advances in power electronics such as miniature electro-mechanical devices
(MEMS) and small, powerful electric motors have enabled the development of small and
lightweight devices. These devices include flying devices such as unmanned aerial vehicles
(UAVs). One such UAV is the open source quadcopter, the Crazyflie.
The technological advances have made it possible to produce lightweight, low power yet accurate
sensors which can be used with controllers that have a high processing power yet low power
consumption. This has allowed for the development of complex control systems that can be
implemented onboard the UAV. With this combination of high accuracy and light weight, realtime onboard guidance and autonomous flight are now practical.
The Quadcopter is an inherently unstable system, having similar kinematics to an inverted
pendulum. By creating an accurate mathematical model of a Quadcopter, control systems are
implemented and tested without the possibility of damage. Once a theoretically stable control
system is designed it can then be implemented in the firmware of the Quadcopter. By maximizing
the accuracy of the mathematical model the resultant motion of the Quadcopter can be predicted.
This prediction can then be used to increase the accuracy, stability and time response of the
overall system.
Experiments were conducted in order to obtain the physical characteristics of the Quadcopter.
These parameters were then used to create a MATLAB model of the Quadcopter. This model was
then used to design and implement various control systems. These control systems where then
implemented in the Quadcopter and the performance compared to that of the mathematical
model.
The mathematical model was found to be capable of accurately predicting the behaviour of the
Crazy Flie under conditions involving low external influences such at wind.
Modelling and control of an unmanned aerial vehicle

ii
Table of Contents
ABSTRACT .................................................................................................................................... ii
List of symbols and abbreviations ................................................................................................... v
List of symbols: ........................................................................................................................... v
Abbreviations: ............................................................................................................................ vi
List of tables ................................................................................................................................... vi
List of figures.................................................................................................................................. vi
List of photographs ........................................................................................................................ vii
1.
Introduction ............................................................................................................................. 1
1.1 Basics ..................................................................................................................................... 1
1.2 History of Quadcopters .......................................................................................................... 2
1.3 Existing Work ........................................................................................................................ 2
1.4 Problem Formulation and Outline of Thesis ......................................................................... 3
2 Literature Review ......................................................................................................................... 4

2.1 Control and Movement of a Quadcopter ............................................................................... 4
2.1.1 Dynamics of the Propeller .............................................................................................. 4
2.1.2 DC motor drivers ............................................................................................................ 5
2.1.3 Motion and Sensors ........................................................................................................ 7
2.1.4 Inertial Measurement Unit ............................................................................................ 11
2.1.5 Microprocessors and Microcontrollers ......................................................................... 12
2.2 Equations of motion............................................................................................................. 13
2.2.1 Frames of reference ...................................................................................................... 13
2.2.2 Translational motion ..................................................................................................... 14
2.2.3 Rotational motion ......................................................................................................... 16
2.2.4 Friction and air-resistance ............................................................................................ 19
2.2.5 Battery Voltage Effects ................................................................................................ 20
3 Experimental determination of physical attributes ..................................................................... 21
3.1 Moments of Inertia .............................................................................................................. 21
3.2 Propeller Lift Constant ........................................................................................................ 22
3.3 Propeller Drag Constant ...................................................................................................... 24
3.4 Battery Discharge Rate ........................................................................................................ 26
3.5 Air Resistance .................................................................................................................. 29
4 Mathematical model development and implementation ............................................................. 34
4.1 Methodology of development .............................................................................................. 34
iii
4.2 MATLAB Implementation .................................................................................................. 35

4.3 Comparison of MATLAB model to experimental results ................................................... 35
5. Controller designs ...................................................................................................................... 35
5.1 Crazy Flie Control System .................................................................................................. 35
5.1.1 Properties of the Existing System ................................................................................. 35
5.2 Comparison of Altitude Control Methods ........................................................................... 38
5.2.1 Barometer Altitude Control .......................................................................................... 38
5.2.2 Accelerometer Altitude Control ................................................................................... 40
5.3 Initial Performance of Controllers ....................................................................................... 41
5.3.1 Barometric pressure control.......................................................................................... 41
5.3.2 Accelerometer control .................................................................................................. 42
6 Results of control system implementation in Quadcopter .......................................................... 44
6.1 Results of implementation ................................................................................................... 44
6.1.1 Altitude Control using MEMS Barometer.................................................................... 44
6.1.2 Altitude Control using MEMS accelerometer .............................................................. 44
7 Conclusions and Recommendations for Future Work ................................................................ 46
7.1 Conclusions ......................................................................................................................... 46
7.2 Recommendations for Future Work .................................................................................... 46
References ........................................................................................................................................ i
Appendices ....................................................................................................................................... i
Appendix A Experimental Data ................................................................................................ i
Appendix B MATLAB Program and Functions........................................................................ i
Appendix C Crazy Flie Construction ........................................................................................ i
Appendix D Crazy Flie Firmware modifications ...................................................................... i
Appendix E Crazy Flie Client modifications ............................................................................ i

iv
List of symbols and abbreviations

List of symbols:
fi
Thrust force produced by propeller i
Propeller lift constant
Torque produced by propeller i drag
Propeller drag constant
ai
Acceleration in the i direction
vi
Velocity in the i direction
si
Displacement in the i direction
IM
Moment of Inertia of propeller
Iii
Moment of Inertia of body about i axis
Angular velocity of propeller i
Angular acceleration of propeller i
PWM duty cycle (as a fraction)
PWM
PWM duty cycle (as a percentage)
max
Maximum motor speed for given supply voltage
roll , pitch , yaw
Angular accelerations of body
roll , pitch , yaw
Angular velocities of body
Froll , Fpitch , Fyaw
Resultant forces due to force imbalances
Ftot
Resultant force acting on body
VB
Battery voltage
VNL
No-load battery voltage
First order air resistance coefficient for translational motion along i axis
Second order air resistance coefficient for translational motion along i axis
First order air resistance coefficient for rotational motion around i axis

v
Abbreviations:
ASL
Altitude Above Sea-level
DC
Direct Current
IMU
Inertial Measurement Unit
LiPo
Lithium Polymer
MEMS
Micro-Electro-Mechanical Systems
MoI
Moment of Inertia
PID
Proportional Integral Derivative
PWM
Pulse Width Modulation
RPY
Roll Pitch and Yaw
UAV
Unmanned Aerial Vehicle
List of tables
Table 1: Rotational Motion............................................................................................................ 16
Table 2: Moment of Inertia of Crazy Flie...................................................................................... 22
Table 3: MATLAB Functions ....................................................................................................... 34
Table 4: MATLAB Sub functions ................................................................................................. 34
List of figures
Figure 1:Forces on a Propeller......................................................................................................... 4
Figure 2: MEMS Gyroscope............................................................................................................ 8
Figure 3: typical MEMS accelerometer construction ...................................................................... 9
Figure 4: MEMS Barometer .......................................................................................................... 10
Figure 5: MEMS Magnetometer.................................................................................................... 11
Figure 6 : Frames of reference ....................................................................................................... 13
Figure 7: Rotational reference angles ............................................................................................ 14
Figure 8:Trifilar Pendulum ............................................................................................................ 21
Figure 9: Crazyflie Propeller Lift Constant ................................................................................... 23
Figure 10: Crazyflie Propeller Drag Coefficient ........................................................................... 26
Figure 11: Battery voltage vs thrust model ................................................................................... 27
Figure 12: Battery Compensated Thrust ........................................................................................ 28
Figure 13: Crazyflie in Freefall data.............................................................................................. 30
Figure 14: Crazyflie in Freefall polynomial fitting ....................................................................... 31
Figure 15: Crazyflie in Freefall air resistance modelling .............................................................. 32
Figure 16: Pendulum degrading .................................................................................................... 33
vi
Figure 17: Original PC Client interface ......................................................................................... 36

Figure 18: Modified PC Client interface ....................................................................................... 37
Figure 19: Thrust effect on ASL reading....................................................................................... 39
Figure 20: Displacement and Velocity Drift over time ................................................................. 41
Figure 21: Barometer Drift over time ............................................................................................ 42
Figure 22: Displacement and Velocity drift over time .................................................................. 43
List of photographs
Photograph 1: Heron I ..................................................................................................................... 1
Photograph 2: Camcopter ................................................................................................................ 1
Photograph 3: ARDrone .................................................................................................................. 1
Photograph 4:De Bothezat Quadrotor built in 1923 ........................................................................ 2
Photograph 5:Disassembled Motor ................................................................................................. 6
Photograph 6:Crazy Flie Motor ....................................................................................................... 6
Photograph 7:Moment of Inertia about z-axis ............................................................................... 22
Photograph 8: Moment of Inertia about x-axis .............................................................................. 22
Photograph 9: Crazyflie yaw rig .................................................................................................... 25

vii
1. Introduction
1.1 Basics
An unmanned aerial vehicle (UAV) is an aircraft that does not carry a human operator. It is
normally a powered aircraft that relies on aerodynamic forces to provide motion. This motion is
controlled either by onboard computer (autonomous) or by remote control. Accurate methods of
detecting and reacting to the UAVs environment are being developed; making some modern
UAVs are virtually crash-proof (Merz & Kendoul 2013).
As UAVs are becoming more advanced and more practical, with longer flight times and larger
payloads, more tasks are being found that they can perform. From pleasure activities to advanced
military versions, UAVs are finding a bigger role in the modern world (Merz & Kendoul 2013).
There are two types of UAVs, rotary-wing and fixed-wing (Photograph 1). Fixed-wing UAVs are
unmanned aeroplanes that use forward propulsion over a fixed airfoil to gain lift. They need a
relatively high forward velocity in order to produce this lift and so are not suitable for operation
in confined or hazardous environments.
Photograph 1: Heron I
Photograph 2: Camcopter
Photograph 3: ARDrone
(IAI 2012)
(Schiebel 2005)
(Krajnk 2010)
Rotary-wing UAVS can further be divided into another 2 types, single rotor (Photograph 2) and
multi-rotor (Photograph 3). Single rotor vehicles are helicopters. They generally use a single
large diameter rotor to generate lift and require a tail rotor for stability and direction control.
Multi-rotor vehicles use multiple rotors to control all forms of motion (Kendoul 2012).

1
1.2 History of Quadcopters

The first successful flight with a quad-rotor aerial vehicle was in the 1920s. It used a heavy steel
frame, was underpowered and a highly complicated device. As materials and engineering
practices developed over the century improvements were made by increasing power, reducing
weight and complexity. By the 1950s the first 1km closed loop flight was completed and
quadcopters were becoming a more realistic vehicle. With the development of electronic control
systems, heavy mechanical systems could be eliminated, reducing weight, cost and complexity.
Coupled with modern manufacturing techniques and modern materials such as carbon fibre,
strong, lightweight, high performance and low cost UAVs are now available (Apostolo 1984).
Photograph 4:De Bothezat Quadrotor built in 1923

(Leishman 2000)
1.3 Existing Work

Many thesis and journal articles exist on all facets of quadcopter technology. Most studies
untaken on quadcopters so far have concentrated on the theory of the kinematics and controller
required for obtaining stable flight. This theory has then normally been used to develop a
working UAV (Kendoul 2012).
Much research has been done into the micro-electro-mechanical systems (MEMS) devices,
inertial measurement units (IMU)s and microcontrollers generally used in a quadcopter. Problems
associated with using these devices in a vehicle such as quadcopter have been investigated and

2
solutions proposed or implemented. Because of this existing work it is possible to buy an off-theshelf UAV and fly it straight out of the box with little to no tuning required (Kendoul 2012).
Due to the non-linear behaviour of the motion of the quadcopter most work done has been based
on simple linearization of the non-linear equations. This linearization has inherent errors that,
although they will allow the quadcopter to be controlled in a stable manner, do not make an
accurate prediction of the response of the quadcopter possible.
Sensors including video camera, infrared and ultrasonic range finders and other assorted devices
have been added to UAVs in order to make them crash-proof and to get them to perform
acrobatic manoeuvres. The complexity of the control system implemented using these systems
has made true autonomous flight possible for not only a single vehicle, but for multiple UAVs to
co-ordinate which each other and perform tasks as a group (Kendoul 2012).
1.4 Problem Formulation and Outline of Thesis

This study will concentrate on the Crazyflie, a small open-source production Quadcopter,
commonly referred to as a nano-copter. By measurement and experimentation the required
physical attributes of the Crazyflie will be determined. This data will then be used to create as
accurate as possible a mathematical model of the Crazyflie using MATLAB. Once the model has
been created, various control systems will be implemented and compared. Controllers found to
have stable behaviours will then be implemented in the Crazyflie firmware and the results
compared to the theoretical predictions.
The sensor signals will be studied whilst in use and the effects of filtering on the control system
investigated. As the control system and sensors will be implemented in a microprocessor it must
be done in discrete time. The effects of these discrete time intervals on the controller and model
performance will be studied and compared.

3
2 Literature Review
2.1 Control and Movement of a Quadcopter
The motion of a quadcopter can be fully controlled by using its four rotors. As each rotor
produces its own lift and torque, by independently varying these values motion will occur. As the
blades traditionally used in quadcopters have fixed blades the only parameters that can be
changed are the angular velocity and angular acceleration. By controlling the revolution of the
direct current (DC) motors driving the propellers we can control the angular velocity of the
propeller, hence controlling the lift and drag (Krajnk 2011).
2.1.1 Dynamics of the Propeller
A propeller is an airfoil that rotates about a central point. Propellers can have any number of
blades from 2 upwards, either odd or even, so long as the centre of gravity of the propeller is
located at the point of rotation. As the propeller rotates 2 forces are created (Figure 1). The main
force of interest with a propeller is usually the thrust produced. This thrust provided forward
motion to aeroplanes and lift to helicopters (Bristeau et al. 2009).
Figure 1:Forces on a Propeller

(Nasa Quest 2014)
When considering the motion of the quadcopter, the torque produced by the blades becomes an
important factor. This torque can be used to rotate the quadcopter in the plane perpendicular to
the blades axes of rotation. This same phenomenon is used in helicopters to rotate them in the
horizontal plane which is then countered by the tail rotor. However as quadcopters have multiple
rotors, by varying the speed of rotation of the propellers we can control the movement in this
plane. This motion is called yawing, and the change in this angle from the reference axis is called
the yaw angle (Bristeau et al. 2009).
4
The thrust (or lift) and drag forces are related to the speed of rotation of the propeller such that
for motor i :
fi ki2
(1.1)
.
i bi2 I M i
(1.2)
Where f is the thrust, k is a lift constant, i is the torque produced by the drag force, b is a drag
.
constant, is the angular velocity of the propeller, is the angular acceleration, I M is the
moment of inertia of the propeller (Wald 2006).
From equations 1.1 and 1.2 we can see that when the propellers are rotating at a constant velocity
then the thrust and torque are proportional to the square of the angular velocity of the propellers.
2.1.2 DC motor drivers

There are two types of DC motor suitable for use in quadcopters, brushed and brushless. Small
brushed motors have been in common use for decades in small, generally battery operated
devices such as radio controlled cars. They are cheap to construct and easy to control but lack the
reliability of brushless DC motors. They consist of an energized rotor spinning between 2
permanent magnets. Energization requires the use of a physical commutator. Commutation
requires brushes which are generally the point of failure of this type of motor (Scott et al. 2009).
Brushless DC motors are now becoming more common in modern equipment. They require no
brushes to achieve commutation as the rotor contains the permanent magnets. Commutation is
achieved by switching of multi-phase coils in a selected sequence. By varying the frequency of
the switching the speed of the brushless motor can be carefully controlled (Kuphaldt 2014).
Brushless motors are lighter in weight than an equivalent brushed motor, though they require
external circuitry for commutation. This can be accomplished through lightweight electronic
speed controllers (ESCs), making them a modern viable option for an unmanned aerial vehicle
(UAV).

5
Photograph 2.1: DC Brushed Motor
Photograph 2.2:Brushless D Motor
(Renesas 2014)
(Kuphaldt 2014)
The Crazyflie nano-copter uses brushed DC motors to drive the propellers. Although it is
generally more common to see brushless motors in a quadcopter, due to the very small size of
this device and the availability of small brushless motors, the manufactures have used brushed
motors.
Photograph 5:Disassembled Motor
Photograph 6:Crazy Flie Motor
(Scott et al. 2009)
(Robotshop 2014)
The DC motors in the Crazyflie are driven using pulse width modulation (PWM). By using
PWM motor speed can be varied without the need to change the voltage. By supplying a constant

6
voltage, efficiency and simplicity are increased in the driver circuit. The speed of the motor can
be determined by the PWM and the supply voltage. If the motor supply voltage is able to provide
a constant voltage at all loads then the motor speed will be proportional to the duty cycle of the
PWM signal such that(Scott et al. 2009):
i ( i max ) /100
(1.3)
Where i is the motor speed, is the PWM as a percentage and max is the maximum motor
speed for the supplied voltage.
As the Crazy Flie does not have a regulated voltage supply to the motor drivers, the battery
voltage drop as load is increased needs to be considered when calculating the motor speed.
2.1.3 Motion and Sensors
In order to calculate the translational and rotation motion of the quadcopter sensors are required.
micro-electro-mechanical systems (MEMS) technology has seen the development of miniature
low power low cost measurement devices. These MEMS devices include gyroscopes,
accelerometers, barometers and magnetic compasses (magnetometer) that can all be used to
control the motion of the quadcopter (Wald 2006).
2.1.3.1 MEMS Gyroscope

A MEMS Gyroscope can be used to measure rotational motion. It is not affected by translational
motion. A MEMS gyroscope uses the Coriolis Effect, as shown in Figure 2.1, to measure the
angular rate.

7
Figure 2.1: MEMS Gyroscope

(Fei & Ding 2011).
Consider a mass (m) moving in direction x with angular rotation velocity z applied. The mass
will experience a force in the direction of the arrow as a result of the Coriolis force. The resulting
displacement caused by the Coriolis force is then found using a capacitive sensing structure (Fei
& Ding 2011).
Figure 2: MEMS Gyroscope

(Esfandyari et al. 2010)
A tuning fork configuration is common to most available MEMS gyroscopes. The two masses
shown in Figure 2 oscillate and move constantly in opposite directions. Under the influence of an
8
applied angular velocity, the Coriolis force acting on each mass will also act in opposite
directions, resulting in a change in capacitance. This capacitance difference is proportional to the
angular velocity z. This capacitance is then converted into either a output voltage for analog
gyroscopes or a binary number for digital gyroscopes (Liewald et al. 2013).
Under the influence of a linear acceleration, the two masses move in the same direction, resulting
in no capacitance difference. As there is no output under this condition it shows that the MEMS
gyroscope is not sensitive to linear effects or vibration (Liewald et al. 2013).
2.1.3.2 MEMS Accelerometer
Figure 3: typical MEMS accelerometer construction

(Albarbar et al. 2009)
Figure 2 show two common forms of MEMS accelerometer, a piezoresistive type using
cantilever design and a capacitive type based on membrane design. The Crazyflie uses a
cantilever type accelerometer with a separate mass for each axis. Acceleration along an axis
creates a displacement in the appropriate mass. The displacement is measured by capacitive
sensors differentially (Invensense 2014).
2.1.3.3 MEMS barometer

A barometer is a device for measuring air pressure. High resolution MEMS barometers are
available that make accurate measurement to within 10cm possible. Devices such as the MS6511
are designed for use as high resolution altimeter sensors and are an ideal method of obtaining the
air pressure in the vicinity of a quadcopter (MEAS 2012).

9
The MEMS barometer works by using a high linearity pressure sensor that can be used with a
number of different modes to produce a suitable conversion speed and accuracy for its
application. The barometer also samples the temperature which is used in conjunction with the air
pressure to calculate an altitude above sea-level (ASL)(MEAS 2012).
Figure 4: MEMS Barometer

(MEAS 2012)
Figure 4 shows the internal structure of a modern MEMs barometer. The device works by
comparing the pressure on one side of a diaphragm which is known, to the other side of the
diaphragm that is open to the atmosphere. The pressure is measured using a piezoresistor, the
output of which is interpreted by the controller and a suitable output provided by the device
(MEAS 2012).
2.1.3.4 MEMS Magnetic Compass
A compass is a simple instrument for telling the direction in which magnetic north lies. This
simple concept has been advanced and implemented in a MEMs device. The MEMS device
however can detect magnetic field intensity in as many axes as it has sensors. A 3 axis MEMs
10
magnetic compass can therefore measure magnetic field intensity in 3 dimensions (Honeywell
2010).
The MEMS compass used in the Crazyflie implements 3 magnetoresistive sensors to provide a 3
dimensional output for the magnetic field it is in. These sensors are made of a thin nickel alloy
(Permalloy) and a resistive strip element that is patterned. When in the presence of a magnetic
field the bridge elements resistance changes, causing the voltage across the bridge outputs to
change correspondingly. This is then interpreted by the onboard circuitry and an output sent to
the buffer (Honeywell 2010).
Figure 5: MEMS Magnetometer

(Du & Chen 2012)
Figure 5 shows the basic principle of operation of the MEMS magnetometer for a single sensor.
Three of these sensors are placed orthogonally to produce 3 independent readings (Honeywell
2010).
2.1.4 Inertial Measurement Unit
An inertial measurement unit (IMU) combines the data from the various sensors and calculates
rotational and translational motion. The simplest IMU uses the gyroscope and accelerometer to
calculate translational and rotational motion. The IMU must be able to filter the incoming signals
for noise, compensate for drifts and offsets in sensors. The IMU output can then be used by a
controller so that the quadcopters motion is known and can be controlled to an appropriate level
(Zhang et al. 2014).

11
2.1.5 Microprocessors and Microcontrollers

There are two basic requirements for the quadcopter, to follow commands send to it and to be
stable. To accomplish these tasks a controller is required. If the system had a predetermined set
point at which to operate and that set point was not required to be changed, a simple
microcontroller could be used to control the quadcopter (Bouabdallah & Siegwart 2007).
However the data from the IMU needs to be interpreted and a response calculated. Instructions
and parameters sent and received, communication with external devices as well as other onboard
functions also need to be done, all within a suitable time frame. To accomplish these tasks a high
speed microprocessor is more suitable as they are more flexible in their ability to be programmed
and have more built in support for external devices than does a microcontroller (Bouabdallah &
Siegwart 2007).
Due to the weight and power consumption limitations, the smaller the microprocessor the better
suited it will be. This can limit the amount of on-board processing power available for
implementation of control systems and other required functions. A compromise between
processing power, power consumption, weight and versatility must be made when selecting the
microprocessor for the quadcopter (Bouabdallah & Siegwart 2007).
The Crazyflie uses a powerful STM32F103CB microprocessor operating at 72 MHz to control all
of the onboard functions and communications. A remote computer is used for sending parameters
and commands to the CrazyFlie so its weight and power consumption are not important (Bitcraze
2014).

12
2.2 Equations of motion

2.2.1 Frames of reference
As the quadcopter is moving in 3 dimensions and is able to revolve around all 3 axes, keeping
track of its location is not a simple mater. As we are observing the quadcopter from the Earth
then it is most logical to reference everything to the Earth frame of reference (Salazar et al.
2008).
As the quadcopter moves and rotates, a point on the quadcopter needs to be used as a reference.
Normally either the centre of gravity or the physical centre of the quadcopter is used. As we have
not yet calculated the centre of gravity and the physical centre of the quadcopter is easily
determined we shall use this as out reference point (Salazar et al. 2008).
As the centre of the body is going to be the reference point for the other frames, the arms of the
quadcopter can be used as obvious axis in the x and y directions, leaving the 3rd z axis as being
orthogonal to these (Bouabdallah 2007).
Figure 6 : Frames of reference

(Bouabdallah 2007)
Figure 6 shows the Earth frame of reference (E), the quadcopter body frame of reference (B). F1,
F2, F3 and F4 represent the torque and thrusts produced by the motors, mg the effect of gravity

13
on the quadcopter and l the distance from the centre to the motor axis. With these definitions in
place, we can now define the rotational coordinate system (Salazar et al. 2008).
Figure 7: Rotational reference angles

(Balasubramanian, Cox & Waeijen 2012)
Figure 7 shows the rotational angles about the body reference frame. Conventionally the angles
are called roll around the x axis, pitch about the y axis and yaw about the z axis. With these
angles and coordinate systems we can now fully describe the position and orientation of the
quadcopter (Salazar et al. 2008).
2.2.2 Translational motion
Translational motion is normally considered at relative to the Earth reference frame, however
since the main concern of this model is stability and control, only the z-axis of the Earth
reference frame shall be used for calculations. The z-axis will give the quadcopters distance from
the origin. If this origin is located on the ground, then the z-axis value will be the altitude of the
quadcopter (Salazar et al. 2008).
Figure 6 shows 5 forces acting on the quadcopter, the 4 thrusts form the propellers and
gravitation (Scott et al. 2009). If we consider the quadcopter to be flat and at rest and the resultant

14
of these forces are not zero, then motion will occur. This motion if governed by Newtons laws of
motion such that:
Ftot f1 f 2 f3 f 4 mg
(1.4)
The resultant acceleration on the quadcopter can then be determined by:
Ftot ma
(1.5)
Where m is the mass of the quadcopter in kg and a is the acceleration in m/s2. This acceleration
will act along the z-axis and either raise or lower the quadcopter.
If all of the forces from the motors are all of equal magnitude, then the quadcopter will move up
and down along the z axis with no other motion (Scott et al. 2009). If the quadcopter has a nonzero roll or pitch angle, then motion will occur in the x or y direction such that:
Fx Ftot sin
Fy Ftot sin
Fz Ftot cos .cos
(1.6)
From this we can determine acceleration by substituting into equation (1.5), velocity and
displacement:
ax Ftot sin / m
a y Ftot sin / m
az Ftot cos .cos / m
(1.7)
vx ax .dt
v y a y .dt
vz az .dt
(1.8)
sx vx .dt
s y vy .dt
sz vz .dt
(1.9)
Where ai , vi and si are the acceleration, velocity and displacement along the i axis
respectively.

15
As the Crazyflie will be logging in discrete time, if we consider the forces to be constant for the
logging time interval, then we can write from equations (1.7, 1.8 & 1.9):
vx (ax ax 1 ) ts
v y (a y a y 1 ) ts
vz (az az 1 ) ts
(1.10)
sx (vx vx 1 )ts
s y (v y v y 1 )ts
sz (vz vz 1 )ts
(1.11)
Where ts is the logging timestamp interval, ai 1 and vi 1 are the acceleration and velocity along
the i axis respectively, at the last time interval.
2.2.3 Rotational motion
A quadcopter is capable of rotation about all 3 axes. For rotation to occur there must be an
imbalance in the forces being applied to the quadcopter by the 4 propellers. This occurs in a
number of ways, resulting in a number of resultant motions. Table 1 shows the force imbalance
and resultant quadcopter motion (Scott et al. 2009).
Table 1: Rotational Motion
Force Imbalance
None
Opposite motors both increased
Odd motors varied by opposite but equal amount
Even motors varied by opposite but equal amount
Adjacent motors increased by the same amount
All motors at unequal amount
Roll
Pitch
Yaw
Nil
Nil
Nil
Nil
Nil
Pos/Neg
Pos/Neg
Nil
Nil
Nil
Pos/Neg
Nil
Pos/Neg Pos/Neg
Nil
Pos/Neg Pos/Neg Pos/Neg
From Table 1 it can be seen that yaw motion only results if there is an imbalance in motor pairs.
This is because the yaw motion is caused by the torque induced by the drags of the propellers.
When these torques do not cancel out there is a resultant torque which causes rotation of the
quadcopter about the z axis (Scott et al. 2009). The torque is shown in Figure 6.
tot 1 2 3 4
(1.12)
Where tot is the resultant torque and i is the torque cause by propeller i both measured in Nm.
The propeller torque is caused by the drag as the propeller rotates through the air. This torque can
be calculated using equation (1.2). It can be seen from equation (1.2) that is the propeller is under
16
a constant angular velocity then the moment of inertia contribution will be zero. By using this
property we can calculate the drag constant experimentally.
When the conditions for rotational motion to occur are fulfilled, the quadcopter will behave such
that the torque will produce yaw motion such that:
yaw tot l
(1.13)
Where yaw is the resultant torque in Nm and the yaw plane and l is the distance from the motor
centre to the centre of the body in m.
This torque will result in an angular acceleration in the yaw plane such that:
yaw yaw I ZZ
(1.14)
Where yaw is the angular acceleration in rad/s2 in the yaw plane and I ZZ is the moment of
inertia in the yaw or zz plane in kg.m2.
Rearrangement of equation (1.14) can give us the angular acceleration:
yaw
yaw
I ZZ
(1.15)
Substituting from equation (1.12 & 1.13):
yaw
( 1 2 3 4 ) l
I ZZ
(1.16)
As all of the variables are either known or can be calculated, we can calculate the resulting
acceleration in the yaw plane.
Similarly for motion in the roll plane, from Table 1, we need motor 2 and motor 4 to be
unbalanced, resulting in:
Froll f 2 f 4
Where Froll is the resultant force in N in the roll direction.

17
(1.17)
Using the theory of superposition we can consider this force as acting at the same position as
motor 1. This will produce a torque which will cause motion in the roll direction such that:
roll Froll l
(1.18)
Where roll is the resultant torque in N in the roll direction.

Substituting equation (1.13) into equation (1.18) gives the angular acceleration:
roll
roll
I yy
(1.19)
Where roll is the angular acceleration in rad/s2 the roll plane and I yy is the moment of inertia in
the roll of yy plane in kg.m2.
Substitution of equation (1.1) into equation (1.19) gives us an expression for angular acceleration
proportional to the motor speed such that:
roll
(k22 k42 ) l
I yy
(1.20)
By applying similar logic we can find the expression for acceleration in the pitch plane from
equation (1.20) such that:
pitch
(k12 k32 ) l
I xx
(1.21)
Where pitch is the angular acceleration in rad/s2 in the pitch plane and I xx is the moment on
inertia in the pitch or xx plane in kg.m2.
As the Crazyflie is operating in discrete time, we can apply the same theory as in equations (1.10
& 1.11) to get expression for the angular velocity and angular displacements in the roll, pitch and
yaw directions such that:

18
roll ( roll roll 1 ) ts

pitch ( pitch pitch 1 ) ts
(1.22)
yaw ( yaw yaw1 ) ts

(roll roll 1 )ts
( pitch pitch 1 )ts
(1.23)
( yaw yaw 1 )ts

Where roll , pitch and yaw are the angular velocities in rad/s in the roll, pitch and yaw directions
respectively.
2.2.4 Friction and air-resistance
As the quadcopter is in motion there will be some frictional forces involved in its motion. The
largest effect on the motion of the quadcopter will be air resistance. Air resistance will affect the
motion of the quadcopter in all degrees of freedom. The effect on each degree of freedom will
not be the same due to the structure of the quadcopter and the way in which motion normally
occurs (Scott et al. 2009).
Considering translational motion first, we will have air resistance affects on all 3 axis. The affect
of the air resistance is to oppose the direction of motion (Scott et al. 2009). The opposing force
has 2 components, the first is proportional to the velocity and the second proportional to the
velocity squared, such that:
Fi mai i vi i vi2
(1.24)
Where is the 1st order air resistance constant and i is the second order air resistance constant
along the i axis. Note that in equation (1.24) the air resistance forces both opposed motion in the
current direction of travel so care must be taken to ensure correct application of positive and
negative forces.
Rearranging equation (1.24) gives:
ai
Fi i vi i vi2
m

19
(1.25)
Rotational motion will also be affected by air resistance. However due to the lower speeds
involved with angular motion in the normal operation of a quadcopter only a first order velocity
constant will be used (Bristeau et al. 2009):
i Iiii ii
(1.26)
Where i is the first air resistance constant for motion in the i plane.
Rearranging equation (1.26) gives us angular acceleration:
i ii
I ii
(1.27)
2.2.5 Battery Voltage Effects

As the Crazyflie uses an unregulated battery power supply, as the load is increased on the battery
the battery voltage drops. As the motor speed is affected by the voltage driving it we need to
consider this battery voltage effect in order to get an accurate speed at which the motors are
turning (Kuphaldt 2014). The DC brushed motors used behave such that:
M PWM MAX
(1.28)
Where M is the angular velocity of the motor in rad/s, PWM is the pulse width modulation
duty cycle driving the motor, and MAX is the maximum motor speed in rad/s for a given voltage.
For small changes in motor voltage, MAX varies linearly. We can therefore express the motors
angular velocity as:
VB
PWM MAX
VNL
(1.29)
Where VB is the operating and VNL the no-load voltages for the battery in V. As the exact load
characteristics of the LiPo batter in the Crazyflie are not known, a voltage versus load experiment
will be used to create a 3rd order approximation to the behaviour of the battery under a range of
load conditions. This polynomial can then be used to calculate VB for the current motor PWM
settings and hence get an accurate motor angular velocity to use when calculating propeller lifts
and drags (Kuphaldt 2014).

20
3 Experimental determination of physical attributes

3.1 Moments of Inertia
A trifilar will be used to calculate the moments of inertia of the Crazy Flie. As the moments of
inertia need to be determined about 3 axis it will be simplest to use the 3 axis used by the mircoelectro-mechanical system (MEMS) devices.
Figure 8:Trifilar Pendulum

21
Photograph 7:Moment of Inertia about z-axis
Photograph 8: Moment of Inertia about x-axis
Results:
Table 2: Moment of Inertia of Crazy Flie
IZZ
IYY
IXX
Mean
2.66
2.49
2.48
St.dev
0.0357
0.0373
0.0401
Median
2.67
2.50
2.46
Mode
2.67
2.51
2.46
Mass(g)
22
22
22
Period
0.665
0.622
0.619
Mass(Kg)
0.022
0.022
0.022
TotalMOI
5.897E05
5.160E05
5.119E05
SpecimenMOI
2.173E05
1.436E05
1.395E05
TheoreticalMOI
2.97E05
Note: Theoretical value is for a square plate the same size and weight as the CrazyFlie
From table 2 we can see that he moment of inertia in the yaw plane is 0.217 kg.m2, in the roll
plane 0.144 kg.m2 and in the pitch plane 0.140 kg.m2
3.2 Propeller Lift Constant

To calculate the lift constant for the Crazyflie propellers the firmware must be modified to allow
for all of the motors to run at the thrust setting without any controller compensation, see
Appendix D.

22
With the motors running at the same thrust the reaction force of the Crazyflie will be measured.
This is done in 2 parts, firstly for thrusts over the take-off thrust the Crazyflie is laden with set
weights and the required lift-off thrust recorded. For thrusts under the take-off thrust the change
in weight of the Crazyflie measured by scales is used. To help eliminate vibration the Crazyflie is
placed on a piece of soft foam rubber on the scales.
With the proportional integral derivative (PID) attitude compensation removed from the firmware
the Crazyflie would be running at a roll pitch and yaw (RPY) set point of [0,0,0] and with a thrust
set by the operator. Although the experiment uses the weight as the independent variable, the
graph is plotted as revolutions per minute (RPM) as the dependant variable as the RPM is what
will be used in the model, the lift being calculated from this.
Thrust weight lifting vs motor RPM
0.05
0.045
experimental data
theoretical lift
0.04
Weight lifted (kg)
0.035
0.03
0.025
0.02
0.015
0.01
k=2.75E-11
0.005
0
0.5
1.5
2
motor RPM
2.5
3
4
x 10
Figure 9: Crazyflie Propeller Lift Constant
Figure 9 shows the resultant thrusts for the used RPMs. The weight was then divided by four to
work out the contribution of each motor. A value of k was then calculated to be 2.75E-11.

23
3.3 Propeller Drag Constant

To calculate the propeller drag constant for the Crazyflie propellers the firmware was modified to
allow only opposite motors to operate. By measuring the resultant torque at various motor thrusts
the drag constant could thus be calculated using equations (1.2 & 1.15).
The Crazyflie is sent a set point that consists of a roll, pitch, yaw and thrust. The firmware uses
PID controllers to adjust the thrusts of the individual motors so this set point can be reached. As
this experiment requires that only opposing motors are in operation and that they are at a set
speed the PID control needs to be changed. This is accomplished by setting the required pair of
motors to the set point thrust and removing the roll, pitch and yaw compensation factors. The
thrust of the other pair of motors is set to zero. See appendix D for the firmware modifications.
Once a means of controlling the motor speeds is implemented the Crazyflie needed to be
restrained from falling. Due to the required thrust for flight not being able to be produced by only
2 motors, there is no need to consider restraint from flight.
A rig was made from the lightest weight fishing line available that held the Crazyflie level, but
enabled it to rotate about the z-axis(photograph 9).Test runs were conducted using the rig to
determine its effect on the motion of the Crazyflie to ensure that the data from the experiment
would be useable.

24
Photograph 9: Crazyflie yaw rig
The experiment was conducted using the odd numbered motors first, then the even numbered.
The thrust was varied from 20% to 99% in 5% intervals in order to cover as much of the
operating ranges as possible.
The yaw angle and thrust of the Crazyflie was then logged and the results used to calculate the
torque being produced.
Due to the effects of air-resistance and the winding up of the yaw-rig if the CrazyFlie is allowed
to spin for too long, the acceleration to 22.5, 45 90 and 180 degree are used to calculate the
torque produced(See appendix A).

25
Velocity vs Time for various thrusts

35
25
30
35
40
45
50
55
60
65
70
75
80
85
90
95
30
25
velocity (rad/s)
20
15
10
-5
5
6
Time(s/10)
10
Figure 10: Crazyflie Propeller Drag Coefficient
Figure 10 shows the results of the experiment using motors 1 and 3. In Figure 10 it can be seen
that as the thrust increases the torque increases. From equation (1.2) the relationship between the
torque and the pulse width modulation (PWM) of the motor should vary with a squared
relationship. A line of best fit was calculated using the equation (1.2) .This produced a value for
the propeller drag constant b of 1 x 10-9 .
3.4 Battery Discharge Rate

The Crazyflie is powered by a lightweight Lithium Polymer (LiPo) battery. As with most
batteries, the voltage of the battery decreased under increasing load and over time as its stored
energy is used. The experiments used in calculating the Crazyflie parameters are always
26
conducted with a fully charged battery to negate the effects of the battery losing energy. The
battery load effect is therefore the only characteristic needed to be considered (Scott et al. 2009).
To simplify measurement and calculations the battery voltage will be considered versus the
summation of the pulse width modulation (PWM) percentages. As only the summation of the
PWMs is being considered, all four motors will be used to load the battery at different thrusts. In
order for the motors to be driven at a constant thrust the firmware will be modified to remove the
PID controller compensation factors from the motor drives (see Appendix C).
The experiment will be conducted over a range of thrusts from 25% to 99% and the onboard
voltmeter used to measure the change in the battery voltage. The Crazyflie logging configuration
will be used to create a log of these parameters.
Figure 11: Battery voltage vs thrust model
Figure 11 shows the effects of thrust on the CrazyFlie LiPo battery. MATLAB was used to
create various order polynomials to fit this curve (see Appendix A). As can be seen from figure
11 a 3rd order polynomial fits the data points to a reasonable degree of accuracy. This
polynomial was found to be:

27
wout=(4.0027-(0.0132*PWMavg)+(3.6371e-05*(PWMavg^2))+(1.2803e07*(PWMavg^3)))/4.0027*win
As the speed of the direct current (DC) motors is affected by the voltage source, this reduction of
voltage at increased loads will result in a lower actual motor speed than the PWM would
theoretically produce. This means that the drag and lift calculations conducted previously are not
as accurate as they could be. Using the battery model polynomial to calculate the actual motor
speeds for the given thrusts the lift and drag constants are recalculated and the results for the lift
shown in Figure 12 (Scott et al. 2009)..
Thrust vs Motor RPM
0.07
Experimental lift data
Uncompensated thrust with new k value
Thrust with Battery Compensation
0.06
Thrust (Kg)
0.05
0.04
0.03
0.02
new value k=2.25E-11

old value k=2.75E-11
0.01
0
0.6
0.8
1.2
1.4
Motor RPM
1.6
1.8
2.2
4
x 10
Figure 12: Battery Compensated Thrust
The battery compensation produced a noticeable effect in the lift and drag constant, increasing
them both by approximately 5%. These corrected constants and the battery 3rd order battery
polynomial are used in the MATLAB mathematical model. A new k value of 2.25E-11 was
obtained from these results.
28
3.5 Air Resistance

Air resistance has a number of effects on the CrazyFlie. Depending upon its direction of travel
the air resistance will vary. To calculate the effect in each direction two experiments were carried
out (Bristeau et al. 2009).
The first experiment is to calculate the air resistance in the z-axis. To calculate this we will use
the accelerometer and drop the Crazyflie so that it is level. The air resistance will slow down the
acceleration of the CrazyFlie proportional to its velocity and proportional to the square of the
velocity as well. These two constants of proportionality will give us the parameters need from
equation (1.24).
The Crazyflie was test dropped to ensure that it stayed relatively level. It was found that on most
attempts the CrazyFlie stayed within 5degree of being level when dropped a height of 1.5m. This
height was then used and the attempt with the most level behaviour used to calculate the
parameters.

29
Acceleration due to Gravity vs Time for Freefall of the Crazyflie

9.85
9.8
Acceleration (m/s/s)
9.75
9.7
9.65
9.6
9.55
9.5
100
200
300
time from release (ms)
400
500
600
Figure 13: Crazyflie in Freefall data
Figure 13 show the results of dropping the Crazyflie. The Crazyflie started at rest, being
restrained against the force of gravity. After the Crazyflie was released it began accelerating
towards the earth due to gravitation. As the velocity of the Crazyflie increased, the rate of
acceleration decreased.

30
Acceleration Due to Gravity vs Time fro Crazyflie in Freefall

9.85
Experimental data points
Acceleration due to gravity with air restistance
9.8
Acceleration(m/s/s)
9.75
9.7
9.65
9.6
az=(-0.7476*t 2)-(0.0279*t)+9.8315
9.55
9.5
0.1
0.2
0.3
0.4
0.5
0.6
0.7
time(s)
Figure 14: Crazyflie in Freefall polynomial fitting
Figure 14 shows a 2nd order polynomial approximation of the experimental data points. This
polynomial was calculated using the MATLAB polyfit command.

31
Acceleration Due to Gravity vs Time for CrazyFlie in Freefall

9.85
Experimental data points
acceleration with air resistance
Polynomial of best fit for data points
9.8
Acceleration (m/s/s)
9.75
9.7
9.65
Air resistance:
a=g-(0.019*v)-(0.001*v2)
9.6
Polynomial for line of best fit of data
a=9.8351-(0.0279*t)-(0.7476*t2)
9.55
9.5
0.1
0.2
0.3
0.4
0.5
0.6
0.7
time(s)
Figure 15: Crazyflie in Freefall air resistance modelling
After the 2nd order polynomial was fitted the velocity was calculated by integrating the
acceleration. Figure 15 show the acceleration with air resistance after the parameters where
calculated. As can be seen in Figure 15 the air resistance model was a very good approximation
to the motion of the Crazyflie.
As the Crazyflie is approximately symmetrical about the z-axis, the air resistance when travelling
in both the x and y directions will be similar. To calculate the air resistance along these axes the
CrazyFlie will be suspended as a pendulum and the degradation due to air resistance calculate
using equation (1.27). The Crazyflie will be restrained so no rotational motion is possible.

32
The Crazyflie was suspended by two thin filaments and an indexed board placed behind so that
the angle of swing could be recorded. The motion was recorded using a video camera and the
video slow motion replay used to get the angles of swing as the pendulum degraded.
Air Resistance Coefficient
0.01
0.0083234
air resistance
0.008
0.006
0.004
0.002
6
Oscillation
Figure 16: Pendulum degrading
The results from Figure 16 produced an air resistance parameter of:
x y 0.0083234

33
10
11
4 Mathematical model development and implementation

4.1 Methodology of development
To simplify development of the MATLAB model functions will be used to perform various tasks.
The first tasks are to create a list of the variables that will be needed to fully describe the state of
the quadcopter and set a naming standard for variables and functions (See appendix C).
To aid in development of the model and calculation of some of the parameters of the Crazyflie
some separate functions will be created that are not part of the final model; they will however
still use the same variable names and programming rules as the main function.
Analysis of the workings of the model led to the development of the following function structure
as seen in table 3.
Table 3: MATLAB Functions
Function Name
Function Description
QuadcopterMain.m
Main function, accepts parameters and returns results
ShowCopter.m
Displays the quadcopter in its 3 dimensional environment
PlotQuadcopter.m
Creates and displays graphs of the variable describing the quadcopter motion
PWMBatteryModifier.m
Uses the Crazyflie battery model to create a compensated motor RPM
QuadcopterController.m
Implements PID controller actions (can also implement any other controller)
QuadcopterDynamics.m
Uses the given variables and calculated the quadcopters responses
SetQuadcopterParameters.m Is used to store and create variables based on given physical attributes
As well as these main functions, some sub-functions were created to simplify function
programming, these can be seen in table 4.
Table 4: MATLAB Sub functions
Function Name
Function Description
titleHeader.m
Called by SetQuadcopterParameters.m to allow user input
Cylinder2P.m
Used by ShowCopter.m to draw surface mapped cylinders

34
4.2 MATLAB Implementation

The functions in table 3 where written in Pseudo code and then implemented in MATLAB. The
functions where then tested first using simple known static cases and then the complexity slowly
increased until the functions where functioning correctly.
4.3 Comparison of MATLAB model to experimental results

After the MATLAB model was fully functional the parameters of the Crazyflie were input into
the main function. The variables where set to those used in obtaining the propeller drag and lift
characteristics and the results compared to those found in the experiment.
5. Controller designs
5.1 Crazy Flie Control System
5.1.1 Properties of the Existing System
5.1.2 Modifications to the existing system
The PC-Client and firmware used for the Crazyflie are 2013.4.2 and 2013.11 respectively. This
allows for logging of the stabilizer, motor drive and gyroscope parameters. To enable a more
detailed inspection of the Crazyflie further parameter logging needs to be implemented. This
required modification to both the PC-Client and the firmware.
To allow for a consistent and repeatable take-off and hover a system will be incorporated into the
Crazyflie to allow for a button activated take-off, hover and landing to occur. This will allow for
a more accurate comparison between control systems.
To allow for a hover mode to be implemented a controller will need to be implemented to allow
the Crazyflie to hover at a constant altitude. There will therefore be two control systems
implemented, an altitude and an attitude. There is already a proportional integral derivative (PID)
attitude control system implemented in the Crazyflie firmware.
5.1.2.1 PC-Client modifications
The PC-Client displays only a minimal set of parameters of the Crazyflie. These include the
actual and required roll, pitch, yaw and thrust as well as individual motor pulse width modulation
35
(PWM) percentage. A beta version of the firmware was found that displays the altitude of the
Crazyflie in the attitude widget. Code from this beta was used to allow for the display of the
altitude above sea level (ASL) for development of a barometer altitude holding controller. The
modification made to the PC Client code can be seen in Appendix E.
To help in implementation of an accelerometer based altitude controller the accelerometer data
needs to be added to the logged parameters and to the data displayed in the PC-Client. The
accelerometer data will be used in determination of the velocity and displacement of the
Crazyflie.
The magnetometer is unused in the current firmware. Although this data is not going to be used
in the control system, logging and display of the magnetometer data will be added to the PCClient.
Figure 17: Original PC Client interface

36
Figure 18: Modified PC Client interface
In figure 17 and figure 18 we can see the changes made to the user interface. Take-off, hover and
landing modes were added to the PC-Client. These new modes take advantage of the existing
communications protocols as much as possible to minimise the amount of extra bandwidth
required to send set points and updated parameters to the Crazyflie. As thrusts fewer than 20%
are not in use, these lower thrust settings will be used in conjunction with a suitable algorithm to
allow for the extra flight modes to be communicated.
5.1.2.2 Firmware modifications

The Crazyflie firmware needs to be modified to accept the new encoded thrust set points sent by
the PC-Client and to implement new attitude and altitude controllers (See Appendix D).
Thrusts fewer than 17000 need to be decoded and used in flight modes other than Normal Flight.
The decoded thrust then needs to be used to set required base thrust setting. The encoding for
thrust values >15000 are encoded the same for all controllers, however for thrust under 15000
there is a difference in the way the encoding is done.

37
When using the barometer for altitude determination the thrust will be calculated using a PID
controller based on the error in the current ASL and thrust settings. The required ASL is sent
encoded such that:
thrust = 1000+(ASL*10)
(1.30)
This ASL value is compared to the current ASL of the Crazyflie and a new thrust calculated
using the controller implemented in the firmware.
The Hover mode implemented using the accelerometer sends the required set point of the
accelerometer z-axis assuming that the roll pitch and yaw (RPY) is [0,0,0]. If RPY are not the
same as the set point then this value needs to be compensated for. The PID controller then
calculates the required thrust to meet this condition. The required accelerometer value is sent as
the 5 most significant figures multiplied by 10000. That is a value of 1.0455g would be sent as a
thrust=10455.
Barometer controller implementation is done via a PID controller implemented in discrete time
programmed in C. The integral calculation is limit to a maximum and minimum value to stop
overload. The differential control signal is calculated over the last 2 values to limit the effect of
the discrete time sampling causing large fluctuations in the derivative of the error signal.
The accelerometer based altitude controller uses a PID controller similar to the barometer.
However since the displacement is being calculated using double integration there is a large
integral effect over time that needs to be compensated for.
5.2 Comparison of Altitude Control Methods

5.2.1 Barometer Altitude Control
The barometer altitude controller will use the ASL calculated by the micro-electro-mechanical
systems (MEMS) barometer and use it to maintain a constant altitude. The PC-Client displays the
ASL in metres to an accuracy of 2 decimal places. Due to the size of the Crazyflie an accuracy of
altitude of 0.1m will be sufficient to prove the functionality of the controller.

38
Relative ASL and Thurst vs Time

1
0.9
Thrust
ASL
0.8
Relative Values
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
10
time(s)
Figure 19: Thrust effect on ASL reading
As the required ASL is being read directly this is no error accumulation such as would occur with
control methods that use integration. There is a variation in local air pressure due to the
disturbance of the propellers to this disturbance has been recorded versus the propeller thrust in
order to compensate for this phenomenon. Figure 19 shows an example of the ASL noise
interference cause by varying the thrust from 0 to 65% over a 10 second period. As can been seen
from the graph there is a rough correlation between the change in ASL and the thrust percentage,
but it is not linear or consistent enough to create an accurate model of.

39
5.2.2 Accelerometer Altitude Control

The accelerometer produces an output signal that measures the acceleration of the Crazyflie in 3
orthogonal directions. By integrating this signal the velocity along these 3 axes can be calculated.
By integrating again, displacement can be calculated.
Due to the discrete nature of the signal there will be an error in the integral calculation due to the
linearization of the signal. This error will eventually average to zero and does not need to be
considered here.
The double integration of the acceleration to obtain displacement does however introduce known
problems with control system. It is therefore necessary to use a control that can handle this, such
as a PID controller. The PID controller is also simple to implement in discrete time using a C
program in the firmware.
The accelerometer is susceptible to inaccuracy due to vibrations. To mean the effect of vibrations
at different thrusts the Crazyflie has various thrust applied to it that were insufficient for take-off.
This allows for a constant altitude (displacement) to which the outputs can be compared.

40
Relative ASL and Thurst vs Time

1
0.9
Thrust
ASL
0.8
Relative Values
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
10
time(s)
Figure 20: Displacement and Velocity Drift over time
Figure 20 shows the displacement drift over time without using any offset compensation.
Although the accelerometer is supposed to self calibrate there is still a residual offset.
Compensation for this offset helps remove some of the drift.
5.3 Initial Performance of Controllers

5.3.1 Barometric pressure control
As the barometric pressure of the Crazyflies environment is not constant and varies at rates that
lead to the ASL variation of over 0.5m/minute, the required accuracy could therefore be obtained

41
for less than 13 seconds. This makes the barometer practically useless for making the Crazyflie
hover to within any accuracy.
The air pressure around the Crazyflie is also affected by the turbulence of the propellers. As the
thrust is increased air pressure under the propellers is increased. When the above factors are
combined it makes the MEMS barometer an impractical means of controlling the CrazyFlie
altitude.
ASL vs Time for a 3 hour random period
Note: 50m drift in 3 hours
110
100
ASL (m)
90
80
70
60
50
6
8
timestamp value (ms*1000)
10
12
14
6
x 10
Figure 21: Barometer Drift over time
The barometer drift was logged over a random selected 3 hour time period. The results shown in
Figure 21 show a drift of in excess of 50m over this time period.
5.3.2 Accelerometer control
When there is no vibration due to the motors and the Crazyflie is moved around gently by hand
useable data was obtained from the accelerometer.

42
The data from the accelerometer showed a distinct drift when used to calculate velocity and
displacement. This is mainly caused by any small errors in readings being amplified by the
double integration that takes place. This can also be seen in figure 22 by the fact that the error in
the velocity is much smaller that the error in displacement.
Acceleration, Velocity and Displacement vs Time
1
magnitude relative to maximum positive values
Displacement
Velocity
Acceleration
0.5
-0.5
-1
-1.5
Accelerometer offset of
0.014
Time(s)
Figure 22: Displacement and Velocity drift over time

43
10
6 Results of control system implementation in Quadcopter

6.1 Results of implementation
6.1.1 Altitude Control using MEMS Barometer
A control system using the Crazyflies micro-electro-mechanical systems (MEMS) barometer was
implemented as described. The system was first tested by moving the Crazyflie by hand and
checking the PC clients response to its movement. A take-off height on 1m was used with a
take-off timer of 3 seconds.
The take-off button was depressed and the flight mode observed changing to mode 1. After a
short time the flight mode was observed changing to mode 2. At this time the Crazyflie was lifted
by hand to a height on in excess of 1m. The flight mode was observed changing to mode 3 as
required.
The firmware was then modified to power the motors as per normal and the altitude controller
tested. Upon pressing the take-off button the Crazyflie motors spun up to their mode 1 speed.
After the timer had reached its count and the mode was changed to mode 2, the Crazyflie took off
in an approximately vertical direction as predicted.
Upon reaching approximately half the take-off height the Crazyflie switched to mode 3 and shut
down and fell to the floor. Upon hitting the floor it applied full thrust and took off and hit the
ceiling (approx 2.5m) at which time the emergency stop was hit. Repeated attempts at various
proportional integral derivative (PID) controller settings where tried with either a similar result or
the Crazyflie not hitting the floor and continuing upwards at increasing speed until it hit the
ceiling or was shut down.
6.1.2 Altitude Control using MEMS accelerometer
The MEMs accelerometer was used to implement an altitude control system in the Crazyflie
firmware as described. As with the barometer control system the accelerometer was first trial run
by hand to ensure its correct operation.

44
The motor power was disabled in the firmware and the take-off height set to 1m with a timer of 3
seconds. Upon pressing the take-off button the flight mode changed to mode 1 and then mode 2
after a short period of time. The Crazyflie was then lifted by hand to the desired height.
When the Crazyflie reached the desired height a steady drift was noticed in the altitude height,
although the Crazyflie was being held steady. Due to the drift it was obvious that the altitude drift
was too great to be able to hold the Crazyflie at an accurate altitude and so the controller was not
implemented and run.

45
7 Conclusions and Recommendations for Future Work

7.1 Conclusions
An accurate mathematical model of a quadcopter is possible to develop and use for the
implementation and testing of control systems. The MATLAB model developed for this Thesis
predicted the behaviour of the Crazyflie when it was operated to within specific limits.
Experiments can by carried out that can be used to accurately determine the physical parameters
of an unmanned aerial vehicle (UAV) required when producing the mathematical model. With
the correct experimental techniques and theoretical knowledge all of the required parameters for
the model were calculated to the required degree of accuracy.
Micro-electro-mechanical systems (MEMS) technology has made available a wide range of low
weight, low power sensors which can be used to stabilize and navigate a UAV. These sensors do
have limitations and do not always perform their tasks as expected. Care must be taken when
using these devices and designing systems in which they will be used.
7.2 Recommendations for Future Work

As the MEMS barometer was found to not provide an accurate altitude above sea level (ASL)
measurement then the use of a reference barometer on the CrazyRadio is recommended. As the
barometers are in close proximity then a differential reading should be able to compensate for the
atmospheric pressure changes.
The MATLAB model needs to be finalised to take into account the rotational reference frame for
calculations of linear motion in the Earth reference frame. This was not needed for the control
system implementation but would produce a more complete and accurate model for navigation
and performance.
As the MATLAB software was written for simplicity of reading and use there is a great deal of
room for optimization and refinement. If the MATLAB could be optimized then real time
visualization should be possible. This would make the mathematical model more useful and
easier visually see what is happening to the UAV.
46
References
[1] Albarbar, A , Bardi, A, Jyoti, K and Starr, A 2009,Performance evaluation of MEMS
accelerometers, Measurement, Vol 42, Iss 5, Pp. 790 795, viewed 23 March 2014,
<http://www.sciencedirect.com.ezproxy.cdu.edu.au/science/article/pii/S0263224108002091>.
[2] Apostolo, G 1984, The Illustrated Encyclopedia of Helicopters, viewed 25 March 2014,
<http://www.aviastar.org/helicopters_eng/bothezat.php>.
[3] Balasubramanian, S, Cox, M & Weijen,L 2012,Design & Implementation of a Quadcopter
with Visual Control, PhD Thesis, Department of Electrical Engineering, Eindhoven University
of Technology, Eindhoven
[4] Bitcraze, 2014,Bitcraze Wiki, Bitcraze, viewed 25 October 2013,
<http://wiki.bitcraze.se/projects:crazyflie:index>
[5] Bouabdallah, S 2007,design and control of quadrotors with application to autonomous
flying, PhD Thesis, Universit Aboubekr Belkaid, Tlemcen, Algerie
[6] Bouabdallah,S & Siegwart,R 2007,Design and Control of a Miniature Quadcopter, Science
and Engineering, Vol. 30, pp170-210, viewed 23 February 2014,<
http://link.springer.com/book/10.1007/978-1-4020-6114-1>
[7] Bristeau, P, Martin, P, Salaun, E & Petit, N 2009,The Role of Propeller Aerodynamics in the
Model of a Quadcopter UAV, Proceedings of the European Control Conference 2009,
Budapest, Hungary, 23-26 August 2009, viewed 23 March 2014,
<http://cas.ensmp.fr/~petit/papers/ecc09/pjb.pdf>.
[8] Du, G & Chen, X 2012,MEMS magnetometer based on magnetorheological elastomer,
Measurement, Vol 45, pp 54-58, viewed 1 May 2014, <www.elsevier.com/locate/measurement>
[9] Esfandyari, J, De Nuccio, R & Xu, G 2010, Introduction to MEMS gyroscopes, Electroiq,
viewed 23 March 2014, <http://electroiq.com/blog/2010/11/introduction-to-mems-gyroscopes/>.
[10] Fei, J & Ding, H 2011, System Dynamics and Adaptive Control for MEMS Gyroscope
Sensor, International Journal of Advanced Robotic Systems, Vol 7, Iss 4, Pp 81-86
, viewed 23 March 2014, <http://www.intechopen.com/articles/show/title/system-dynamics-andadaptive-control-for-mems-gyroscope-sensor>.
[11] Honeywell, 2010,HMC5883 MEMS Magnetometer, Seedstudio, viewed 18 March 2014,
<http://www.seeedstudio.com/wiki/images/4/42/HMC5883.pdf>

i
[12] Invensense, MPU-6000 and MPU-6050 Product Specification Revision 3.4, Invensense,
California, viewed 23 March 2014, <http://invensense.com/mems/gyro/documents/PS-MPU6000A-00v3.4.pdf>.
[13] Israel Aerospace Industries (IAI), 2012, digital image, viewed 23 March 2014,
<http://www.unmannedsystemstechnology.com/2012/08/swiss-plan-in-flight-tests-for-new-uavsystem/air_uav_heron_canada_lg/>.
[14] Kendoul, F 2009,Survey of Advances in Guidance, Navigation, and Control of Unmanned
Rotorcraft Systems, Journal of Field Robotics, Vol 29, Iss 2, pp315-378, viewed 1 May 2014, <
http://onlinelibrary.wiley.com/doi/10.1002/rob.20414/abstract>
[15] Krajnk, T , Vonasek, V, Fiser, D & Faigl, J 2011, AR-Drone as a Platform for Robotic
Research, The Gerstner Laboratory for Intelligent Decision Making and Control Department of
Cybernetics, Faculty of Electrical Engineering Czech Technical University, Prague
[16] Kuphaldt, T 2014, Brushless DC motor, All About Circuits, viewed 23 March 2014,
<http://www.allaboutcircuits.com/vol_2/chpt_13/6.html>
[17] Leishman, J 2000, digital image, viewed 23 March 2014,
<http://terpconnect.umd.edu/~leishman/Aero/history.html>.
[18] Liewald, J, Kuhlmann, B, Balslink, T, Trachtler, M, Dienger, M & Manoli, Y 2013,100
kHz MEMS Vibratory Gyroscope, Journal of Microelectromechanical Systems, Vol 22, No 5,
Pp 1115-1125, viewed 23 March 2014,
<http://ieeexplore.ieee.org.ezproxy.cdu.edu.au/stamp/stamp.jsp?tp=&arnumber=6562743>.
[19] MEAS, 2012, MS5611-01BA03 Barometric Pressure Sensor, with stainless steel cap,
Measurement Specialities, viewed 12 March 2014, <http://www.measspec.com/downloads/MS5611-01BA03.pdf>
[20] Merz, T & Kendoul, F 2013,Dependable Low-altitude Obstacle Avoidance for Robotic
Helicopters Operating in Rural Areas, Journal of Field Robotics, Vol 30, Iss 3, pp 439-471,
viewed 1 April 2014, < http://onlinelibrary.wiley.com/doi/10.1002/rob.21455/abstract>
[21] Nasa Quest, 2014, digital image, viewed 23 March 2014,
<http://quest.nasa.gov/aero/planetary/atmospheric/propulsion1.html>.
[22] Renesas 2014, Renesas' motor inverter solution that is a key technology for reducing the
energy consumed by motor-driven appliances, Japan, viewed 23 March 2014,
<http://www.renesas.com/edge_ol/features/05/index.jsp>.
[23] Robotshop 2014, digital image, viewed 23 March 2014,
<http://www.robotshop.com/en/crazyflie-nano-quadcopter-dc-motor.html>.

ii
[24] Salazar, S, Romero, H, Lozano, R & Castillo, P 2009,Modelling and Real-Time

Stabilizatino of an Aircraft Having Eight Rotors, Unmanned Aircraft Systems 2009, pp 455470, viewed 23 February 2014, < http://link.springer.com/chapter/10.1007%2F978-1-4020-91377_24>
[25] Schiebel, 2005, digital image, viewed 23 March 2014, < http://thefutureofthings.com/5639camcopter-s-100-uav/>.
[26] Scott, J, McLeish, J & Round,W 2009, Speed Control With Low Armature Loss for
Very Small Sensorless Brushed DC Motors, IEEE Transactions on industrial electronics, Vol.
56, No. 4, Pp 1223-1229, viewed 23 March 2014, <
<http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4663813&url=http%3A%2F%2Fieeexpl
ore.ieee.org%2Fiel5%2F41%2F4808369%2F04663813.pdf%3Farnumber%3D4663813>.
[27] Wald, Q 2006,The aerodynamics of propellers, Progress in Aerospace Sciences, Vol 42,
Pp 85128, viewed 23 March 2014, < www.elsevier.com/locate/paerosci>.
[28] Zhang, R , Hoflinger, F & Reindl, L 2007,Calibration of an IMU Using 3-D Rotation
Platform, IEEE sensors Journal, Vol. 14, No 6, viewed 5 May 2014,
<http://ieeexplore.ieee.org/xpl/articleDetails.jsp?reload=true&arnumber=6728637>

iii
Appendices
Appendix A Experimental Data
Figure 9:Crazyflie propeller lift constant
Thrust
kw2
mass
90.7
27200
0.0480896
0.0405
89.3
26800
0.0466856
0.0396
88
26400
0.0453024
0.0387
86
25800
0.0432666
0.0378
84.3
25300
0.04160585
0.037
84.7
25400
0.0419354
0.0361
80.7
24200
0.0380666
0.0352
80.7
24200
0.0380666
0.0343
79.3
23800
0.0368186
0.0326
78
23400
0.0355914
0.0317
76.3
22900
0.03408665
0.0308
74
22200
0.0320346
0.0299
70.3
21100
0.02893865
0.0282
71.3
21400
0.0297674
0.0273
68.3
20500
0.02731625
0.0264
66.7
20000
0.026
0.0255
63
18900
0.02321865
0.0238
58.7
17600
0.0201344
0.022
52
15600
0.0158184
0.02
50
15000
0.014625
0.018
38
11400
0.0084474
0.016
30
9000
0.005265
0.014
25
7500
0.00365625
0.01

i
Figure 10:Crazyflie propeller drag coefficient

(First 19 rows 104 created by Crazyflie logging, showing Thrusts of 25-40 out the of 25-95 used)
Timestamp
Angle 25
Vel 25
Acc 25
Angle 30
Vel 30
Acc 30
Angle 35
Vel 35
Acc 35
Angle 40
102.2063
-0.1046
-0.2105
148.1783
0.088
-0.1498
110.7927
0.1148
-0.2317
80.8403
0.02
102.1016
0.1059
-0.314
148.2663
0.2378
-0.4102
110.9075
0.3465
-0.5081
80.565
0.04
102.2075
0.4199
-0.3053
148.5041
0.648
-0.3853
111.254
0.8546
-0.4755
80.4553
0.06
102.6274
0.7252
-0.2918
149.1521
1.0332
-0.375
112.1086
1.3301
-0.4562
80.9328
0.08
103.3526
1.0169
-0.2836
150.1853
1.4082
-0.3579
113.4387
1.7863
-0.4537
81.9859
0.1
104.3695
1.3005
-0.2739
151.5936
1.7662
-0.3356
115.2249
2.24
-0.401
83.5788
0.12
105.6701
1.5744
-0.2473
153.3597
2.1017
-3.041
117.4649
2.641
-0.3827
85.6733
0.14
107.2444
1.8217
-0.2453
155.4614
5.1427
2.1002
120.106
3.0237
-0.3736
88.2416
0.16
109.0661
2.067
-0.2391
160.6041
3.0425
-0.288
123.1297
3.3973
-0.3642
91.2593
0.18
111.1331
2.306
-0.2527
163.6466
3.3304
-0.2742
126.527
3.7615
-0.3246
94.7405
0.2
113.4391
2.5588
-0.2333
166.977
3.6046
-0.2742
130.2885
4.0861
-0.4865
98.6428
0.22
115.9979
2.7921
-0.2201
170.5816
3.8789
-0.2533
134.3746
4.5726
-0.7751
102.9635
0.24
118.79
3.0122
-0.2097
174.4605
4.1322
-0.2539
143.5197
5.3476
-0.5787
107.6854
0.26
121.8022
3.2219
-0.2046
4.3861
-0.2539
159.5626
5.9263
-0.2834
112.7741
0.28
125.0241
3.4265
-0.2067
178.5927
177.0141
4.6401
-0.2524
165.4889
6.2097
-0.2819
118.2054
0.3
128.4506
3.6332
-0.1981
4.8925
-0.2263
171.6986
6.4916
-0.2516
123.9889
0.32
132.0838
3.8313
-0.2005
5.1188
-0.2228
6.7432
-0.2516
130.095
0.34
135.9151
4.0318
-0.1944
5.3416
-0.2264
178.1902
175.0618
6.9948
-0.2371
136.4918
0.36
139.9469
4.2261
-0.1954
5.568
-0.2157
-168.067
7.2318
-0.2434
143.2147
-172.374
167.4815
162.3627
157.0211
Figure 11: thrust vs battery voltage

thrust=[25 35 45 55 65 75 85 95];
voltage0=[3.915 3.829 3.742 3.589 3.522 3.4 3.378 3.3254];

ii
Figure 12: Battery Compensated Lift

W
uncomp Thrust
W comp
mass
comp thrust
27200
0.0665856 21747.4226581869 0.0425655353
0.0405
26800
0.0646416 21459.228446311 0.0414448637
0.0396
26400
0.0627264 21171.7479056197 0.0403418618
0.0387
25800
0.0599076 20741.6747381877 0.0387195364
0.0378
25300
0.0576081 20384.1472073308 0.0373962112
0.037
25400
0.0580644 20455.5985898664 0.0376588362
0.0361
24200
0.0527076 19599.5028945229 0.0345726462
0.0352
24200
0.0527076 19599.5028945229 0.0345726462
0.0343
23800
0.0509796 19314.5689235933 0.0335747315
0.0326
23400
0.0492804 19029.712574888 0.0325916965
0.0317
22900
0.0471969 18673.6268797075 0.0313833907
0.0308
22200
0.0443556 18174.7957700512 0.0297290881
0.0299
21100
0.0400689 17389.241748148 0.0272147156
0.0282
21400
0.0412164 17603.7606799933 0.0278903151
0.0273
20500
0.0378225 16959.3954609747 0.0258858985
0.0264
20000
0.036 16600.2133744907 0.0248010376
0.0255
18900
0.0321489 15806.1266920201 0.0224850277
0.0238
17600
0.0278784 14859.0327730572 0.0198711769
0.022
15600
0.0219024 13378.3016287366 0.0161081059
0.02
15000
0.02025 12927.3911984411 0.0150405699
0.018
11400
0.0116964 10140.9930402538 0.0092555766
0.016
9000
0.00729 8190.9751892473 0.0060382867
0.014
7500
0.0050625 6928.0092382954 0.0043197581
0.01

iii
Figure 12: Crazyflie in Freefall

timestamp
acceleration(z)
9.8148925781
20
9.8166015625
40
9.8148925781
60
9.8104980469
80
9.8063476563
100
9.80390625
120
9.7975585937
140
9.7899902344
160
9.7846191406
180
9.7787597656
200
9.7721679687
220
9.7670410156
240
9.7609375
260
9.7526367187
280
9.7482421875
300
9.7406738281
320
9.7328613281
340
9.7262695312
360
9.7155273437
380
9.7011230469
400
9.6874511719
420
9.6737792969
440
9.6549804687
460
9.6395996094
480
9.6225097656
500
9.6051757812
520
9.5902832031
540
9.5780761719
560
9.5673339844
580
9.5490234375

iv
Figure 13: Thrust effect on ASL reading

(First 40 readings out of 3503 of logged data)
rel vel
rel dis
rel acc
0.0936514
0.0550819
0.0073739
0.0946641
0.0557053
0.0060802
0.0956183
0.0562881
0.0065977
0.0966018
0.056861
0.0064683
0.0976728
0.0574677
0.007762
0.0988218
0.0581235
0.0075032
0.0994742
0.0586634
0.0011643
0.1006037
0.059184
0.0138422
0.1032522
0.0602804
0.0213454
0.1086758
0.0626297
0.0507115
0.1176728
0.0668462
0.0688228
0.1277118
0.072464
0.0645537
0.1408374
0.0792745
0.1098318
0.1626485
0.0894893
0.1799483
0.1986076
0.1063833
0.2978008
0.2531451
0.132898
0.4267788
0.3245278
0.1699185
0.5216041
0.4058325
0.2149565
0.5586028
0.4892502
0.2636655
0.5496766
0.5697274
0.3122008
0.5195343
0.644557
0.3582331
0.4746442
0.7111003
0.4001799
0.4094437
0.7658909
0.4362463
0.3184994
0.8085297
0.4652453
0.2479948
0.844187
0.4885102
0.2257439
0.8780331
0.5091007
0.2239327
0.9101753
0.5286496
0.2031048
0.9399903
0.5470145
0.1930142
0.9683836
0.5642575
0.1842173
0.9915579
0.5795891
0.123674
0.5891514
-0.0115136
0.9857157
0.587743
-0.1782665
0.9533204
0.5741864
-0.2521345
0.9147128
0.5532653
-0.2608021
0.8138948
-0.4089263
0.7510906
0.4639392
-0.4254851
0.6903311
0.4273499
-0.3817594

v
0.633778
0.3925797
-0.369599
0.5770107
0.359053
-0.3846054
0.5165433
0.3244173
-0.4187581
Figure 14: Displacement and Velocity drift over time

(First 40 rows of 126 recorded by Crazyflie logging)
rel vel
rel dis
rel acc
0.0936514
0.0550819
0.0073739
0.0946641
0.0557053
0.0060802
0.0956183
0.0562881
0.0065977
0.0966018
0.056861
0.0064683
0.0976728
0.0574677
0.007762
0.0988218
0.0581235
0.0075032
0.0994742
0.0586634
0.0011643
0.1006037
0.059184
0.0138422
0.1032522
0.0602804
0.0213454
0.1086758
0.0626297
0.0507115
0.1176728
0.0668462
0.0688228
0.1277118
0.072464
0.0645537
0.1408374
0.0792745
0.1098318
0.1626485
0.0894893
0.1799483
0.1986076
0.1063833
0.2978008
0.2531451
0.132898
0.4267788
0.3245278
0.1699185
0.5216041
0.4058325
0.2149565
0.5586028
0.4892502
0.2636655
0.5496766
0.5697274
0.3122008
0.5195343
0.644557
0.3582331
0.4746442
0.7111003
0.4001799
0.4094437
0.7658909
0.4362463
0.3184994
0.8085297
0.4652453
0.2479948
0.844187
0.4885102
0.2257439
0.8780331
0.5091007
0.2239327
0.9101753
0.5286496
0.2031048
0.9399903
0.5470145
0.1930142
0.9683836
0.5642575
0.1842173
0.9915579
0.5795891
0.123674
0.5891514
-0.0115136
0.9857157
0.587743
-0.1782665
0.9533204
0.5741864
-0.2521345
0.9147128
0.5532653
-0.2608021

vi
0.8138948
-0.4089263
0.7510906
0.4639392
-0.4254851
0.6903311
0.4273499
-0.3817594
0.633778
0.3925797
-0.369599
0.5770107
0.359053
-0.3846054
0.5165433
0.3244173
-0.4187581
Figure 15: Barometer drift over time

(first 20 lines of 6,530 lines of logged data)
Min
Max
Range
Min
Max
Range
Min
Max
Range
1003.77563 1009.6365 5.8609008 25.332187 28.772811 3.4406242 52.580680 103.17162 50.590942

477
3564
7
6526
8896
37
8472
323
3828
relative
Rel
Timest
ASL
Rel
Pressure
relative
rel change Temp
relative
change
change
amp
144101 1009.57440186 0.9999384593
0.989398595 27.296562194
0.570935506 52.747222900 0.511257080
0.9486928945
0.0032919342
6
8
7
4
7
146101 1009.54998779 0.9999142782
0.985233012 27.302186965
0.572570317 53.058170318 0.514270965
0.9488883836
0.0094382403
5
9
9
6
8
148101 1009.5234375 0.9998879813
0.980702942 27.297813415
0.571299167 53.250663757 0.516136725
0.9487363807
0.0132431396
7
5
7
3
3
150101 1009.45343018 0.9998186422
0.968758137 27.293125152
3
6
152101 1009.52154541 0.9998861073
0.980380110 27.290937423
0.569300695 53.305271148 0.516666012
0.9484974054
0.0143225302
1
7
5
7
2
154101 1009.54437256 0.9999087166
0.984274929 27.298749923
52.985137939
0.9487689291 0.571571359
0.513563093 0.0079946542
4
7
5
156101 1009.63653564
158101 1009.53814697 0.9999025504

160101 1009.48095703 0.9998459063
0.94857344
0.569936547 53.795372009 0.521416357

0.0240100521
8
3
8
27.301250457
0.572298126 52.584308624 0.509678019
0.9488558352
0.000071708
8
6
3
8
0.983212705 27.304374694
0.573206170 53.180171966 0.515453477
0.9489644182
0.0118497717
3
8
3
6
4
0.97345483
27.290624618
0.569209780 53.665874481 0.520161191
0.9484865339
0.0214503542
5
3
2
6
162101 1009.51281738 0.9998774626
0.978890914 27.283124923
53.550159454 0.519039613
0.9482258817 0.567030032
0.0191630865
1
7
3
6
164101 1009.50939941 0.9998740772
0.978307732
4
166101 1009.60314941 0.9999669324
0.994303566 27.147188186
0.527520708 52.810878753 0.511874070
0.9435013961
0.0045501802
9
6
2
7
6
27.2421875 0.9468031003
0.555131777 53.411136627 0.517692122

0.0164151079
2
2
7
168101 1009.59747314 0.9999613103 0.993335069
27.046249389
0.498183358 52.715950012 0.510953965
0.9399932649
0.0026737823
6
3
2
5
170101 1009.55438232 0.9999186308 0.985982817
26.976249694
0.477838301 53.032405853 0.514021241
0.9375604233
8
7
3
4
172101 1009.5168457 0.9998814524 0.979578235
26.940000534
0.467302666 53.365074157 0.517245658
0.9363005826
0.0155046195
1
8
7
1
0.00892897
174101 1009.51031494 0.999874984
0.978463942 26.919687271
0.461398719 53.388389587
0.9355945945
0.517471645 0.0159654812
2
1
9
4
176101 1009.54315186 0.9999075075
0.984066650 26.914375305
0.459854823 53.094131469 0.514619522
0.9354099769
0.0101490622
8
2
9
7
4
178101 1009.59405518 0.999957925 0.992751889
26.923749923
0.462579509 52.791038513 0.511681767
0.9357357921
0.0041580104
7
3
2
3

vii
180101 1009.61627197 0.9999799297
0.996542567 26.941562652
0.467756688 52.580680847 0.509642857
0.936354874
4
6
7
2
2
182101 1009.54345703 0.9999078098
0.984118719 26.962812423
0.473932826 53.156604766 0.515225050
0.9370934105
0.0113839334
6
7
9
8
3

viii
Figure 16 Displacement and Velocity drift over time

(First 30 rows of 342 rows of data logged by Crazyflie)
timestamp acc.z
00132
00152
00172
00192
00212
00232
00252
00272
00292
00312
00332
00352
00372
00392
00412
00432
00452
00472
00492
00512
00532
00552
00572
00592
00612
00632
00652
00672
00692
00712
RelativeVelocity
RelativeDisplacement
1.041259766
1.041503906
1.037841797
1.03515625
1.037353516
1.039550781
1.039794922
1.038574219
1.035888672
1.038574219
1.035400391
1.034423828
1.038574219
1.040527344
1.039550781
1.038574219
1.037109375
1.036621094
1.039794922
1.037353516
1.035644531
1.037841797
1.038574219
0
0.005852597
0.011463495
0.01662552
0.021753017
0.027191271
0.032802168
0.038344008
0.04360962
0.048875231
0.054106314
0.059043903
0.064205929
0.069799561
0.075462252
0.080986827
0.08633876
0.091552578
0.096956304
0.102411823
0.107573848
0.112770402
0.118174128
RelativeAcceleration
0
0.08109405
0.000817825
0.081573896
0.001699305
0.0743762
0.002640605
0.069097889
0.003639041
0.073416507
0.004696145
0.077735125
0.005814604
0.078214971
0.006994991
0.075815739
0.00823539
0.070537428
0.009534267
0.075815739
0.01089143
0.069577735
0.012305057
0.067658349
0.013774765
0.075815739
0.015304198
0.079654511
0.016896134
0.077735125
0.018550191
0.075815739
0.020264643
0.07293666
0.022037765
0.071976967
0.023869844
0.078214971
0.025762221
0.073416507
0.027713556
0.070057582
0.02972241
0.0743762
0.031790125
0.075815739
1.037353516
1.040527344
1.041259766
1.039794922
1.038330078
1.038574219
1.036865234
0.123543325
0.129050636
0.134834176
0.140565924
0.146090499
0.151528754
0.156863422
0.03391766
0.03610559
0.038356216
0.040670784
0.043047856
0.045485802
0.047983568

ix
0.073416507
0.079654511
0.08109405
0.078214971
0.075335892
0.075815739
0.072456814
Appendix B MATLAB Program and Functions

QuadcopterMAIN.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%
%%
Quadcopter Controller Paramater Passing and main function
%%
%%
%%
%% By: William Hanna
william.hanna@bellaliant.net %%
%% Uses function:
%%
%%
%%
%%
ShowQuadcopter.m
%%
%%
PlotQuadcopter.m
%%
%%
QuadcopterDynamicCalculations.m
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% QuadcopterController.m
%%
%% Calls on SetQuadcopterParameters to setup variables/parameters %%
%% Calls on QuadcopterDynamicCalculations to calculate motion.
%%
%%
%%
%% Testing Mode: enter PWM's below and controller turned off
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global mass motordistance Ixx Iyy Izz drag lift qx qy qz qx2 qy2 qz2 wmax p1
p2 p3;
global t g x y z vx vy vz ax ay az psi theta phi vpsi vtheta vphi apsi atheta
aphi PWM;
global xArray yArray zArray vxArray vyArray vzArray axArray ayArray azArray;
global psiArray thetaArray phiArray vpsiArray vthetaArray vphiArray apsiArray
athetaArray aphiArray;
global PWM1Array PWM2Array PWM3Array PWM4Array tArray DisplayMode Mode
flighttime;
global fx fy fz fphi ftheta fpsi;
clear ALL;
clear GLOBAL;
close all;
clc;
%
%
% Initialise Matrices to ensure no redundant data
%
xArray=0;yArray=0;zArray=0;vxArray=0;vyArray=0;vzArray=0;axArray=0;ayArray=0;a
zArray=0;
psiArray=0;thetaArray=0;phiArray=0;vpsiArray=0;vthetaArray=0;vphiArray=0;apsiA
rray=0;athetaArray=0;aphiArray=0;
PWM1Array=0;PWM2Array=0;PWM3Array=0;PWM4Array=0;tArray=0;flighttime=0.1;
%
SetQuadcopterParameters();
% set test PWM setting
% hover PWM = 0.558
g=9.8;
PWM1=0.5785;
PWM2=0.5775;
PWM3=0.5785;

i
PWM4=0.5782;
%
QuadcopterController(PWM1,PWM2,PWM3,PWM4);
%
% END of MAIN function
ShowQuadcopter.m
function
ShowCopter(xx,yy,zz,phi,theta,psi,SX,SY,SZ,detail,motorradius,bodyradius,frame
radius,rodradius);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%
%%
%%
%% function
ShowCopter(xx,yy,xx,phi,theta,psi,SX,SY,SZ,detail,motorradius,bodyradius, %%
%%
frameradius,rodradius)
%%
%%
Written by:William Hanna
%%
%%
%%
%%
william.hanna@bellaliant.net
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%
[a,b,c]=sphere(detail);
ma=a*motorradius; mb=b*motorradius; mc=c*motorradius;
ba=a*bodyradius; bb=b*bodyradius; bc=c*bodyradius;
hold off;
m1x=frameradius*(cos(psi)*cos(theta));
m1y=frameradius*(sin(psi)*cos(theta));
m1z=frameradius*(sin(theta))*-1;
m2x=frameradius*((cos(psi)*sin(phi)*sin(theta))-(sin(psi)*cos(phi)));
m2y=frameradius*((sin(psi)*sin(phi)*sin(theta))+(cos(psi)*cos(phi)));
m2z=frameradius*(cos(theta)*sin(phi));
[a,b,c]=cylinder2P([rodradius,rodradius],10,[xx,yy,zz],[xx+m1x,yy+m1y,zz+m1z])
;
surf(a,b,c,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
hold on;
[a,b,c]=cylinder2P([rodradius,rodradius],10,[xx,yy,zz],[xx-m1x,yy-m1y,zzm1z]);
[a,b,c]=cylinder2P([rodradius,rodradius],10,[xx,yy,zz],[xx+m2x,yy+m2y,zz+m2z])
;
[a,b,c]=cylinder2P([rodradius,rodradius],10,[xx,yy,zz],[xx-m2x,yy-m2y,zzm2z]);
surf(ba+xx,bb+yy,bc+zz,'FaceColor','interp','EdgeColor','none','FaceLighting',
'phong')
surf(ma+m1x+xx,mb+m1y+yy,mc+m1z+zz,'FaceColor','interp','EdgeColor','none','Fa
ceLighting','phong');

ii
surf(ma+m2x+xx,mb+m2y+yy,mc+m2z+zz,'FaceColor','interp','EdgeColor','none','Fa
ceLighting','phong');
surf(ma-m1x+xx,mb-m1y+yy,mcm1z+zz,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
surf(ma-m2x+xx,mb-m2y+yy,mcm2z+zz,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
axis([-1*SX SX -1*SY SY -1*SZ SZ]);
view(-50,30)
camlight left
drawnow
hold off
end
Cylinder2P.m
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
%
CYLINDER:
A function to draw a N-sided cylinder based on the

generator curve in the vector R.
Usage:
[X, Y, Z] = cylinder(R, N)
Arguments:
R - The vector of radii used to define the radius of

the different segments of the cylinder.
N - The number of points around the circumference.
Returns:
X - The x-coordinates of each facet in the cylinder.

Y - The y-coordinates of each facet in the cylinder.
Z - The z-coordinates of each facet in the cylinder.
Author:
Date:
Modified:
Luigi Barone
9 September 2001
Per Sundqvist July 2004
function [X, Y, Z] = cylinder2P(R, N,r1,r2)

% The parametric surface will consist of a series of N-sided
% polygons with successive radii given by the array R.
% Z increases in equal sized steps from 0 to 1.
% Set up an array of angles for the polygon.
theta = linspace(0,2*pi,N);
m = length(R);
% Number of radius values

% supplied.
if m == 1
R = [R; R];
m = 2;
end
% Only one radius value supplied.

% Add a duplicate radius to make
% a cylinder.
X = zeros(m, N);
Y = zeros(m, N);
Z = zeros(m, N);
% Preallocate memory.

iii
v=(r2-r1)/sqrt((r2-r1)*(r2-r1)');
%Normalized vector;
%cylinder axis described by: r(t)=r1+v*t for 0<t<1
R2=rand(1,3);
%linear independent vector (of v)
x2=v-R2/(R2*v');
%orthogonal vector to v
x2=x2/sqrt(x2*x2');
%orthonormal vector to v
x3=cross(v,x2);
%vector orthonormal to v and x2
x3=x3/sqrt(x3*x3');
r1x=r1(1);r1y=r1(2);r1z=r1(3);
r2x=r2(1);r2y=r2(2);r2z=r2(3);
vx=v(1);vy=v(2);vz=v(3);
x2x=x2(1);x2y=x2(2);x2z=x2(3);
x3x=x3(1);x3y=x3(2);x3z=x3(3);
time=linspace(0,1,m);
for j = 1 : m
t=time(j);
X(j, :) = r1x+(r2x-r1x)*t+R(j)*cos(theta)*x2x+R(j)*sin(theta)*x3x;
Y(j, :) = r1y+(r2y-r1y)*t+R(j)*cos(theta)*x2y+R(j)*sin(theta)*x3y;
Z(j, :) = r1z+(r2z-r1z)*t+R(j)*cos(theta)*x2z+R(j)*sin(theta)*x3z;
end
%surf(X, Y, Z);
QuadcopterDynamicCalculations.m
function QuadcopterDynamicCalculations();
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
Quadcopter Dynamics Calculations
%%
%% Calculates the displacement, velocity, acceleration in linear
%%
%% and rotation given input parameters and PWM
%%
%%
%%
%%
%%
%% Uses fucntion:
%%
%%
%%
%% Uses file:
%%
%%
NA
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% QuadcopterDynamicCalculations.m
%%
%% Calculated the translational and rotational motion from the
%%
%% current known position and motion, given the 4 motor PWM inputs %%
%%
%%
%% Calls on PWMBatteryModifier to lower the used maximum angular
%%
%% velocity of the motor due to the battery voltage drop when under%%
%% different loadings.
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% position,motion and PWM are stored in Global variable for ease %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

iv
p2 p3;
global t dt g x y z vx vy vz ax ay az phi theta psi vphi vtheta vpsi aphi
atheta apsi PWM;
%%
%% Get modified motor speed, given maximum speed and motor PWM's
Bwmax=PWMBatteryModifier(wmax,PWM);
%%
%% Set each motor angular velocity
w1=PWM(1)*Bwmax;
w2=PWM(2)*Bwmax;
w3=PWM(3)*Bwmax;
w4=PWM(4)*Bwmax;
%%
%% Calculate accelerations in 3 directions
%%
%% NOTE!!!!!!!!!!!!!!!!!!!! ay and ax need to have gravitational componenet
added
%%
Fz=(((w1^2)*lift)+((w2^2)*lift)+((w3^2)*lift)+((w4^2)*lift))*cos(theta)*cos(ph
i);
Fy=(((PWM(1)+PWM(2)+PWM(3)+PWM(4))*(Bwmax^2)*lift)*sin(phi));
% y direction
caused by motors 2%4
Fx=(((PWM(1)+PWM(2)+PWM(3)+PWM(4))*(Bwmax^2)*lift)*sin(theta));
ax=(Fx/mass)+(g*sin(phi));
ay=(Fy/mass)+(g*sin(theta));
az=(Fz/mass)-(g*cos(theta)*cos(phi));
%%
%% Air resistance Model, constants for air restisance prop to v and v^2
%%
arx=abs(qx*vx)+(qx2*vx^2);
ary=abs(qy*vy)+(qy2*vy^2);
arz=abs(qz*vz)+(qz2*vz^2);
if vx>0
ax=ax+arx;
else
ax=ax-arx;
end
if vy>0
ay=ay+ary;
else
ay=ay-ary;
end
if vx>0
az=az+arz;
else
az=az-arz;
end
%%
%% Update displacement and velocity values using basic Newtons Laws
%%
%% Displacements
z=z+(vz*dt)+0.5*(az*dt^2);
%% NOTE!!!!!!!!!!!!! need to adjust for rotation as the output is in the E
frame
y=y+(vy*dt)+0.5*(ay*dt^2);

v
x=x+(vx*dt)+0.5*(ax*dt^2);
% Velocities
vz=vz+(az*dt);
vy=vy+(ay*dt);
vx=vx+(ax*dt);
%%
%% Calculate torques acting or quadcopter
%%
apsi=(((w1^2)*drag)-((w2^2)*drag)+((w3^2)*drag)-((w4^2)*drag))/Izz;
%acceleration in psi
atheta=((PWM(1)-PWM(3))*(Bwmax^2)*lift*motordistance)/Iyy;
aphi=((PWM(2)-PWM(4))*(Bwmax^2)*lift*motordistance)/Ixx;
%%
%% Update angular displacement and velocity using basic Newtons Laws
%%
psi=psi+(vpsi*dt)+0.5*(apsi*(dt^2));
theta=theta+(vtheta*dt)+0.5*(atheta*(dt^2));
phi=phi+(vphi*dt)+0.5*(aphi*(dt^2));
vpsi=vpsi+(apsi*dt);
vtheta=vtheta+(atheta*dt);
vphi=vphi+(aphi*dt);
%% END of QuadcopterDynamicCalculations.m
function [wout] = PWMBatteryModifier(win,PWMin)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%
%% funtion PWMBatteryModifier - modify non-linear PWM for battery %%
%%
load using 3rd order polynomial
%%
%%
approximation
%%
%%
%%
%% TIME variation to be added later.
%%
%%
%%
%%
Written by William Hanna
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% put input arguments into variables for computations
%
% To save on calculation times these vairables have been manually entered
% into the function and the calculations commented out
%
%battpoly1 =
%
%
1.2803e-07
%
%
%battpoly2 =
%
% 3.6371e-05

vi
%
%
%battpoly3 =
%
%
-0.0132
%
%
%battpoly4 =
%
%
% 4.0027
%thrust=[25 35 45 55 65 75 85 95];
%voltage2=[3.597 3.514 3.366 3.349 3.250 3.215 3.195 3.134];
%voltage1=[3.715 3.634 3.490 3.447 3.347 3.280 3.260 3.181];
%voltage3=[3.516 3.486 3.325 3.276 3.119 3.183 3.154 3.051];
%voltage0=[3.915 3.829 3.742 3.589 3.522 3.4 3.378 3.3254];
%order=3;
%%
% Find a polynomial to fit each curve
%p0= polyfit(thrust,voltage0,order);
%p1= polyfit(thrust,voltage1,order);
%p2 = polyfit(thrust,voltage2,order);
%p3 = polyfit(thrust,voltage3,order);
%
% Find the average value of each polynomial coefficient
%battpoly1=(p0(1)+p1(1)+p2(1)+p3(1))/4
%battpoly2=(p0(2)+p1(2)+p2(2)+p3(2))/4
%battpoly3=(p0(3)+p1(3)+p2(3)+p3(3))/4
%battpoly4=(p0(4)+p1(4)+p2(4)+p3(4))/4
%
% output result as speed in angular velocity
%
PWM=PWMin*100;
PWMavg=(PWM(1)+PWM(2)+PWM(3)+PWM(4))/4;
%
wout=(battploy4+(battpoly3*PWMavg)+(battpoly2*(PWMavg^2))+(battpoly1*(PWMavg^3
)))/battpoly4*win;
%
wout=(4.0027-(0.0132*PWMavg)+(3.6371e-05*(PWMavg^2))+(1.2803e07*(PWMavg^3)))/4.0027*win;
PlotQuadcopter.m
function PlotQuadcopter(inputparameter)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
PlotQuadcopter
%%
%% Plots graphs of Selected Quadcopter parameters for the flight %%
%%
%%
%%
%%
%% Uses fucntion:
%%
%%
NA
%%

vii
%% Uses file:
%%
%%
NA
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PlotQuadcopter.m
%%
%% Plots displacement,velocity and acceleration for translational %%
%% and rotational motion. Also plots PWM for each motor vs time
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Graph Display via DisplayMode:
%%
%% Display mode: {A B C D E F G}
%%
%% Set Flags A-G to 1 for display, 0 for no display
%%
%% A: displacement
B:velocity
C:acceleration
%%
%% D:angular displacement
E:angular velocity F:angular accel
%%
%% G:PWM %
%%
%%
%%
%% Example: to display all linear motion and PWM:
%%
%% DisplayMode=1110001
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Global variables for array storage for plotting
global PWM1Array PWM2Array PWM3Array PWM4Array tArray DisplayMode Mode
flighttime;
%
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
Scale output graphs for 80% resolution
%%
%%
and same axes for group
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% Default min and max axis values, incase min and max are zeros
%
% scale displacement
if DisplayMode>999999
DisplayMode=DisplayMode-1000000; %remove displacement flag
temparray=[xArray;yArray;zArray];
displacementplus=max(temparray(:));
displacementminus=min(temparray(:));
if displacementplus > 0 && displacementminus > 0
displacementplus=displacementplus+(0.1*(displacementplusdisplacementminus));
displacementminus=displacementminus-(0.1*(displacementplusdisplacementminus));
elseif displacementplus <0 && displacementminus<0
displacementplus=displacementplus+(0.1*(displacementplusdisplacementminus));
displacementminus=displacementminus-(0.1*(displacementplusdisplacementminus));
elseif displacementplus==0 && displacementminus==0

viii
displacementplus=1;
displacementminus=0;
else
displacementplus=displacementplus*1.1;
displacementminus=displacementminus*1.1;
end
%
if Mode ==0
figure;
subplot(1, 3, 1);
plot(tArray,xArray);
title('x displacement');
xlabel('seconds');
ylabel('displacement(m)');
axis([0 flighttime displacementminus displacementplus]);
grid MINOR;
%
subplot(1, 3, 2);
plot(tArray,yArray);
title('y displacement');
xlabel('time(s)');
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,zArray);
title('z displacement');
xlabel('time(s)');
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,xArray,'-',tArray,yArray,'+',tArray,zArray,':');
title('Displacement');
xlabel('time(s)');
legend('x','y','z');
grid MINOR;
else
end
end
if DisplayMode>99999
DisplayMode=DisplayMode-100000; %remove velocity flag%
% scale velocity
%
temparray=[vxArray;vyArray;vzArray];
velocityplus=max(temparray(:));
velocityminus=min(temparray(:));
if velocityplus > 0 && velocityminus > 0
velocityplus=velocityplus+(0.1*(velocityplus-velocityminus));

ix
velocityminus=velocityminus-(0.1*(velocityplus-velocityminus));
elseif velocityplus <0 && velocityminus<0
velocityplus=velocityplus+(0.1*(velocityplus-velocityminus));
velocityminus=velocityminus-(0.1*(velocityplus-velocityminus));
elseif velocityplus==0 && velocityminus==0
velocityplus=1;
velocityminus=0;
else
velocityplus=velocityplus*1.1;
velocityminus=velocityminus*1.1;
end
%
if Mode ==0
figure;
subplot(1, 3, 1);
plot(tArray,vxArray);
title('x velocity');
xlabel('time(s)');
ylabel('velocity(m/s)');
axis([0 flighttime velocityminus velocityplus]);
grid MINOR;
%
subplot(1, 3, 2);
plot(tArray,vyArray);
title('y velocity');
xlabel('time(s)');
ylabel('velocty(m/s)');
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,vzArray);
title('z velocity');
xlabel('time(s)');
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,vxArray,'-',tArray,vyArray,'+',tArray,vzArray,':');
title('Velocity');
xlabel('time(s)');
grid MINOR;
else
end
end
if DisplayMode>9999
DisplayMode=DisplayMode-10000; %remove acceleration flag%%
% scale acceleration
%
temparray=[axArray;ayArray;azArray];

x
accelerationplus=max(temparray(:));
accelerationminus=min(temparray(:));
if accelerationplus > 0 && accelerationminus > 0
accelerationplus=accelerationplus+(0.1*(accelerationplusaccelerationminus));
accelerationminus=accelerationminus-(0.1*(accelerationplusaccelerationminus));
elseif accelerationplus <0 && accelerationminus<0
accelerationplus=accelerationplus+(0.1*(accelerationplusaccelerationminus));
accelerationminus=accelerationminus-(0.1*(accelerationplusaccelerationminus));
elseif accelerationplus==0 && accelerationminus==0
accelerationplus=1;
accelerationminus=0;
else
accelerationplus=accelerationplus*1.1;
accelerationminus=accelerationminus*1.1;
end
if Mode ==0
figure;
subplot(1, 3, 1);
plot(tArray,axArray);
title('x acceleration');
xlabel('time(s)');
ylabel('acceleration(m/s/s)');
axis([0 flighttime accelerationminus accelerationplus]);
grid MINOR;
%
subplot(1, 3, 2);
plot(tArray,ayArray);
title('y acceleration');
xlabel('time(s)');
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,azArray);
title('z acceleration');
xlabel('time(s)');
grid MINOR;
elseif Mode==1
figure;
plot(tArray,axArray,'-',tArray,ayArray,'+',tArray,azArray,':');
title('Acceleration');
xlabel('time(s)');
grid MINOR;
else

xi
end
end
if DisplayMode>999
DisplayMode=DisplayMode-1000; %remove angular displacement flag%%
% scale angular displacment
%
temparray=[psiArray;thetaArray;phiArray];
angleplus=max(temparray(:));
angleminus=min(temparray(:));
if angleplus > 0 && angleminus > 0
angleplus=angleplus+(0.1*(angleplus-angleminus));
angleminus=angleminus-(0.1*(angleplus-angleminus));
elseif angleplus <0 && angleminus<0
angleplus=angleplus+(0.1*(angleplus-angleminus));
angleminus=angleminus-(0.1*(angleplus-angleminus));
elseif angleplus==0 && angleminus==0
angleplus=1;
angleminus=0;
else
angleplus=angleplus*1.1;
angleminus=angleminus*1.1;
end
if Mode ==0
figure;
subplot(1, 3, 1);
plot(tArray,psiArray);
title('yaw displacement');
xlabel('time(s)');
ylabel('displacement(rad)');
axis([0 flighttime angleminus angleplus]);
grid MINOR;
%
subplot(1, 3, 2);
plot(tArray,thetaArray);
title('roll displacement');
xlabel('time(s)');
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,phiArray);
title('pitch displacement');
xlabel('time(s)');
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,psiArray,'-',tArray,thetaArray,'+',tArray,phiArray,':');
title('Angular Displacement vs Time');
xlabel('time(s)');

xii
legend('yaw','roll','pitch');
grid MINOR;
else
end
end
if DisplayMode>99
DisplayMode=DisplayMode-100; %remove angular velocity flag%%
% scale angular velocity
%
temparray=[vpsiArray;vthetaArray;vphiArray];
angvelplus=max(temparray(:));
angvelminus=min(temparray(:));
if angvelplus > 0 && angvelminus > 0
angvelplus=angvelplus+(0.1*(angvelplus-angvelminus));
angvelminus=angvelminus-(0.1*(angvelplus-angvelminus));
elseif angvelplus <0 && angvelminus<0
angvelplus=angvelplus+(0.1*(angvelplus-angvelminus));
angvelminus=angvelminus-(0.1*(angvelplus-angvelminus));
elseif angvelplus==0 && angvelminus==0
angvelplus=1;
angvelminus=0;
else
angvelplus=angvelplus*1.1;
angvelminus=angvelminus*1.1;
end
if Mode ==0
figure;
subplot(1, 3, 1);
plot(tArray,vpsiArray);
title('yaw velcoity');
xlabel('time(s)');
ylabel('velocity(rad/s)');
axis([0 flighttime angvelminus angvelplus]);
grid MINOR;%
subplot(1, 3, 2);
plot(tArray,vthetaArray);
title('roll velocity');
xlabel('time(s)');
ylabel('velocty(rad/s)');
grid MINOR;
%
subplot(1,3 , 3);
plot(tArray,vphiArray);
title('pitch velocity');
xlabel('time(s)');
ylabel('velocity(rad/s');
grid MINOR;
elseif Mode ==1
figure;

xiii
plot(tArray,vpsiArray,'',tArray,vthetaArray,'+',tArray,vphiArray,':');
title('Angular Velcoity');
xlabel('time(s)');
ylabel('velocity(rad/s)');
grid MINOR;%
else
end
end
if DisplayMode>9
DisplayMode=DisplayMode-10; %remove angular acceleration flag%%
%
% scale angular acceleration
%
temparray=[apsiArray;athetaArray;aphiArray];
angaccplus=max(temparray(:));
angaccminus=min(temparray(:));
if angaccplus > 0 && angaccminus > 0
angaccplus=angaccplus+(0.1*(angaccplus-angaccminus));
angaccminus=angaccminus-(0.1*(angaccplus-angaccminus));
elseif angaccplus <0 && angaccminus<0
angaccplus=angaccplus+(0.1*(angaccplus-angaccminus));
angaccminus=angaccminus-(0.1*(angaccplus-angaccminus));
elseif angaccplus==0 && angaccminus==0
angaccplus=1;
angaccminus=0;
else
angaccplus=angaccplus*1.1;
angaccminus=angaccminus*1.1;
end
if Mode ==0
figure;
subplot(1, 3, 1);
plot(tArray,apsiArray);
title('yaw acceleration');
xlabel('time(s)');
ylabel('acceleration(rad/s/s)');
axis([0 flighttime angaccminus angaccplus]);
grid MINOR;
%
subplot(1, 3, 2);
plot(tArray,athetaArray);
title('roll acceleration');
xlabel('time(s)');
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,aphiArray);
title('pitch acceleration');

xiv
xlabel('time(s)');
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,apsiArray,'',tArray,athetaArray,'+',tArray,aphiArray,':');
title('Angular Acceleration');
xlabel('time(s)');
grid MINOR;
else
end
end
if DisplayMode>1
figure;
subplot(1, 4, 1);
plot(tArray,PWM1Array);
title('Motor 1 PWM');
xlabel('time(s)');
ylabel('duty cyle');
axis([0 flighttime 0 1]);
%
subplot(1, 4, 2);
xlabel('time(s)');
ylabel('duty cycle');
%
subplot(1, 4, 3);
xlabel('time(s)');
%
subplot(1, 4, 4);
xlabel('time(s)');
end
titleheader.m
function titleHeader();
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xv
%%
%%
%%
Display a title header to make user interface look nice
%%
%%
%%
%%
%%
%% Uses function:
%%
%%
NA
%%
%% Uses files:
%%
%%
NA
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% titleHeader.m
%%
%% Clears the screen and prints a title header
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
msg0='################################################################\n';
msg1='##
Quadcopter Mathematical Model and Controller Design
##\n';
msg2='##
Written By:William Hanna
##\n';
msg3='##
##\n';
msg4='## CrazyFlie Quadcopter values are set as default parameters ##\n';
msg5='##
##\n';
msg6='## Model can run as either implementation of controller or
##\n';
msg7='## simulation using PWM inputs from data file.
##\n';
msg8='################################################################\n\n';
fprintf(msg0);fprintf(msg1);fprintf(msg2);fprintf(msg3);fprintf(msg4);
fprintf(msg5);fprintf(msg6);fprintf(msg7);fprintf(msg8);
SetQuadcopterParameters.m
function SetQuadcopterParameters();
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%
%%
Set Quadcopter paramaters for use by other functions
%%
%%
NOTE:The default values for the crazyflie are stored in:
%%
%%
CF.dat
%%
%%
%%
%%
%%
%% Uses function:
%%
%%
titleHeader.m
%%
%% Uses files:
%%
%%
CF.dat
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% SetQuadcopterParameters.m
%%
%% Checks with the uses to see if they want to use Crazyflie vlaues%%
%% Uses may edit Crazyflie values for a one-off run or enter ALL
%%
%% new values.
%%
%% Uses titleHeader to print a title screen
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CF.dat
%%
%% Parameter storage format:
%%
%% mass,motordistance,Ixx,Iyy,Izz,kp,bp,qx,qy,qz,qx2,qy2,qz2,p1... %%
%% ,p2,p3,wmax
%%
%%
%%
%% ALL units are in SI units unless otherwise specified (MKS)
%%

xvi
%% mass
:mass of the quadcopter
%%
%% motordistance :distance from COG to motor centre
%%
%% Ixx
:Moment of inertia about X axis
%%
%% Iyy
:Moment of inertia about Y axis
%%
%% Izz
:Moment of inertia about Z axis
%%
%% drag,lift
:propeller lift and drag constants respectively %%
%% qx,qy,qz
:air resistance constants for proportional to velocity
%% qx2,qy2,qz2 :air resistance constants for prop to velocity squared
%% p1,p2,p3
:battery polynomial
%%
%% wmax
:motor maximum rpm (rad/s)
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
p2 p3;
global flighttime DisplayMode Mode PWMMode timestamp PWM1Array PWM2Array
PWM3Array PWM4Array ;
%%
%% Load in default data from CF.dat
Data=importdata('CF.dat');
mass=Data(1);motordistance=Data(2);
Ixx=Data(3);Iyy=Data(4);Izz=Data(5);
drag=Data(6);lift=Data(7);
qx=Data(8);qy=Data(9);qz=Data(10);
qx2=Data(11);qy2=Data(12);qz2=Data(13);
p1=Data(14);p2=Data(15);p3=Data(16);
wmax=Data(17);
%%
%% Get User Input - Does Uses want to use CrazyFlie values or enter new ones
titleHeader;
msg='Quadcopter Parameter Input \n\n';
fprintf(msg)
prompt='Are you using the CrazyFlie Quadcopter (Y/N) [Y]?';
str = input(prompt,'s');
if isempty(str)
str = 'Y';
end
titleHeader;
if (str=='n')|(str=='N')
%% User has selected to enter ALL new values:
msg='Function not created yet - too bad - using CrazyFlie defaults \n\n';
fprintf(msg)
else
%% Use has selected to use CrazyFlie parameters
msg='Default Values are stored in CF.dat\n\n';
fprintf(msg)
prompt='Do you want to use Defulat CrazyFlie data (Y/N) [Y]?';
if isempty(str)
str = 'Y';
end
ele='-1';
titleHeader;
msg='Entering New CrazyFlie Parameters\n\n';
fprintf(msg)
prompt='Which array element do you wish to change (0 to exit)?';
ele = input(prompt);

xvii
while ele >0,

%% Use has selected to enter new CrazyFlie parameters
prompt='Change to Value(floating point):';
val = input(prompt);
Data(ele)=val;
titleHeader;
msg='\nEntering New CrazyFlie Parameters\n\n';
fprintf(msg)
prompt='Which array element do you wish to change (0 to exit)?';
ele = input(prompt);
end
mass=Data(1);motordistance=Data(2);
Ixx=Data(3);Iyy=Data(4);Izz=Data(5);
drag=Data(6);lift=Data(7);
qx=Data(8);qy=Data(9);qz=Data(10);
qx2=Data(11);qy2=Data(12);qz2=Data(13);
p1=Data(14);p2=Data(15);p3=Data(16);
wmax=Data(17);
end
end
titleHeader;
msg='Quadcopter Graph Output Mode \n\n';
fprintf(msg)
prompt='Are you Using default Display mode (Display All grpahs) (Y/N) [Y]?';
if isempty(str)
str = 'Y';
end
prompt='\nNew Display mode (ABCDEFG):';
DisplayMode = input(prompt);
else
DisplayMode=1111111;
end
titleHeader;
prompt='Create graphs with all inputs on the one graph(Y/N) [Y]?';
if isempty(str)
str = 'Y';
end
Mode=0;
else
Mode=1;
end
PWMMode=0;
titleHeader;
prompt='Run off of input data file (PWM%) (Y/N) [N]?';
str=input(prompt,'s');
if isempty(str)
str='N';
end
if (str=='y')|(str=='Y')
%% Get PWM data from file: PWMlog.dat
PWMMode=1;
filename='PWMlog.dat'

xviii
PWM=csvread(filename);
timestamp=(PWM(:,1));
PWM1Array=PWM(:,2);
PWM2Array=PWM(:,3);
PWM3Array=PWM(:,4);
PWM4Array=PWM(:,5);
else
prompt='\nFlight Time of 1 second (Y/N) [Y]?';
if isempty(str)
str = 'Y';
end
prompt='\nNew Flifht time (sec):';
flighttime = input(prompt);
else
flighttime=1;
end
end
%% END OF FUNCTION SetQuaCopterParameters
%%
%% to add, if PWMMode=1, get PWM data and let controller know not to run
function QuadcopterController(PWM1,PWM2,PWM3,PWM4)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
QuadcopterController
%%
%% Calculates controller reaction given current and desired values %%
%%
%%
%%
%%
%% Uses fucntion:
%%
%%
QuadcopterDynamicCalcultoins.m
%%
%%
ShowQuadcopter.m
%%
%%
PlotQuadcopter.m
%%
%% Uses file:
%%
%%
NA
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% QuadcopterController.m
%%
%% Implements the desired control algorithm
%%
%% Requires a setpoint and current point
%%
%%
%%
%% Calls on QuadcopterDynamicCalculations to work out motion
%%
%%
%%
%% Calls on ShowQuadcopter to provide a visual display
%%
%%
%%
%% Calls on PlotQuadcopter to show various graphs of variables
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xix
% IF PWMMode=1, running off of input data for PWM, not controll values
%
% Global variables for array storage for plotting
p2 p3;
global t dt g x y z vx vy vz ax ay az psi theta phi vpsi vtheta vphi apsi
atheta aphi PWM;
global PWM1Array PWM2Array PWM3Array PWM4Array tArray Mode PWMMode timestamp
flighttime;
global fx fy fz fphi ftheta fpsi;
%
frameradius=1;bodyradius=0.2;motorradius=0.1;rodradius=0.01;detail=10;sx=5;sy=
5;sz=5;
%
% Set parameters and constants
%
t=0.005;dt=t;g=9.8;
PWM=[PWM1,PWM2,PWM3,PWM4,wmax,motordistance,t];
%
% Set initial conditions of Quadcopter
%
x=1;y=1;z=1;psi=0;theta=0;phi=0;
vx=0;vy=0;vz=0;ax=0;ay=0;az=0;
vphi=0;vtheta=0;vpsi=0;aphi=0;atheta=0;apsi=0;
%
% Set required final position & motion
%
fx=0;fy=0;fz=0;fpsi=0;fptheta=0;fphi=0;
fvx=0;fvy=0;fvz=0;fax=0;fay=0;faz=0;
fvpsi=0;fvtheta=0;fvphi=0;fapsi=0;fatheta=0;faphi=0;
% Initialise Quadcopter Variables for current location at start of program
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%
%%
Implementation of Quadcopter Control Theory
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% GOAL: To move from initlocation to setpoint and stay there
%
% Final roll,pitch and yaw are irelevant, want x,y,z=0 and no motion
%
% METHOD:
% 1. Calculate vector from current position to setpoint
%
% 2. Use controller to set PWM for motors
%
% 3. Calculate motion
%
% 4. Repeat 1 to 3
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%

xx
% Heading is the distance, bearing and elevation of the quadcopter the

% quadcopters point of view
% heading=quadcopterHeading(setpoint,point);
% NOTE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
%% ####################################
%% need to change reference frames as all the displacments etc are in CF frame
%% #####################################
%translate Forces and torques into equations of motion and direction
%
if PWMMode ==1
lt=length(timestamp);
flighttime=(timestamp(lt)-timestamp(1))/1000;
dt=(timestamp(2)-timestamp(1))/1000;
t=dt;
end
it=1;
for loop = t:dt:flighttime
% returns motion in R and A reference frames
if PWMMode==1
PWM=[PWM1Array(it),PWM2Array(it),PWM3Array(it),PWM4Array(it),wmax,motordistanc
e,t];
else
%% IMPLEMENT CONTROLLER for ALTITUDE First
%% PID values, Kp, Ki and Kd for proportional, integral and derivative
% Kp=5.5;Ki=0.1;Kd=0.2;
% error=z-fz;
% integral=((z+oldz)/2)*dt;
% derivative=(z-oldz)/dt;
% output=(Kp*error)+(Ki*intergral)+(Kd*derivative);
% zold=z;
%
%% IMPLEMENT CONTROLLER for ATTITUDE
%
end
QuadcopterDynamicCalculations();
ShowQuadcopter(x,y,z,phi,theta,psi,sx,sy,sz,detail,motorradius,bodyradius,fram
eradius,rodradius)
% Update storage arrays for plotting
xArray(it)=x;
yArray(it)=y;
zArray(it)=z;
vxArray(it)=vx;
vyArray(it)=vy;
vzArray(it)=vz;
axArray(it)=ax;
ayArray(it)=ay;
azArray(it)=az;
psiArray(it)=psi;
thetaArray(it)=theta;
phiArray(it)=phi;
vpsiArray(it)=vpsi;
vthetaArray(it)=vtheta;
vphiArray(it)=vphi;
apsiArray(it)=apsi;

xxi
athetaArray(it)=atheta;
aphiArray(it)=aphi;
PWM1Array(it)=PWM1;
PWM2Array(it)=PWM2;
PWM3Array(it)=PWM3;
PWM4Array(it)=PWM4;
tArray(it)=loop;
it=it+1;
end
PlotQuadcopter;
Figure 16: Pendulum degrading

MATLAB code for calculation of air resistance parameter:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%
%%
Pendulum Air Resistance Calculator For CrazyFlie Quadcopter
%%
%%
%%
%%
By: William Hanna s210566
Date: Feb 2014
%%
%%
%%
%%
Thesis: Control of a Unmanned Aerial Vehicle
%%
%%
Charles Darwin University
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
period=2.148;
% experiment value for period in sec
omega=2.93;
% frequency in rad/s (from period)
length=1.165;
% pendulum length in metres
mass=22e-3;
% crazyflie mass in kilograms
g=9.8;
% acceleration due to gravity m/s/s
%
% angle array values are average values from experiment
angle=[35 30.5 27.8 24.8 23 21 19.8 18 16.8 15.8 15 14]; %in degrees
%
angrad=angle*pi/180;
% convert angle to rads for calculations
chord=angrad*length;
% calculate the distance travelled in meters
height=length-(length*cos(angrad)); %calc max height in metres
PE=mass*g*height;
% potential energy at max height
maxV=sqrt(g*height)
% max velocity based on PE and no resistance
%
avg=0;
% average b value - initialize avg variable
for loop = 1:11
work(loop)=PE(loop)-PE(loop+1); %work is change in potentail energy
Ffunc = @(t) (((maxV(loop)+maxV(loop+1))/2)*sin(omega*t)).^2;
int=integral(Ffunc,0,(period/2));
% integrat the velocity over half
oscillation
b(loop)=((period/2)*work(loop))/((chord(loop)+chord(loop+1))*int); %calc
air restistance
avg=avg+b(loop);
% update average value
end
%
% PLOT results and average
avg=avg/11
% calculate average and display on screen
str=num2str(avg);
% convert average to a string for plotting
a=[avg,avg,avg,avg,avg,avg,avg,avg,avg,avg,avg]; % create array of avg values

xxii
ti=[1:1:11];
% set X variable value
plot(ti,b,ti,a);
% plot the air restistance and average value
axis([1 11 0 (max(b)*1.1)]);
title('Air Resistance Coefficient');
xlabel('Oscillation');
ylabel('air resistance');
text(6,avg,str,'Fontsize',18);

xxiii
Appendix C Crazy Flie Construction

Crazy Flie Motor Properties (http://www.epictinker.com/Crazyflie-Spare-Motorp/mot01313m.htm)
Motor Specifications:
Diameter: 6 mm
Length: 15 mm
Shaft length: 3.5 mm
Shaft diameter: 0.8 mm
Weight: 1.7 g
Kv: 12000 rpm/V
Rated voltage: 4.2 V
Rated current: 810 mA
Wire length: 67 mm

i
Appendix D Crazy Flie Firmware modifications

Firmware Modification to Stabilizer.c needed for getting experimental data
Mod I used for lift calculation, all motors running with compensation removed
Mod II- used for drag modelling in the counter-clockwise direction. Only M2 and M4 used
Mod III used for drag modelling in clock wise direction. Only M1 and M3 used.
Mod IV used for rotational air resistance. Only a single motor running specified by submark.
Mod I
#ifdef QUAD_FORMATION_X
roll = roll >> 1;
pitch = pitch >> 1;
motorPowerM1 = limitThrust(thrust);
#else // QUAD_FORMATION_NORMAL
#endif
Mod II
i
roll = roll >> 1;

pitch = pitch >> 1;
motorPowerM1 = limitThrust(0);
#endif
Mod III
roll = roll >> 1;
pitch = pitch >> 1;
ii
#endif
ModIV-I
roll = roll >> 1;
pitch = pitch >> 1;
#endif

iii
Appendix E Crazy Flie Client modifications

PC Client modifications made to take-off and landing mode. Modifications also used for
experiements with modified firmware.
Note: ony modifications to Commander.py are shown due to the length of code. Other .py fiels
are shown in full.
Commander.py
#######################################
## write over existingly calculated values for all modes, change to except mode 4 once it works
#######################################
# set up for hover
pitch=0
roll=0
yaw=0
####
### set up if statements for different modes
##GuiConfig().set("Fmode", 0)
fmode=GuiConfig().get("Fmode")
timestamp=GuiConfig().get("timeStamp")
totimer=GuiConfig().get("toTimer")
totime=GuiConfig().get("toTime")
warmUpTime=GuiConfig().get("warmUpTime")
# Mode 0 : waiting
if fmode ==0:
pitch=0
roll=0
yaw=0

i
thrust=0 #thrust is barometer asllong

GuiConfig().set('toTime',timestamp)
#print(timestamp)
# Mode 1 : warmup #skipping mode 2 for now
if fmode ==1:
pitch=0
roll=0
yaw=0
warmupthrust=GuiConfig().get("warmUpThrust")
takeoff=GuiConfig().get("takeOff")
takeoffstop=GuiConfig().get("takeOffStop")
ground=GuiConfig().get("ground")
altitude=ground+takeoff
GuiConfig().set("altitude",altitude)
stoptime=totime+warmUpTime
#print("Hi, in Mode 1")
if (timestamp >stoptime):
GuiConfig().set("Fmode",2)
print("going to enter mode 2")
else:
thrust=15000+(warmupthrust*100);
# Mode 2 : take off until takeoff height
if fmode==2:
pitch=0
roll=0
yaw=0

ii
takeoffthrust=GuiConfig().get("takeOffThrust")
altitude=GuiConfig().get("altitude")
altRequired=ground+takeoff
stoptime=totime+totimer
if (timestamp > stoptime):
GuiConfig().set("Fmode",9)
print("going to enter mode 9 due to take off timer")
if (altitude>altRequired):
GuiConfig().set("Fmode",3) #CHANGE TO MODE 3 WHEN DONE TESTING
thrust=16000+(takeoffthrust*100)
# Mode 3 : Hovering at takeoff height
if fmode==3:
pitch=0
roll=0
yaw=0
altRequired=ground+takeoff
#print(altitude)
# print("In Mode 3")
thrust=1000+int(altRequired*10) #set thurst to 10x altitude(so accurate to 0.1 when firmware
# Mode 4: Normal Flight
# Mode 5: NA
# Mode 6: NA

iii
# Mode 7: NA
# Mode 8: landing
# if fmode==8:
# pitch=0
# roll=0
# yaw=0
# # setup countdown for landing
# landrate=GuiConfig().get("landRate")
# landtime=GuiConfig().get("landTime")
# landtimer=GuiConfig().get("landTimer")
# baro=GuiConfig().get("baro")
# altitude=GuiConfig().get("tafeOff")
# ground=GuiConfig().get("ground") #NEED TO SET ALTITUDE AND GROUND
# error=GuiConfig().get("landingError")
# thrust=altitide+((altitude-ground)*landRate)
# if timestamp>(landtime+landtimer): # if enough time has pasted for landing to happen
# if (baro-error) < ground:
# fmode=9
# else:
# fmode=3 #abort landing and go back to hovering
# Mode 9: landed or shutdown abort
if fmode==9:
pitch=0
roll=0
thrust=0
# end of Modes

iv
print(thrust)
Flighttab.Py
"""
The flight control tab shows telimitry data and flight settings.
"""
__author__ = 'Bitcraze AB'

__all__ = ['FlightTab']
import sys
import logging
logger = logging.getLogger(__name__)
from time import time

from PyQt4 import QtCore, QtGui, uic
from PyQt4.QtCore import Qt, pyqtSlot, pyqtSignal, QThread, SIGNAL
from cflib.crazyflie import Crazyflie
from cfclient.ui.widgets.ai import AttitudeIndicator
from cfclient.utils.guiconfig import GuiConfig
from cflib.crazyflie.log import Log
from cfclient.ui.tab import Tab
from cfclient.utils.logconfigreader import LogVariable, LogConfig
import math

v
flight_tab_class = uic.loadUiType(sys.path[0] +
"/cfclient/ui/tabs/flightTab.ui")[0]
MAX_THRUST = 65365.0
class FlightTab(Tab, flight_tab_class):
uiSetupReadySignal = pyqtSignal()
# mag added
_mag_data_signal = pyqtSignal(object, int)
_motor_data_signal = pyqtSignal(object, int)
_imu_data_signal = pyqtSignal(object, int)
_althold_data_signal = pyqtSignal(object, int)
_baro_data_signal = pyqtSignal(object, int)
_input_updated_signal = pyqtSignal(float, float, float, float)

_rp_trim_updated_signal = pyqtSignal(float, float)
_emergency_stop_updated_signal = pyqtSignal(bool)
#UI_DATA_UPDATE_FPS = 10
connectionFinishedSignal = pyqtSignal(str)
disconnectedSignal = pyqtSignal(str)
def __init__(self, tabWidget, helper, *args):

vi
super(FlightTab, self).__init__(*args)
self.setupUi(self)
self.tabName = "Flight Control"

self.menuName = "Flight Control"
#####
## New variables for magnetometer
#####
GuiConfig().set("MAXX", -2000)
GuiConfig().set("MAXY", -2000)
GuiConfig().set("MAXZ", -2000)
GuiConfig().set("MINX", 2000)
GuiConfig().set("MINY", 2000)
GuiConfig().set("MINZ", 2000)
#####
## New variables for THL
#####
GuiConfig().set("toTimer", 3500) #take off timer in ms (if not reach take off height by totimer shutdown
GuiConfig().set("toTime",-1)
GuiConfig().set("landTimer", 0)
GuiConfig().set("altitude", -888)
GuiConfig().set("Fmode", 0)
GuiConfig().set("ground", -888)
GuiConfig().set("landTime", 0)
GuiConfig().set("warmUpThrust", 0.4)
GuiConfig().set("warmUpTime", 1000)

vii
GuiConfig().set("takeOffStop", 0.9)
GuiConfig().set("takeOffThrust",0.8)
GuiConfig().set("landingError", 0.2)
GuiConfig().set("timeStamp",-1)
GuiConfig().set("baro",-888)
GuiConfig().set("takeOff",0.5)
GuiConfig().set("landRate",0.1)
self.tabWidget = tabWidget
self.helper = helper
self.disconnectedSignal.connect(self.disconnected)
self.connectionFinishedSignal.connect(self.connected)
# Incomming signals
self.helper.cf.connectSetupFinished.add_callback(
self.connectionFinishedSignal.emit)
self.helper.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self._input_updated_signal.connect(self.updateInputControl)
self.helper.inputDeviceReader.input_updated.add_callback(
self._input_updated_signal.emit)
self._rp_trim_updated_signal.connect(self.calUpdateFromInput)
self.helper.inputDeviceReader.rp_trim_updated.add_callback(
self._rp_trim_updated_signal.emit)
self._emergency_stop_updated_signal.connect(self.updateEmergencyStop)
self.helper.inputDeviceReader.emergency_stop_updated.add_callback(
self._emergency_stop_updated_signal.emit)

viii
self.helper.inputDeviceReader.althold_updated.add_callback(
lambda enabled: self.helper.cf.param.set_value("flightmode.althold", enabled))
self._imu_data_signal.connect(self._imu_data_received)
self._mag_data_signal.connect(self._mag_data_received)
self._baro_data_signal.connect(self._baro_data_received)
self._althold_data_signal.connect(self._althold_data_received)
self._motor_data_signal.connect(self._motor_data_received)
# Connect UI signals that are in this tab

self.flightModeCombo.currentIndexChanged.connect(self.flightmodeChange)
self.minThrust.valueChanged.connect(self.minMaxThrustChanged)
self.maxThrust.valueChanged.connect(self.minMaxThrustChanged)
self.thrustLoweringSlewRateLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.slewEnableLimit.valueChanged.connect(
self.thrustLoweringSlewRateLimitChanged)
self.targetCalRoll.valueChanged.connect(self._trim_roll_changed)
self.targetCalPitch.valueChanged.connect(self._trim_pitch_changed)
self.maxAngle.valueChanged.connect(self.maxAngleChanged)
self.maxYawRate.valueChanged.connect(self.maxYawRateChanged)
self.uiSetupReadySignal.connect(self.uiSetupReady)
self.clientXModeCheckbox.toggled.connect(self.changeXmode)
self.isInCrazyFlightmode = False
self.uiSetupReady()

ix
self.clientXModeCheckbox.setChecked(GuiConfig().get("client_side_xmode"))
self.crazyflieXModeCheckbox.clicked.connect(
lambda enabled:
self.helper.cf.param.set_value("flightmode.x",
str(enabled)))
self.helper.cf.param.add_update_callback(
group="flightmode", name="xmode",
cb=( lambda name, checked:
self.crazyflieXModeCheckbox.setChecked(eval(checked))))
self.ratePidRadioButton.clicked.connect(
lambda enabled:
self.helper.cf.param.set_value("flightmode.ratepid",
str(enabled)))
self.angularPidRadioButton.clicked.connect(
lambda enabled:
self.helper.cf.param.set_value("flightmode.ratepid",
str(not enabled)))
group="flightmode", name="ratepid",
cb=(lambda name, checked:
self.ratePidRadioButton.setChecked(eval(checked))))
group="flightmode", name="althold",
cb=(lambda name, enabled:
self.helper.inputDeviceReader.setAltHold(eval(enabled))))

x
group="imu_sensors",
cb=self._set_available_sensors)
self.logBaro = None
self.logAltHold = None
self.ai = AttitudeIndicator()
self.verticalLayout_4.addWidget(self.ai)
self.splitter.setSizes([1000,1])
self.targetCalPitch.setValue(GuiConfig().get("trim_pitch"))
self.targetCalRoll.setValue(GuiConfig().get("trim_roll"))
def thrustToPercentage(self, thrust):

return ((thrust / MAX_THRUST) * 100.0)
def uiSetupReady(self):
flightComboIndex = self.flightModeCombo.findText(
GuiConfig().get("flightmode"), Qt.MatchFixedString)
if (flightComboIndex < 0):
self.flightModeCombo.setCurrentIndex(0)
self.flightModeCombo.currentIndexChanged.emit(0)
else:
self.flightModeCombo.setCurrentIndex(flightComboIndex)
self.flightModeCombo.currentIndexChanged.emit(flightComboIndex)

xi
def loggingError(self):
logger.warning("Callback of error in LogEntry :(")
def _motor_data_received(self, data, timestamp):

self.actualM1.setValue(data["motor.m1"])
def _baro_data_received(self, data, timestamp):

self.actualASL.setText(("%.2f" % data["baro.aslLong"]))
self.ai.setBaro(data["baro.aslLong"])
fM=GuiConfig().get("Fmode")
if fM==0:
GuiConfig().set("ground",data["baro.aslLong"])
######################
## THL send parameters for GUI display
######################
totimer=GuiConfig().get("toTimer")
self.toTimer.setText(("%d" % totimer))
landtimer=GuiConfig().get("landTimer")
self.landTimer.setText(("%d" % landtimer))
fmode=GuiConfig().get("Fmode")
self.Fmode.setText(("%d" % fmode))
self.timeStamp.setText(("%d" % timestamp))

xii
GuiConfig().set("timeStamp",timestamp)
GuiConfig().set("altitude", data["baro.aslLong"])
self.magX.setText(("%d" % data["acc.x"]))
self.magY.setText(("%d" % data["acc.y"]))
self.magZ.setText(("%d" % data["acc.z"]))
#####################
## Magnetometer display and calculate some parameters
#####################
def _mag_data_received(self, data, timestamp):

self.magX.setText(("%d" % data["acc.x"]))
self.magY.setText(("%d" % data["acc.y"]))
self.magZ.setText(("%d" % data["acc.z"]))
maxx=GuiConfig().get("MAXX")
if (data["acc.x"] > maxx):
GuiConfig().set("MAXX", data["acc.x"])
self.maxX.setText("%d" % maxx)
maxy=GuiConfig().get("MAXY")
if (data["acc.y"] > maxy):
GuiConfig().set("MAXY", data["acc.y"])
self.maxY.setText("%d" % maxy)
maxz=GuiConfig().get("MAXZ")
if (data["acc.z"] > maxz):
GuiConfig().set("MAXZ", data["acc.z"])
self.maxZ.setText("%d" % maxz)

xiii
minx=GuiConfig().get("MINX")
if (data["acc.x"] < minx):
GuiConfig().set("MINX", data["acc.x"])
self.minX.setText("%d" % minx)
miny=GuiConfig().get("MINY")
if (data["acc.y"] < miny):
GuiConfig().set("MINY", data["acc.y"])
self.minY.setText("%d" % miny)
minz=GuiConfig().get("MINZ")
if (data["acc.z"] < minz):
GuiConfig().set("MINZ", data["acc.z"])
self.minZ.setText("%d" % minz)
rangex=GuiConfig().get("MAXX")-GuiConfig().get("MINX")
rangey=GuiConfig().get("MAXY")-GuiConfig().get("MINY")
rangez=GuiConfig().get("MAXZ")-GuiConfig().get("MINZ")
self.rangeX.setText(("%d" % rangex))
self.rangeY.setText(("%d" % rangey))
self.rangeZ.setText(("%d" % rangez))
midX=(maxx+minx)/2
val=(data["acc.x"]-midX)
if rangex!=0:
valX=(float(val)/float(rangex))*2
else:
valX=-888
midY=(maxy+miny)/2
val=(data["acc.y"]-midY)

xiv
if rangey!=0:
valY=(float(val)/float(rangey))*2
else:
valY=-888
midZ=(maxz+minz)/2
val=(data["acc.z"]-midZ)
if rangey!=0:
valZ=(float(val)/float(rangez))*2
else:
valZ=-888
self.valX.setText(("%.2f" % valX))
self.valY.setText(("%.2f" % valY))
self.valZ.setText(("%.2f" % valZ))
########################################################
### ###
### Calculate the heading from the magnetometer ###
### ###
########################################################
#
# DOESN'T WORK - FIX LATER
if (valX>-1)and(valX<1):
magpitch=math.asin(valX)*180/3.14159
else:
magpitch=-888
if (valY>-1)and(valY<1):
magroll=math.asin(valY)*180/3.14159

xv
else:
magroll=-888
if (valZ>-1)and(valZ<1):
magyaw=math.asin(valZ)*180/3.14159
else:
magyaw=-888
self.magPitch.setText(("%.2f" % magpitch))
self.magRoll.setText(("%.2f" % magroll))
self.magYaw.setText(("%.2f" % magyaw))
def _althold_data_received(self, data, timestamp):
target = data["altHold.target"]
if target>0:
if not self.targetASL.isEnabled():
self.targetASL.setEnabled(True)
self.targetASL.setText(("%.2f" % target))
self.ai.setHover(target)
elif self.targetASL.isEnabled():
self.targetASL.setEnabled(False)
self.targetASL.setText("Not set")
self.ai.setHover(0)
def _imu_data_received(self, data, timestamp):
self.actualRoll.setText(("%.2f" % data["stabilizer.roll"]))
self.actualPitch.setText(("%.2f" % data["stabilizer.pitch"]))
self.actualYaw.setText(("%.2f" % data["stabilizer.yaw"]))
self.actualThrust.setText("%.2f%%" %
self.thrustToPercentage(

xvi
data["stabilizer.thrust"]))
self.ai.setRollPitch(-data["stabilizer.roll"],
data["stabilizer.pitch"])
def connected(self, linkURI):

# IMU & THRUST
lg = LogConfig("Stabalizer", 50)
lg.addVariable(LogVariable("stabilizer.roll", "float"))
lg.addVariable(LogVariable("stabilizer.pitch", "float"))
lg.addVariable(LogVariable("stabilizer.yaw", "float"))
lg.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))
self.log = self.helper.cf.log.create_log_packet(lg)
if (self.log is not None):
self.log.data_received.add_callback(self._imu_data_signal.emit)
self.log.error.add_callback(self.loggingError)
self.log.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
# MOTOR
lg = LogConfig("Motors", 50)
lg.addVariable(LogVariable("motor.m1", "uint32_t"))

xvii
self.log = self.helper.cf.log.create_log_packet(lg)
if (self.log is not None):
self.log.data_received.add_callback(self._motor_data_signal.emit)
self.log.error.add_callback(self.loggingError)
self.log.start()
else:
"connection!")
#lg = LogConfig("Magnetometer", 100)
#lg.addVariable(LogVariable("mag.x", "int16_t"))
#lg.addVariable(LogVariable("mag.y", "int16_t"))
#lg.addVariable(LogVariable("mag.z", "int16_t"))
#self.log = self.helper.cf.log.create_log_packet(lg)
#if (self.log is not None):
# self.log.data_received.add_callback(self._mag_data_signal.emit)
# self.log.error.add_callback(self.loggingError)
# self.log.start()
#else:
# logger.warning("Could not setup logconfiguration after connection!")
def _set_available_sensors(self, name, available):

logger.info("[%s]: %s", name, available)
available = eval(available)

xviii
if ("HMC5883L" in name):
if (not available):
self.actualASL.setText("N/A")
else:
self.actualASL.setEnabled(True)
self.helper.inputDeviceReader.setAltHoldAvailable(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
lg = LogConfig("Baro", 50)
lg.addVariable(LogVariable("baro.asl", "float"))
lg.addVariable(LogVariable("baro.aslLong", "float"))
lg.addVariable(LogVariable("baro.aslRaw", "float"))
lg.addVariable(LogVariable("baro.temp", "float"))
self.logBaro = self.helper.cf.log.create_log_packet(lg)
if (self.logBaro is not None):
self.logBaro.data_received.add_callback(self._baro_data_signal.emit)
self.logBaro.error.add_callback(self.loggingError)
self.logBaro.start()
else:
"connection!")
lg = LogConfig("AltHold", 50)
lg.addVariable(LogVariable("altHold.target", "float"))
self.logAltHold = self.helper.cf.log.create_log_packet(lg)

xix
if (self.logAltHold is not None):

self.logAltHold.data_received.add_callback(self._althold_data_signal.emit)
self.logAltHold.error.add_callback(self.loggingError)
self.logAltHold.start()
else:
"connection!")
def disconnected(self, linkURI):

self.ai.setRollPitch(0, 0)
self.actualM1.setValue(0)
self.actualRoll.setText("")
self.actualPitch.setText("")
self.actualYaw.setText("")
self.actualThrust.setText("")
self.actualASL.setText("")
self.targetASL.setText("Not Set")
self.targetASL.setEnabled(False)
def minMaxThrustChanged(self):
self.helper.inputDeviceReader.set_thrust_limits(
self.minThrust.value(), self.maxThrust.value())
if (self.isInCrazyFlightmode == True):

xx
GuiConfig().set("min_thrust", self.minThrust.value())
GuiConfig().set("max_thrust", self.maxThrust.value())
def thrustLoweringSlewRateLimitChanged(self):
self.helper.inputDeviceReader.set_thrust_slew_limiting(
self.thrustLoweringSlewRateLimit.value(),
self.slewEnableLimit.value())
GuiConfig().set("slew_limit", self.slewEnableLimit.value())
GuiConfig().set("slew_rate", self.thrustLoweringSlewRateLimit.value())
def maxYawRateChanged(self):
logger.debug("MaxYawrate changed to %d", self.maxYawRate.value())
self.helper.inputDeviceReader.set_yaw_limit(self.maxYawRate.value())
GuiConfig().set("max_yaw", self.maxYawRate.value())
def maxAngleChanged(self):
logger.debug("MaxAngle changed to %d", self.maxAngle.value())
self.helper.inputDeviceReader.set_rp_limit(self.maxAngle.value())
GuiConfig().set("max_rp", self.maxAngle.value())
def _trim_pitch_changed(self, value):

logger.debug("Pitch trim updated to [%f]" % value)
self.helper.inputDeviceReader.set_trim_pitch(value)

xxi
GuiConfig().set("trim_pitch", value)
def _trim_roll_changed(self, value):

logger.debug("Roll trim updated to [%f]" % value)
self.helper.inputDeviceReader.set_trim_roll(value)
GuiConfig().set("trim_roll", value)
def calUpdateFromInput(self, rollCal, pitchCal):

logger.debug("Trim changed on joystick: roll=%.2f, pitch=%.2f",
rollCal, pitchCal)
self.targetCalRoll.setValue(rollCal)
self.targetCalPitch.setValue(pitchCal)
def updateInputControl(self, roll, pitch, yaw, thrust):

self.targetRoll.setText(("%0.2f" % roll))
self.targetPitch.setText(("%0.2f" % pitch))
self.targetYaw.setText(("%0.2f" % yaw))
self.targetThrust.setText(("%0.2f %%" %
self.thrustToPercentage(thrust)))
self.thrustProgress.setValue(thrust)
def setMotorLabelsEnabled(self, enabled):

self.M1label.setEnabled(enabled)

xxii
def emergencyStopStringWithText(self, text):

return ("<html><head/><body><p>"
"<span style='font-weight:600; color:#7b0005;'>{}</span>"
"</p></body></html>".format(text))
def updateEmergencyStop(self, emergencyStop):

if emergencyStop:
self.setMotorLabelsEnabled(False)
self.emergency_stop_label.setText(
self.emergencyStopStringWithText("Kill switch active"))
else:
self.setMotorLabelsEnabled(True)
self.emergency_stop_label.setText("")
def flightmodeChange(self, item):

GuiConfig().set("flightmode", self.flightModeCombo.itemText(item))
logger.info("Changed flightmode to %s",
self.flightModeCombo.itemText(item))
self.isInCrazyFlightmode = False
if (item == 0): # Normal
self.maxAngle.setValue(GuiConfig().get("normal_max_rp"))
self.maxThrust.setValue(GuiConfig().get("normal_max_thrust"))
self.minThrust.setValue(GuiConfig().get("normal_min_thrust"))
self.slewEnableLimit.setValue(GuiConfig().get("normal_slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(

xxiii
GuiConfig().get("normal_slew_rate"))
self.maxYawRate.setValue(GuiConfig().get("normal_max_yaw"))
if (item == 1): # Advanced
self.maxAngle.setValue(GuiConfig().get("max_rp"))
self.maxThrust.setValue(GuiConfig().get("max_thrust"))
self.minThrust.setValue(GuiConfig().get("min_thrust"))
self.slewEnableLimit.setValue(GuiConfig().get("slew_limit"))
self.thrustLoweringSlewRateLimit.setValue(
GuiConfig().get("slew_rate"))
self.maxYawRate.setValue(GuiConfig().get("max_yaw"))
self.isInCrazyFlightmode = True
if (item == 0):
newState = False
else:
newState = True
self.maxThrust.setEnabled(newState)
self.maxAngle.setEnabled(newState)
self.minThrust.setEnabled(newState)
self.thrustLoweringSlewRateLimit.setEnabled(newState)
self.slewEnableLimit.setEnabled(newState)
self.maxYawRate.setEnabled(newState)
@pyqtSlot(bool)
def changeXmode(self, checked):
self.helper.cf.commander.set_client_xmode(checked)

xxiv
GuiConfig().set("client_side_xmode", checked)
logger.info("Clientside X-mode enabled: %s", checked)
inputconfigdialog.py
"""
Dialogue used to select and configure an inputdevice. This includes mapping buttuns and
axis to match controls for the Crazyflie.
"""

__all__ = ['InputConfigDialogue']
import sys
import json
import logging
logger = logging.getLogger(__name__)
from cfclient.utils.config_manager import ConfigManager

from cflib.crtp.exceptions import CommunicationException
from pygame.locals import *
from PyQt4 import Qt, QtCore, QtGui, uic

from PyQt4.QtCore import *
from PyQt4.QtGui import *
from PyQt4.Qt import *

xxv
from cfclient.utils.input import JoystickReader
(inputconfig_widget_class,
connect_widget_base_class) = (uic.loadUiType(sys.path[0] +
'/cfclient/ui/dialogs/inputconfigdialogue.ui'))
class InputConfigDialogue(QtGui.QWidget, inputconfig_widget_class):
def __init__(self, joystickReader, *args):

super(InputConfigDialogue, self).__init__(*args)
self.setupUi(self)
self.joystickReader = joystickReader
self.rawinputreader = RawJoystickReader(self.joystickReader)
self.rawinputreader.start()
self.rawinputreader.rawAxisUpdateSignal.connect(self.rawAxisUpdate)
self.rawinputreader.rawButtonUpdateSignal.connect(self.rawButtonUpdate)
self.cancelButton.clicked.connect(self.close)
self.saveButton.clicked.connect(self.saveConfig)
self.detectPitch.clicked.connect(lambda : self.doAxisDetect("pitch", "Pitch axis",
"Press the pitch axis to max %s pitch", ["forward", "backward"]))
self.detectRoll.clicked.connect(lambda : self.doAxisDetect("roll", "Roll axis",

xxvi
"Press the roll axis to max %s roll", ["right", "left"]))

self.detectYaw.clicked.connect(lambda : self.doAxisDetect("yaw", "Yaw axis",
"Press the yaw axis to max rotation %s", ["right", "left"]))
self.detectThrust.clicked.connect(lambda : self.doAxisDetect("thrust", "Thrust axis",
"Press the thrust axis to max thrust (also used to adjust target altitude in altitude hold mode)"))
self.detectPitchPos.clicked.connect(lambda : self.doButtonDetect("pitchPos", "Pitch Cal Positive",
"Press the button for Pitch postive calibration"))
self.detectPitchNeg.clicked.connect(lambda : self.doButtonDetect("pitchNeg", "Pitch Cal Negative",
"Press the button for Pitch negative calibration"))
self.detectRollPos.clicked.connect(lambda : self.doButtonDetect("rollPos", "Roll Cal Positive",
"Press the button for Roll positive calibration"))
self.detectRollNeg.clicked.connect(lambda : self.doButtonDetect("rollNeg", "Roll Cal Negative",
"Press the button for Roll negative calibration"))
self.detectKillswitch.clicked.connect(lambda : self.doButtonDetect("killswitch", "Killswitch",
"Press the button for the killswitch (will disable motors)"))
self.detecttakeoffland.clicked.connect(lambda : self.doButtonDetect("takeoffland", "takeoffland",
"Press the button for Take-off and Landing Modes"))
self.detectExitapp.clicked.connect(lambda : self.doButtonDetect("exitapp", "Exit application",
"Press the button for the exiting the application"))
self.detectAltHold.clicked.connect(lambda : self.doButtonDetect("althold", "Altitude hold",
"Press the button for altitude hold mode activation (releasing returns to manual mode)"))
self.configButton.clicked.connect(self.startConfigOfInputDevice)
self.loadButton.clicked.connect(self.loadConfig)
self.deleteButton.clicked.connect(self.deleteConfig)

xxvii
self.box = None
self.combinedButton = None
self.detectButtons = [self.detectPitch, self.detectRoll,
self.detectYaw, self.detectThrust,
self.detectPitchPos, self.detectPitchNeg,
self.detectRollPos, self.detectRollNeg,
self.detectKillswitch, self.detectExitapp,
self.detecttakeoffland,self.detectAltHold]
self._reset_mapping()
self.btnDetect = ""
self.axisDetect = ""
self.combinedDetection = 0
for d in self.joystickReader.getAvailableDevices():
self.inputDeviceSelector.addItem(d["name"], d["id"])
if (len(self.joystickReader.getAvailableDevices()) > 0):

self.configButton.setEnabled(True)
def _reset_mapping(self):
self.buttonmapping = {
"pitchPos": {"id":-1, "indicator": self.pitchPos},
"pitchNeg": {"id":-1, "indicator": self.pitchNeg},
"rollPos": {"id":-1, "indicator": self.rollPos},
"rollNeg": {"id":-1, "indicator": self.rollNeg},

xxviii
"killswitch": {"id":-1, "indicator": self.killswitch},

"takeoffland": {"id":-1, "indicator": self.takeoffland},
"exitapp": {"id":-1, "indicator": self.exitapp},
"althold": {"id":-1, "indicator": self.althold},
}
self.axismapping = {
"pitch": {"id":-1,
"indicator": self.pitchAxisValue,
"scale":-1.0},
"roll": {"id":-1,
"indicator": self.rollAxisValue,
"scale":-1.0},
"yaw": {"id":-1,
"indicator": self.yawAxisValue,
"scale":-1.0},
"thrust": {"id":-1,
"indicator": self.thrustAxisValue,
"scale":-1.0}
}
def cancelConfigBox(self, button):

self.btnDetect = ""
def showConfigBox(self, caption, message, directions=[]):

xxix
self.box = QMessageBox()
self.box.directions = directions
self.combinedButton = QtGui.QPushButton('Combined Axis Detection')
self.cancelButton = QtGui.QPushButton('Cancel')
self.box.addButton(self.cancelButton, QMessageBox.DestructiveRole)
self.box.setWindowTitle(caption)
self.box.setWindowFlags(Qt.Dialog|Qt.MSWindowsFixedSizeDialogHint)
if len(directions) > 1:
self.box.originalMessage = message
message = self.box.originalMessage % directions[0]
self.combinedButton.setCheckable(True)
self.combinedButton.blockSignals(True)
self.box.addButton(self.combinedButton, QMessageBox.ActionRole)
self.box.setText(message)
self.box.show()
def startConfigOfInputDevice(self):
self.joystickReader.enableRawReading(self.inputDeviceSelector.currentIndex())
self.rawinputreader.startReading()
self.populateDropDown()
self.profileCombo.setEnabled(True)
for b in self.detectButtons:
b.setEnabled(True)
def rawAxisUpdate(self, data):

if (len(self.axisDetect) > 0):

xxx
if self.combinedButton and self.combinedButton.isChecked() and self.combinedDetection == 0:

self.combinedButton.setDisabled(True)
for a in data:
# TODO: Some axis on the PS3 controller are maxed out by default which causes problems...check change instead?
# TODO: This assumes a range [-1.0,1.0] from input device driver, but is that safe?
if (abs(data[a]) > 0.8 and abs(data[a]) < 1.0 and len(self.axisDetect) > 0):
if self.combinedDetection == 0:
self.axismapping[self.axisDetect]["id"] = a
if (data[a] >= 0):
self.axismapping[self.axisDetect]["scale"] = 1.0
else:
self.axismapping[self.axisDetect]["scale"] = -1.0
self.checkAndEnableSave()
if (self.box != None):
self.cancelButton.click()
elif self.combinedDetection == 2: #finished detection
if self.axismapping[self.axisDetect]["ids"][0] != a: # not the same axe again ...
self.axismapping[self.axisDetect]["ids"].insert(0,a)
self.cancelButton.click()
elif self.combinedDetection == 1:

xxxi
if "id" in self.axismapping[self.axisDetect]:
del self.axismapping[self.axisDetect]["id"]
self.axismapping[self.axisDetect]["ids"] = [a]
self.axismapping[self.axisDetect]["scale"] = 1.0
message = self.box.originalMessage % self.box.directions[1]
self.box.setText(message)
for a in data:
for m in self.axismapping:
if "id" in self.axismapping[m]:
if (self.axismapping[m]["id"] == a):
self.axismapping[m]["indicator"].setValue(50+data[a]*50*self.axismapping[m]["scale"])
else:
for id in self.axismapping[m]["ids"]:
if (id == a):
pos = -1 if id == self.axismapping[m]["ids"][0] else 1
self.axismapping[m]["indicator"].setValue(50+data[a]*50*self.axismapping[m]["scale"]*pos)
def rawButtonUpdate(self, data):

if (len(self.btnDetect) > 0):
for b in data:
if (data[b] > 0):
self.buttonmapping[self.btnDetect]["id"] = b
self.btnDetect = ""

xxxii
self.box.close()
for b in data:
for m in self.buttonmapping:
if (self.buttonmapping[m]["id"] == b):
if (data[b] == 0):
self.buttonmapping[m]["indicator"].setChecked(False)
else:
self.buttonmapping[m]["indicator"].setChecked(True)
def checkAndEnableSave(self):
canSave = True
for m in self.axismapping:
if "id" in self.axismapping[m]:
if (self.axismapping[m]["id"] == -1):
canSave = False
else:
if (len(self.axismapping[m]["ids"]) != 2):
canSave = False
if (canSave == True):
self.saveButton.setEnabled(True)
def populateDropDown(self):
configs = ConfigManager().get_list_of_configs()
if len(configs):
self.loadButton.setEnabled(True)
for c in configs:

xxxiii
self.profileCombo.addItem(c)
logger.info("Found inputdevice [%s]", c)
def doAxisDetect(self, varname, caption, message, directions=[]):

self.axisDetect = varname
self.showConfigBox(caption, message, directions)
def doButtonDetect(self, varname, caption, message):

self.btnDetect = varname
self.showConfigBox(caption, message)
def showError(self, caption, message):

QMessageBox.critical(self, caption, message)
def parseButtonConfig(self, key, btnId, scale):

newKey = ""
if ("pitch" in key and scale > 0):
newKey = "pitchPos"
if ("pitch"in key and scale < 0):
newKey = "pitchNeg"
if ("roll" in key and scale > 0):
newKey = "rollPos"
if ("roll" in key and scale < 0):
newKey = "rollNeg"
if ("estop" in key):
newKey = "killswitch"

xxxiv
if ("takeoffland" in key):
newKey = "takeoffland"
if ("exit" in key):
newKey = "exitapp"
if ("althold" in key):
newKey = "althold"
if (len(newKey) > 0):
self.buttonmapping[newKey]['id'] = btnId
else:
logger.warning("Could not find new key for [%s]", key)
def parseAxisConfig(self, key, axisId, scale):

if self.axismapping[key]['id'] != -1: #second axis
if scale > 0:
self.axismapping[key]['ids'] = [self.axismapping[key]['id'], axisId]
else:
self.axismapping[key]['ids'] = [axisId, self.axismapping[key]['id']]
del self.axismapping[key]['id']
else:
self.axismapping[key]['id'] = axisId
self.axismapping[key]['scale'] = scale
def loadConfig(self):
conf = ConfigManager().get_config(self.profileCombo.currentText())
self._reset_mapping()
if (conf != None):

xxxv
for c in conf:
if (conf[c]['type'] == "Input.BUTTON"):
self.parseButtonConfig(conf[c]['key'],
conf[c]['id'], conf[c]['scale'])
elif (conf[c]['type'] == "Input.AXIS"):
self.parseAxisConfig(conf[c]['key'],
conf[c]['id'], conf[c]['scale'])
else:
logger.warning("Could not load configfile [%s]",
self.profileCombo.currentText())
self.showError("Could not load config",
"Could not load config [%s]" %
self.profileCombo.currentText())
def deleteConfig(self):
logger.warning("deleteConfig not implemented")
def saveConfig(self):
configName = str(self.profileCombo.currentText())
saveConfig = {}
inputConfig = {'inputdevice': {'axis': []}}
for a in self.axismapping:
newC = {}
if "id" in self.axismapping[a]:

xxxvi
newC['id'] = self.axismapping[a]['id']
elif "ids" in self.axismapping[a]:
newC['ids'] = self.axismapping[a]['ids']
else:
raise Exception("Problem during save")
newC['key'] = a
newC['name'] = a
newC['scale'] = self.axismapping[a]['scale']
newC['type'] = "Input.AXIS"
inputConfig['inputdevice']['axis'].append(newC)
for a in self.buttonmapping:
newC = {}
newC['id'] = self.buttonmapping[a]['id']
newC['type'] = "Input.BUTTON"
if (a.find("Neg") >= 0):
newC['scale'] = -1.0
else:
newC['scale'] = 1.0
if ("pitch" in a):
newC['key'] = "pitchcal"
newC['name'] = a
if ("roll" in a):

xxxvii
newC['key'] = "rollcal"
newC['name'] = a
if ("killswitch" in a):
newC['key'] = "estop"
newC['name'] = a
if ("takeoffland" in a):
newC['key'] = "takeoffland"
newC['name'] = a
if ("exit" in a):
newC['key'] = "exit"
newC['name'] = a
if ("althold" in a):
newC['key'] = "althold"
newC['name'] = a
inputConfig['inputdevice']['axis'].append(newC)
inputConfig['inputdevice']['name'] = configName
inputConfig['inputdevice']['updateperiod'] = 10
saveConfig['inputconfig'] = inputConfig
config_name = self.profileCombo.currentText()

xxxviii
filename = ConfigManager().configs_dir + "/%s.json" % config_name

logger.info("Saving config to [%s]", filename)
json_data = open(filename, 'w')
json_data.write(json.dumps(saveConfig, indent=2))
json_data.close()
ConfigManager().conf_needs_reload.call(config_name)
self.close()
def showEvent(self, event):

self.joystickReader.stop_input()
def closeEvent(self, event):

self.rawinputreader.stopReading()
class RawJoystickReader(QThread):
rawAxisUpdateSignal = pyqtSignal(object)
rawButtonUpdateSignal = pyqtSignal(object)
def __init__(self, joystickReader):

QThread.__init__(self)
self.joystickReader = joystickReader
self.readTimer = QTimer()

xxxix
self.readTimer.setInterval(25)
self.connect(self.readTimer, SIGNAL("timeout()"), self.read_input)
def stopReading(self):
self.readTimer.stop()
def startReading(self):
self.readTimer.start()
@pyqtSlot()
def read_input(self):
[rawaxis, rawbuttons] = self.joystickReader.readRawValues()
self.rawAxisUpdateSignal.emit(rawaxis)
self.rawButtonUpdateSignal.emit(rawbuttons)
pygamereader.py
"""
Driver for reading data from the PyGame API. Used from Inpyt.py for reading input data.
"""

__all__ = ['PyGameReader']
import pygame
from pygame.locals import *
import time
class PyGameReader():

xl
"""Used for reading data from input devices using the PyGame API."""
def __init__(self):
self.inputMap = None
pygame.init()
def start_input(self, deviceId, inputMap):

"""Initalize the reading and open the device with deviceId and set the mapping for axis/buttons using the
inputMap"""
self.data = {"roll":0.0, "pitch":0.0, "yaw":0.0, "thrust":0.0, "pitchcal":0.0, "rollcal":0.0, "estop": False, "exit":False,"takeoffland":
False, "althold":False}
self.inputMap = inputMap
self.j = pygame.joystick.Joystick(deviceId)
self.j.init()
def read_input(self):
"""Read input from the selected device."""
# We only want the pitch/roll cal to be "oneshot", don't
# save this value.
self.data["pitchcal"] = 0.0
self.data["rollcal"] = 0.0
for e in pygame.event.get():
if e.type == pygame.locals.JOYAXISMOTION:
index = "Input.AXIS-%d" % e.axis
try:
if (self.inputMap[index]["type"] == "Input.AXIS"):
key = self.inputMap[index]["key"]
axisvalue = self.j.get_axis(e.axis)

xli
# All axis are in the range [-a,+a]

axisvalue = axisvalue * self.inputMap[index]["scale"]
# The value is now in the correct direction and in the range [-1,1]
self.data[key] = axisvalue
except Exception:
# Axis not mapped, ignore..
pass
if e.type == pygame.locals.JOYBUTTONDOWN:
index = "Input.BUTTON-%d" % e.button
try:
if (self.inputMap[index]["type"] == "Input.BUTTON"):
if (key == "estop"):
self.data["estop"] = not self.data["estop"]
elif (key == "exit"):
self.data["exit"] = True
elif (key == "takeoffland"):
self.data["takeoffland"] = not self.data["takeoffland"]
elif (key == "althold"):
self.data["althold"] = not self.data["althold"]
else: # Generic cal for pitch/roll
self.data[key] = self.inputMap[index]["scale"]
except Exception:
# Button not mapped, ignore..
pass

xlii
if e.type == pygame.locals.JOYBUTTONUP:
index = "Input.BUTTON-%d" % e.button
try:
if (self.inputMap[index]["type"] == "Input.BUTTON"):
if (key == "althold"):
self.data["althold"] = False
except Exception:
# Button not mapped, ignore..
pass
return self.data
def enableRawReading(self, deviceId):

"""Enable reading of raw values (without mapping)"""
self.j = pygame.joystick.Joystick(deviceId)
self.j.init()
def disableRawReading(self):
"""Disable raw reading"""
# No need to de-init since there's no good support for multiple input devices
pass
def readRawValues(self):
"""Read out the raw values from the device"""
rawaxis = {}
rawbutton = {}

xliii
for e in pygame.event.get():
if e.type == pygame.locals.JOYBUTTONDOWN:
rawbutton[e.button] = 1
if e.type == pygame.locals.JOYBUTTONUP:
rawbutton[e.button] = 0
if e.type == pygame.locals.JOYAXISMOTION:
rawaxis[e.axis] = self.j.get_axis(e.axis)
return [rawaxis,rawbutton]
def getAvailableDevices(self):
"""List all the available devices."""
dev = []
pygame.joystick.quit()
pygame.joystick.init()
nbrOfInputs = pygame.joystick.get_count()
for i in range(0,nbrOfInputs):
j = pygame.joystick.Joystick(i)
dev.append({"id":i, "name" : j.get_name()})
return dev

xliv

Thesis CDU 41657 Hanna W

Cargado por

Información del documento

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Thesis CDU 41657 Hanna W

Cargado por

Copyright:

Formatos disponibles

Modelling and Control of an

Unmanned Aerial Vehicle

Modelling and control of an unmanned aerial vehicle

2 Literature Review ......................................................................................................................... 4

4.2 MATLAB Implementation .................................................................................................. 35

Modelling and control of an unmanned aerial vehicle

List of symbols and abbreviations

Thrust force produced by propeller i

Propeller lift constant

Torque produced by propeller i drag

Propeller drag constant

Acceleration in the i direction

Velocity in the i direction

Displacement in the i direction

Moment of Inertia of propeller

Moment of Inertia of body about i axis

Angular velocity of propeller i

Angular acceleration of propeller i

PWM duty cycle (as a fraction)

PWM duty cycle (as a percentage)

Maximum motor speed for given supply voltage

roll , pitch , yaw

Angular accelerations of body

roll , pitch , yaw

Angular velocities of body

Froll , Fpitch , Fyaw

Resultant forces due to force imbalances

Resultant force acting on body

No-load battery voltage

Modelling and control of an unmanned aerial vehicle

Altitude Above Sea-level

Inertial Measurement Unit

Proportional Integral Derivative

Pulse Width Modulation

Roll Pitch and Yaw

Unmanned Aerial Vehicle

Figure 17: Original PC Client interface ......................................................................................... 36

Modelling and control of an unmanned aerial vehicle

Modelling and control of an unmanned aerial vehicle

1.2 History of Quadcopters

Photograph 4:De Bothezat Quadrotor built in 1923

1.3 Existing Work

Modelling and control of an unmanned aerial vehicle

1.4 Problem Formulation and Outline of Thesis

Modelling and control of an unmanned aerial vehicle

Figure 1:Forces on a Propeller

2.1.2 DC motor drivers

Modelling and control of an unmanned aerial vehicle

Photograph 2.1: DC Brushed Motor

Photograph 2.2:Brushless D Motor

Photograph 5:Disassembled Motor

Photograph 6:Crazy Flie Motor

(Scott et al. 2009)

Modelling and control of an unmanned aerial vehicle

2.1.3.1 MEMS Gyroscope

Modelling and control of an unmanned aerial vehicle

Figure 2.1: MEMS Gyroscope

Figure 2: MEMS Gyroscope

2.1.3.2 MEMS Accelerometer

Figure 3: typical MEMS accelerometer construction

2.1.3.3 MEMS barometer

Modelling and control of an unmanned aerial vehicle

Figure 4: MEMS Barometer

Figure 5: MEMS Magnetometer