Sei sulla pagina 1di 136

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
Modelling and control of an unmanned aerial vehicle
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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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
Modelling and control of an unmanned aerial vehicle
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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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).
Modelling and control of an unmanned aerial vehicle
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).

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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
Modelling and control of an unmanned aerial vehicle
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).

Modelling and control of an unmanned aerial vehicle


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
Modelling and control of an unmanned aerial vehicle
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).

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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
Modelling and control of an unmanned aerial vehicle
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.

Modelling and control of an unmanned aerial vehicle


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:

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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
Modelling and control of an unmanned aerial vehicle
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:

Modelling and control of an unmanned aerial vehicle


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.
Modelling and control of an unmanned aerial vehicle
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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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
Modelling and control of an unmanned aerial vehicle
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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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.
Modelling and control of an unmanned aerial vehicle
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>

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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>

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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];

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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:
%%
%%
QuadcopterController.m
%%
%%
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;

Modelling and control of an unmanned aerial vehicle


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]);
surf(a,b,c,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
[a,b,c]=cylinder2P([rodradius,rodradius],10,[xx,yy,zz],[xx+m2x,yy+m2y,zz+m2z])
;
surf(a,b,c,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
[a,b,c]=cylinder2P([rodradius,rodradius],10,[xx,yy,zz],[xx-m2x,yy-m2y,zzm2z]);
surf(a,b,c,'FaceColor','interp','EdgeColor','none','FaceLighting','phong');
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');

Modelling and control of an unmanned aerial vehicle


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.

Modelling and control of an unmanned aerial vehicle


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
%%
%%
%%
%% By: William Hanna
william.hanna@bellaliant.net %%
%%
%%
%% Uses fucntion:
%%
%%
PWMBatteryModifier.m
%%
%% 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 %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Modelling and control of an unmanned aerial vehicle


iv

global mass motordistance Ixx Iyy Izz drag lift qx qy qz qx2 qy2 qz2 wmax p1
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);

Modelling and control of an unmanned aerial vehicle


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

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

Modelling and control of an unmanned aerial vehicle


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 %%
%%
%%
%% By: William Hanna
william.hanna@bellaliant.net %%
%%
%%
%% Uses fucntion:
%%
%%
NA
%%

Modelling and control of an unmanned aerial vehicle


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
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% position,motion and PWM are stored in Global variable for ease %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Global variables for array storage for plotting
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;
%
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

Modelling and control of an unmanned aerial vehicle


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)');
ylabel('displacement(m)');
axis([0 flighttime displacementminus displacementplus]);
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,zArray);
title('z displacement');
xlabel('time(s)');
ylabel('displacement(m)');
axis([0 flighttime displacementminus displacementplus]);
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,xArray,'-',tArray,yArray,'+',tArray,zArray,':');
title('Displacement');
xlabel('time(s)');
ylabel('displacement(m)');
legend('x','y','z');
axis([0 flighttime displacementminus displacementplus]);
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));

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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)');
ylabel('acceleration(m/s/s)');
axis([0 flighttime accelerationminus accelerationplus]);
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,azArray);
title('z acceleration');
xlabel('time(s)');
ylabel('acceleration(m/s/s)');
axis([0 flighttime accelerationminus accelerationplus]);
grid MINOR;
elseif Mode==1
figure;
plot(tArray,axArray,'-',tArray,ayArray,'+',tArray,azArray,':');
title('Acceleration');
xlabel('time(s)');
ylabel('acceleration(m/s/s)');
legend('x','y','z');
axis([0 flighttime accelerationminus accelerationplus]);
grid MINOR;
else

Modelling and control of an unmanned aerial vehicle


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)');
ylabel('displacement(rad)');
axis([0 flighttime angleminus angleplus]);
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,phiArray);
title('pitch displacement');
xlabel('time(s)');
ylabel('displacement(rad)');
axis([0 flighttime angleminus angleplus]);
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,psiArray,'-',tArray,thetaArray,'+',tArray,phiArray,':');
title('Angular Displacement vs Time');
xlabel('time(s)');
ylabel('displacement(rad)');

Modelling and control of an unmanned aerial vehicle


xii

legend('yaw','roll','pitch');
axis([0 flighttime angleminus angleplus]);
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)');
axis([0 flighttime angvelminus angvelplus]);
grid MINOR;
%
subplot(1,3 , 3);
plot(tArray,vphiArray);
title('pitch velocity');
xlabel('time(s)');
ylabel('velocity(rad/s');
axis([0 flighttime angvelminus angvelplus]);
grid MINOR;
elseif Mode ==1
figure;

Modelling and control of an unmanned aerial vehicle


xiii

plot(tArray,vpsiArray,'',tArray,vthetaArray,'+',tArray,vphiArray,':');
title('Angular Velcoity');
xlabel('time(s)');
ylabel('velocity(rad/s)');
legend('yaw','roll','pitch');
axis([0 flighttime angvelminus angvelplus]);
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)');
ylabel('acceleration(rad/s/s)');
axis([0 flighttime angaccminus angaccplus]);
grid MINOR;
%
subplot(1, 3, 3);
plot(tArray,aphiArray);
title('pitch acceleration');

Modelling and control of an unmanned aerial vehicle


xiv

xlabel('time(s)');
ylabel('acceleration(rad/s/s)');
axis([0 flighttime angaccminus angaccplus]);
grid MINOR;
elseif Mode ==1
figure;
plot(tArray,apsiArray,'',tArray,athetaArray,'+',tArray,aphiArray,':');
title('Angular Acceleration');
xlabel('time(s)');
ylabel('acceleration(rad/s/s)');
legend('yaw','roll','pitch');
axis([0 flighttime angaccminus angaccplus]);
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);
plot(tArray,PWM2Array);
title('Motor 2 PWM');
xlabel('time(s)');
ylabel('duty cycle');
axis([0 flighttime 0 1]);
%
subplot(1, 4, 3);
plot(tArray,PWM3Array);
title('Motor 3 PWM');
xlabel('time(s)');
ylabel('duty cycle');
axis([0 flighttime 0 1]);
%
subplot(1, 4, 4);
plot(tArray,PWM4Array);
title('Motor 4 PWM');
xlabel('time(s)');
ylabel('duty cycle');
axis([0 flighttime 0 1]);
end

titleheader.m
function titleHeader();
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Modelling and control of an unmanned aerial vehicle


xv

%%
%%
%%
Display a title header to make user interface look nice
%%
%%
%%
%% By: William Hanna
william.hanna@bellaliant.net %%
%%
%%
%% 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
%%
%%
%%
%% By: William Hanna
william.hanna@bellaliant.net %%
%%
%%
%% 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)
%%

Modelling and control of an unmanned aerial vehicle


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)
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global mass motordistance Ixx Iyy Izz drag lift qx qy qz qx2 qy2 qz2 wmax p1
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]?';
str = input(prompt,'s');
if isempty(str)
str = 'Y';
end
if (str=='n')|(str=='N')
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);

Modelling and control of an unmanned aerial vehicle


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]?';
str = input(prompt,'s');
if isempty(str)
str = 'Y';
end
if (str=='n')|(str=='N')
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]?';
str = input(prompt,'s');
if isempty(str)
str = 'Y';
end
if (str=='n')|(str=='N')
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'

Modelling and control of an unmanned aerial vehicle


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]?';
str = input(prompt,'s');
if isempty(str)
str = 'Y';
end
if (str=='n')|(str=='N')
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

QuadcopterController.m
function QuadcopterController(PWM1,PWM2,PWM3,PWM4)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
QuadcopterController
%%
%% Calculates controller reaction given current and desired values %%
%%
%%
%% By: William Hanna
william.hanna@bellaliant.net %%
%%
%%
%% 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
%%
%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% position,motion and PWM are stored in Global variable for ease %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Modelling and control of an unmanned aerial vehicle


xix

% IF PWMMode=1, running off of input data for PWM, not controll values
%
% Global variables for array storage for plotting
global mass motordistance Ixx Iyy Izz drag lift qx qy qz qx2 qy2 qz2 wmax p1
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 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 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
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%

Modelling and control of an unmanned aerial vehicle


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;

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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);

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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);
motorPowerM2 = limitThrust(thrust);
motorPowerM3 = limitThrust(thrust);
motorPowerM4 = limitThrust(thrust);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(thrust);
motorPowerM2 = limitThrust(thrust);
motorPowerM3 = limitThrust(thrust);
motorPowerM4 = limitThrust(thrust);
#endif

Mod II
#ifdef QUAD_FORMATION_X
Modelling and control of an unmanned aerial vehicle
i

roll = roll >> 1;


pitch = pitch >> 1;
motorPowerM1 = limitThrust(0);
motorPowerM2 = limitThrust(thrust);
motorPowerM3 = limitThrust(0);
motorPowerM4 = limitThrust(thrust);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(0);
motorPowerM2 = limitThrust(thrust);
motorPowerM3 = limitThrust(0);
motorPowerM4 = limitThrust(thrust);
#endif

Mod III
#ifdef QUAD_FORMATION_X
roll = roll >> 1;
pitch = pitch >> 1;
motorPowerM1 = limitThrust(thrust);
motorPowerM2 = limitThrust(0);
motorPowerM3 = limitThrust(thrust);
motorPowerM4 = limitThrust(0);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(thrust);
Modelling and control of an unmanned aerial vehicle
ii

motorPowerM2 = limitThrust(0);
motorPowerM3 = limitThrust(thrust);
motorPowerM4 = limitThrust(0);
#endif

ModIV-I
#ifdef QUAD_FORMATION_X
roll = roll >> 1;
pitch = pitch >> 1;
motorPowerM1 = limitThrust(thrust);
motorPowerM2 = limitThrust(0);
motorPowerM3 = limitThrust(0);
motorPowerM4 = limitThrust(0);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(thrust);
motorPowerM2 = limitThrust(0);
motorPowerM3 = limitThrust(0);
motorPowerM4 = limitThrust(0);
#endif

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


ii

takeoffthrust=GuiConfig().get("takeOffThrust")
takeoff=GuiConfig().get("takeOff")
ground=GuiConfig().get("ground")
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
takeoff=GuiConfig().get("takeOff")
ground=GuiConfig().get("ground")
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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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):

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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()

Modelling and control of an unmanned aerial vehicle


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)))
self.helper.cf.param.add_update_callback(
group="flightmode", name="ratepid",
cb=(lambda name, checked:
self.ratePidRadioButton.setChecked(eval(checked))))
self.helper.cf.param.add_update_callback(
group="flightmode", name="althold",
cb=(lambda name, enabled:
self.helper.inputDeviceReader.setAltHold(eval(enabled))))

Modelling and control of an unmanned aerial vehicle


x

self.helper.cf.param.add_update_callback(
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)

Modelling and control of an unmanned aerial vehicle


xi

def loggingError(self):
logger.warning("Callback of error in LogEntry :(")

def _motor_data_received(self, data, timestamp):


self.actualM1.setValue(data["motor.m1"])
self.actualM2.setValue(data["motor.m2"])
self.actualM3.setValue(data["motor.m3"])
self.actualM4.setValue(data["motor.m4"])

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

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


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(

Modelling and control of an unmanned aerial vehicle


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"))
lg.addVariable(LogVariable("motor.m2", "uint32_t"))

Modelling and control of an unmanned aerial vehicle


xvii

lg.addVariable(LogVariable("motor.m3", "uint32_t"))
lg.addVariable(LogVariable("motor.m4", "uint32_t"))

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:
logger.warning("Could not setup logconfiguration after "
"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)

Modelling and control of an unmanned aerial vehicle


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:
logger.warning("Could not setup logconfiguration after "
"connection!")
lg = LogConfig("AltHold", 50)
lg.addVariable(LogVariable("altHold.target", "float"))

self.logAltHold = self.helper.cf.log.create_log_packet(lg)

Modelling and control of an unmanned aerial vehicle


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:
logger.warning("Could not setup logconfiguration after "
"connection!")

def disconnected(self, linkURI):


self.ai.setRollPitch(0, 0)
self.actualM1.setValue(0)
self.actualM2.setValue(0)
self.actualM3.setValue(0)
self.actualM4.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):

Modelling and control of an unmanned aerial vehicle


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())
if (self.isInCrazyFlightmode == True):
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())
if (self.isInCrazyFlightmode == True):
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())
if (self.isInCrazyFlightmode == True):
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)

Modelling and control of an unmanned aerial vehicle


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)
self.M2label.setEnabled(enabled)
self.M3label.setEnabled(enabled)
self.M4label.setEnabled(enabled)

Modelling and control of an unmanned aerial vehicle


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(

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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

__author__ = 'Bitcraze AB'


__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 *

Modelling and control of an unmanned aerial vehicle


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",

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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},

Modelling and control of an unmanned aerial vehicle


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.axisDetect = ""
self.btnDetect = ""

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

Modelling and control of an unmanned aerial vehicle


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):

Modelling and control of an unmanned aerial vehicle


xxx

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


self.combinedButton.setDisabled(True)
self.combinedDetection = 1
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.axisDetect = ""
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.axisDetect = ""
self.checkAndEnableSave()
if (self.box != None):
self.cancelButton.click()
self.combinedDetection = 0
elif self.combinedDetection == 1:

Modelling and control of an unmanned aerial vehicle


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
self.combinedDetection = 2
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 = ""
self.checkAndEnableSave()
if (self.box != None):

Modelling and control of an unmanned aerial vehicle


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:

Modelling and control of an unmanned aerial vehicle


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"

Modelling and control of an unmanned aerial vehicle


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):

Modelling and control of an unmanned aerial vehicle


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())
self.checkAndEnableSave()

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]:

Modelling and control of an unmanned aerial vehicle


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):

Modelling and control of an unmanned aerial vehicle


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()

Modelling and control of an unmanned aerial vehicle


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()

Modelling and control of an unmanned aerial vehicle


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

__author__ = 'Bitcraze AB'


__all__ = ['PyGameReader']
import pygame
from pygame.locals import *
import time

class PyGameReader():

Modelling and control of an unmanned aerial vehicle


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)

Modelling and control of an unmanned aerial vehicle


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"):
key = self.inputMap[index]["key"]
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

Modelling and control of an unmanned aerial vehicle


xlii

if e.type == pygame.locals.JOYBUTTONUP:
index = "Input.BUTTON-%d" % e.button
try:
if (self.inputMap[index]["type"] == "Input.BUTTON"):
key = self.inputMap[index]["key"]
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 = {}

Modelling and control of an unmanned aerial vehicle


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

Modelling and control of an unmanned aerial vehicle


xliv