DEGREE PROJECT IN TECHNOLOGY, FIRST CYCLE, 15 CREDITS
STOCKHOLM, SWEDEN 2017
Quadrotor in Y4 Configuration
RICHARD SAGRÉN ALEXANDER YUN
KTH ROYAL INSTITUTE OF TECHNOLOGY
Quadrotor in Y4 Configuration
RICHARD SAGRÉN ALEXANDER YUN
Bacherlor’s Thesis in Mechatronics Supervisor: Didem Gürdür Examiner: Nihad Subasic
TRITA MMK 2017:12 MDAB 630
Abstract
Small aerial vehicles are increasing in popularity and the market is rapidly growing, especially for the ones called quadrotors or quadcopters due to their simplicity. Our project for this bachelor thesis was to build, investigate and fly a quadcopter in Y4 configuration. In this case the aircraft comprises one centered main rotor for lift and three boom rotors facing upwards for stability and control. Light and rigid material were chosen for the construction, an Arduino Uno R3 board as flight controller and an open source software to control it. It can be concluded that a similar construction is feasible, and is able to fly. In this specific project stability is the main issue.
Sammanfattning
Quadrotor i Y4 Konfiguration
Små flygande farkoster blir allt vanligare och det är en snabbt växande mark- nad, speciellt för quadcoptrar eller quadrotors som de även kallas, då dessa har en simpel mekanisk konstruktion och är relativt billiga att tillverka. Detta kandidatexamensprojekt går ut på att konstruera, bygga, undersöka och flyga en quandrotor i Y4-format. I detta fall består denna av en huvudrotor för att generera lyftkraft samt tre armar, var och en med en mindre rotor riktad uppåt, för att ge stabilitet och styrförmåga. Lätta och styva material har använts för konstutionen, en Arduino Uno R3 mikrokontroller till flygdator och en mjuk- vara med öppen källkod för att kontrollera denna. Den slutsats som dras är att en sådan konstruktion är rimlig, och kan flyga. I detta specifika projekt är stabilitet det största problemet.
iv
Preface
We would like to thank our supervisor Didem Gürdür for her guidance and support.
We would also like to thank Daniel at Rcflight.se for an excellent customer service and technical advice. Our special thanks to the course and prototype assistants who taught us everything from 3D-printing and laser cutting to finding our way through the well equipped laboratories.
Richard Sagrén & Alexander Yun Stockholm, May 2017
Contents
Abstract iii
Sammanfattning iv
Preface v
Contents vii
Nomenclature xi
1 Introduction 1
1.1 Background . . . 1
1.2 Purpose . . . 2
1.3 Scope . . . 3
1.4 Method . . . 3
2 Theory 5 2.1 Initial Research . . . 5
2.1.1 A Basic Understanding . . . 5
2.1.2 The Transition . . . 6
2.2 Rotor placement . . . 6
2.3 The PID Control . . . 7
3 Demonstrator 9 3.1 Considered Factors . . . 9
3.2 Hardware . . . 9
3.2.1 The Frame . . . 9
3.2.2 Power and Propulsion . . . 11
3.2.3 Sensors and Radio . . . 12
3.3 The Flight Controller and Software . . . 13
3.3.1 Arduino Uno . . . 13
3.3.2 Modified MultiWii . . . 13
3.3.3 The Software Development . . . 14
3.3.4 Sketches of Different Parts . . . 15
3.3.5 The ESC Output . . . 17
4 Discussion and conclusions 19
5 Recommendations and future work 21
Bibliography 23
Appendices
A Images and Drawings 27
B Arduino Code 39
C Modified MultiWii code 45
D Data sheets 47
viii
List of Figures
1.1 The de Bothezat helicopter, 1923. One of the earliest quadrotors. [Avi-
astar, 2007] . . . 1
1.2 The triangular quadrotor. [Driessens and Pounds., 2013] . . . 2
2.1 The block diagram of a PID controller [Radhesh, 2008] . . . 8
3.1 A first idea and design concept. The image is made in Solid Edge ST8 and refined in KeyShot 5. . . 10
3.2 The second design concept. Drawn in Solid Edge ST8 and refined in KeyShot 5. . . 10
A.1 The Details of The Final Prototype. . . 27
A.2 This is the lower part of the frame, spacers and top disc not included. . 28
A.3 Here the entire frame is shown, with motors and propellers mounted for illustrative purpose. . . 28
A.4 To the left the bottom disc can be seen, in the middle the top disc and to the right the three arm motor holders. Made in Solid Edge ST8 and Adobe Illustrator CC. . . 29
A.5 GY-85 9DOF IMU Sensor Module [SainSmart, 2013] . . . 29
A.6 Futaba T7C Transmitter and R617FS Receiver [rcmart.com, 2010] . . . 30
A.7 Arduino Uno board [sparkfun.com, 2015] . . . 30
A.8 The General Connection between Arduino Uno and ESC Array, IMU Sensor. Made in Fritzing 0.9.3b. . . 31
A.9 The Flow Chat of Y4 Quadrotor Software Development. . . 32
A.10 Software Overview . . . 33
A.11 ATmega168/328-Arduino Pin Mapping [arduino.cc, 2010] . . . 33
A.12 Coordinate System of Accelerometer [Starlino, 2009] . . . 34
A.13 Coordinate System of Gyroscope [Starlino, 2009] . . . 34
A.14 The result of using complementary filter for Angle of Pitch shown in Arduino Serial Plotter. . . 35
A.15 The result of using complementary filter for Angle of shown in Arduino Serial Plotter. . . 35
A.16 Rotor Velocity Combinations of Y4 [Driessens and Pounds., 2013] . . . . 36
A.17 The Orientation System [Redshift Labs, 2012] . . . 36
A.18 The Draw of Main Rotor: T-Motor MT2814-19 KV400. [rctigermo- tor.com, 2013] . . . 37 A.19 The Data of Main Rotor: T-Motor MT2814-19 KV400. [rctigermo-
tor.com, 2013] . . . 37 A.20 The Draw and Data of Boom Rotor: Emax MT2208 KV2000. [emax-
model.com, 2014] . . . 38
List of Tables
D.1 The acquired parts for the Y4 Quadrotor. . . 47
x
Nomenclature
Symbols
Symbols Description [U nit]
A Area [m2]
χ Distance of vena contracta [m], [see apendix]
D Diameter [m]
d Distance between the center of a rotor and the tip of the next rotor [m]
ρ Air density [kg/m3]
r Radius [m]
Abbreviations
Abbreviation Description
BEC Battery Eliminator Circuit
CAD Computer Aided Design
CF Carbon Fiber
DOF Degree Of Freedom
ESC Electronic speed control
FASST Futaba Advanced Spread Spectrum Technology
FC Flight Controller
GLONASS Globalnaja Navigatsionnaja Sputnikovaja Sistema, a positioning system GPS Global Positioning System
GUI Graphical User Interface
LiPo Lithium Polymer
IMU Inertial Measurement Unit
I/O Input/Output
PCB Printed Circuit Board
PID Proportional-Integrative-Derivative PLA Polylactide, plastic
PWM Pulse Width Modulation
QC quadcopter
UAV Unmanned Aerial Vehicle VTOL Vertical takeoff and landing
Chapter 1
Introduction
1.1 Background
The quadrotor is an aerial vehicle capable of vertical take off and landing, VTOL, equipped with four motors connected to one propeller each. Early attempts were made in the 1920s and 30s, see Figure 1.1, with the first one as early as 1907 [Young, 1982], and were among the first manned vehicles to successfully perform a vertical takeoff. Although functioning, these were too complex to operate and first after the invention of the microcontroller they started to reemerge. Now a days these quadrotors are common elements in our lives, from RC-enthusiasts, air circuit competitions [MultiGP, 2017] and professional aerial photography all the way to land survey and delivery jobs, see [sUAS News, 2017]. So far these are almost exclusively unmanned and therefore belongs to the group of unmanned aerial vehicles, abbreviated UAV. Quadrotors comes in a variety of configurations with X, + and H being the most common. This means that the frame resembles one of these forms and the rotors are situated at the extremities of the former. The Y configuration, indeed resembles a Y and usually has two rotors on it’s tail, one pointing upwards and one downwards [Oscar Liang, 2013]. These air crafts are known for being very agile and manoeuvrable, however they come with some major drawbacks: flight time and lift force.
Figure 1.1. The de Bothezat helicopter, 1923. One of the earliest quadrotors.
[Aviastar, 2007]
CHAPTER 1. INTRODUCTION
Therefore it is of interest to investigate new designs and concepts to handle these drawbacks and improve the capabilities of the quadrotor family, a task here under- taken by the authors, however not alone. Recently attempts at other configurations have been made, like the Y4 for example [Driessens and Pounds., 2013].
1.2 Purpose
As above mentioned this project originates in the desire to improve the quadcopter.
After finding a scientific report on the subject [Driessens and Pounds., 2013] the final idea for this project was decided. The authors of the previously mentioned article built their thesis around the fact that a large rotor is more efficient than a small one and constructed an aircraft with one large rotor and three small tilted ones, as seen in Figure 1.2. This resulted in a large gain in efficiency and they called it the triangular quadrotor or the Y4 configuration, this will be investigated in chapter two. After reading this it was decided to try another approach: a Y4 configuration, but instead of tilting the boom rotors, keeping them pointing upwards in the direction of the main rotor, hoping to gain even more lift. The project was aimed to see if it was still able to maneuver, able to maintain a decent flight and how such a vehicle would be constructed. Therefore the following questions were researched:
• How could a quadcopter in Y4 configuration, with all rotors facing upwards, be constructed with an Arduino microcontroller and limited hardware?
• Which are the important factors to achieve control of the prototype?
Figure 1.2. The triangular quadrotor. [Driessens and Pounds., 2013]
2
1.3. SCOPE
1.3 Scope
To answer the research questions some limitations had to be imposed. This concept is limited to the simplest construction paired with the ability to lift and land. It will also aim at testing the throttle, pitch, roll and yaw commands of the vehicle.
All these under the control of the authors and without automation, except for a control circuit to handle stability during flight. This project will not contain any optimizing or theoretically deep calculations. Basic mechanics and approximations will be used for dimensioning, information will be gathered from scientific reports, literature and expert advice.
1.4 Method
To answer the first research question mentioned in section 1.2 the quadcopter was first created as a model in Solid Edge ST8, then the parts were the manufactured and assembled. Hobby grade motors were selected similarly to those used in [Driessens and Pounds., 2013], and chosen to be optimally driven by the same battery. See section 3.2.2 for the chosen hardware. An Arduino Uno microcontroller was set up as the flight controller.
To answer the second research question software was implemented. The Arduino was programmed by using an open source program [MultiWii, 2015] containing a PID controller and modifying it to accommodate the construction of the Y4 and its various components.
Chapter 2
Theory
This chapter focuses on presenting the conducted research and providing the nec- essary understanding of the various aspects needed to build and program a quad- copter. Specifically in Y4 configuration. Before approaching the theory deeper some initial questions had to be answered. The following section lists those ques- tions along with the answers that were uncovered.
2.1 Initial Research
2.1.1 A Basic Understanding
How does a quadcopter achieve stability, with which sensors and how many? What are the differences between the various tiers of quadcopters, price and performance wise?
To achieve stability in a quadrotor aerial vehicle the dynamic system is modelled.
After modelling the subsequent equations can be solved and a controller can be applied, as seen in [Li and Li, 2011]. These controllers are typically PID controllers [Duc et al., 2015], but others based on e.g Lyapunov theory or fuzzy logic [Filatov et al., 2016], are also commonly used. These controllers use the input from a desired control signal and various sensors to regulate the output, destined for the actuators which in this case are the motors of the aircraft. In this way the four rotors are used to achieve impressive stability and agility, since only their individual speed needs to be changed by the on-board micro computer for manoeuvres such as pitch, roll, yaw and altitude control.
The most common sensors used are a three axis accelerometer and a three axis gyroscope to handle six degrees of freedom, abbreviated DOF, these two are usually combined to form an IMU or inertial measurement unit [Hobden, Andrew, 2015].
Other common sensors are the magnetometer, pressure sensor or barometer and GPS/GLONASS. Some quadrotors even use ultrasonic, photo- or video-graphic, in- fra red and laser equipment to perform specific tasks [Wei, 2016]. To be able to
CHAPTER 2. THEORY
control the vehicle it is also equipped with a receiver of some kind, usually radio, wifi or bluetooth depending on the intended use. Other more rarely used ones are temperature and humidity sensors to mention a few.
The low cost quadcopters intended for recreational use or as toys commonly use a single IMU unit, with a three axis accelerometer and a three axis gyroscope. Some of the more advanced quadcopters aimed at hobbyists, usually equipped with a camera, in addition to an IMU unit also have a barometer to measure altitude, a compass to ensure heading and a GPS unit. With these additional sensors they can provide smart functions like "return home", or be used for planned flight missions such as surveillance, aerial photography, etc.
On the more advanced side of the spectrum we find the multirotors intended for professional use. In addition to the various sensors needed for their specific pur- pose they also come with double IMUs, double barometers, double GPS and double receivers to ensure reliable performance. These copters are many times equipped with a number of proximity sensors, of different kinds, to avoid obstacles since a crash could be very expensive.
2.1.2 The Transition
How can the knowledge of a regular quadcopter be translated to this project? What needs to be added or subtracted?
The kinematic equations, how the regular quadcopter (QC from now on) and its positional frame moves about the "earth-fixed" inertial frame are the same, since the system has the same degrees of freedom. From here we can look at the QC and see which rotors are contributing to which motion, and then we can translate it to our concept (named Y4 here after). It was discovered that the Y4 is similar to the QC in this aspect when it comes to pitch and roll, but differs slightly more when it comes to yaw. This change is then applied in the control system of the copter.
Regarding the add and subtract part, here the two considered fliers (QC and Y4) consist of the same parts, except for one rotor. The sensors must be the same since the same DOFs will be controlled. A three axis accelerometer and a three axis gyroscope have been chosen as the essential sensors for this project.
2.2 Rotor placement
This section is written for the purpose of describing how the placement of motors and propellers is determined and the principle of counter torque by drag torque.
6
2.3. THE PID CONTROL
Propellers rotating at high speed produce a wake that simplified can be divided into three fields: The wake itself, the outer field where the wake is minimal or non existent, and a boundary layer in between. In a configuration with one rotor located upstream and a second one downstream the placement of the lower is affected by the wake of the upper one. As the authors of [Driessens and Pounds., 2013] men- tioned, the Y4 has the same kind of configuration, however that quadrotor used tilted boom rotors placed beneath the main one. To minimize wake, boom rotors were placed around the vena contraca, which is the smallest area of turbulence that occurs downstream of the rotor plane. This is represented in equation 2.1, were χ is the location of the vena contracta from the rotor and D is the rotor diameter.
χ = D/4 [m] (2.1)
When placing propellers side by side the following approximation [Driessens and Pounds., 2013] is used, the propellers should be placed at least the distance of the rotor radius times the square root of two apart from each other, see equation 2.2,
d = r√
2 [m] (2.2)
Where d is the shortest distance from the center of one rotor, to the tip of the next.
This is to minimize the effect of blade vortices created by the spinning blade.
All multirotors and helicopters rely on counteracting torque to control yaw. Heli- copter rely on a tilted tail rotor to counteract the main rotors torque by pushing or pulling air, that is, using the lift force. Multirotors commonly use drag torque to achieve torque equilibrium. When a motor spins it exerts an equal force on the body it’s attached to as that needed to rotate its shaft, according to Newton’s second law of motion. When a rotor or propeller is added the force needed increases and by extension the torque. To reach a stable flight one must achieve torque equilibrium, calculated bellow discarding however thermal changes.
2.3 The PID Control
The Y4 quadrotor is an inherently unstable system which basically need an au- tomatic control system like a PID feedback controller to help the operator. The main characteristics of the PID control (see Figure 2.1) are stability (P and I reduce system stability, D improve system stability), accuracy (P and I improve steady-state accuracy, D has no effect) and speed (P and D increase response speed,
CHAPTER 2. THEORY
I reduces response speed). The equation of the PID control is shown bellow, see equation 2.3.
u(t) = Kpe(t) + Ki
Z t 0
e(τ )dτ + Kdde
dt (2.3)
In this equation, Kp is multiplied by the error e(t) to eliminate the current error;
the integral term Ki is multiplied by the integral of the error e(τ ) to eliminate the accumulation of historical errors. The term Kd is multiplied by the differential of the error e(t) to eliminate the error variation, which means to ensure that the error is constant. The P control is obviously the core element of the adjustment system and it can be used to eliminate the current system error. The existence of I control is auxiliary and it can be used to eliminate the residual error of the P control. The D control is used to enhance the system stability, increase the degree of system damping and modify the P I curve to make the overshoot less. Its existence is also auxiliary as the I control.
Figure 2.1. The block diagram of a PID controller [Radhesh, 2008]
The PID algorithm, which is widely used for quadrotor, is a linear controller. To control the quadrotor, controlling the angle is the simplest way.
8
Chapter 3
Demonstrator
On the path to answering the research questions stated in section 1.2 several chal- lenges were encountered. The purpose of this chapter is to explain these, how they were confronted and finally the outcome.
3.1 Considered Factors
These were the considered challenges to solve:
• What shape and size should the Y4 have and how will it be constructed to be acceptably light weight and have a high enough rigidity for the purpose?
• What motors and which power source should be used to lift and control it?
• How is the flight controller code written, can an open source code be imple- mented, if so which?
• Which sensors will be used?
• Which communication protocol will be used and what kind of receiver and transmitter pairs well with it, for this close range application?
3.2 Hardware
In this section the mechanical aspects of the construction are presented and the road to answering the above listed problem formulations.
3.2.1 The Frame
The initial concept for the Y4-copter is shown here in Figure 3.1. It includes ducted fans as the arm rotors and was mostly intended to show the first idea for this project.
CHAPTER 3. DEMONSTRATOR
Figure 3.1. A first idea and design concept. The image is made in Solid Edge ST8 and refined in KeyShot 5.
This concept was created part by part and then assembled, all in Solid Edge ST8.
Afterwards a render was made in KeyShot 5, where colours and shading were ap- plied to give a better understanding of the authors idea. It was quickly abandoned as it contained to many design elements, which are not relevant for this track. A new concept began to emerge, see Figure 3.2, made with the same software as the first one. It only includes the main shape of the quadrotor. The circular base was chosen for its ease of construction and it’s excellent ability to accommodate the three arms. It was decided to put the arms at 120◦ intervals for an even force distribution. Based on this the manufacturing began.
Figure 3.2. The second design concept. Drawn in Solid Edge ST8 and refined in KeyShot 5.
It was decided not to use ducted fans for the arm motors, like seen in Figure 3.1, due to the desire for a simpler construction. It was decided to use carbon fiber tubes for the arms, this since they are very light and posses excellent rigidity. A commonly found dimension, 12 ∗ 10 mm, that being twelve millimeter as the outer
10
3.2. HARDWARE
diameter and ten millimeter as the inner. After this first choice was made it then came to the plastic attachments. These were designed in Solid edge ST8, processed for the 3D-printer with Curo 2.4 slicing software. The final stage was printing using an Ultimaker 2 3D-printer and the material being in PLA bioplastic. During the de- sign phase a yellow colour was used to highlight these parts, however for the actual printing chartreuse became the colour of choice due to higher visibility compared to the yellows filaments available. To attach the motors and the body plate 2,5mm (M3) holes were drilled through the attachments, and the same dimension of screws were used to fixate all parts.
The two main body discs as well as the motor holders were made out of acrylic glass due to good rigidity and the possibility to cut and engrave them using a laser cutter.
A sheet of six millimeter thick acrylic glass was used for this purpose and cut into circles of 150mm diameter, in appendix A, Figure A.4 the images fed to the laser cutter can bee seen. The contours were cut and the remaining lines engraved to facilitate mounting.
To unite the bottom and top disc six hexagonal 45mm long aluminum spacers were used, which were bought drilled and threaded in both ends to facilitate fix- ation to the body using the same kind of M3 screws that were used for the rest of the body. Aluminum was chosen for its light weight and, for this purpose, high strength. The assembled bottom frame including the above mentioned parts can be seen in appendix A, Figure A.2 and the complete frame assembly is shown in Figure A.3, appendix A. This construction was deemed sufficiently rigid, durable and light weight by the authors.
3.2.2 Power and Propulsion
To decide which motors should be used the initial approach was to emulate the mo- tor choice of [Driessens and Pounds., 2013]. The goal was one larger motor to power a relatively large rotor, and three smaller ones to power the small boom rotors. The final choice was made based on the first considerations and the comparably low price of the components. An other important aspect when choosing different sized motors is the recommended power source, the motors chosen all supports 14,8V.
See appendix D, Table D for a complete list parts used.
The propellers were chosen to match these motors, and several alternatives for test- ing were purchased. The main rotor was decided to be rotating counter clockwise and propellers in sizes 18, 16, 14, 12 times 5.5 inches were purchased. These num- bers represent the diameter, D, and the vertical distance it would travel during one revolution. This was done since the authors didn’t have a clear understanding of the fliers requirements at the time. For the boom rotors five times thee inch propellers were purchased, a large variety of smaller ones could be obtained at the institution.
CHAPTER 3. DEMONSTRATOR
The main rotor to be used was chosen as the 16 ∗ 5.5 one to maximize lift force, since the effective area of lift is compromised by the large body of the quadcopter.
By converting the size to metric and utilizing equation 2.2 the smallest distance at which the boom rotor tips must be located relative to the center of the quadrotor was calculated to 287.4 mm. Adding the radius of the smaller rotor got the total recommended distance to 350.9 mm. For the vertical distance equation 2.1 is used and yelded 101.6 mm as a result.
It however the last one was not taken in to consideration since it was decided to follow equation 2.2 and the boom rotors would therefore be far from the vortex of the main rotor.
The battery simply has to match the motors and be able to provide the required voltage and current. The common requirements of the motors regarding voltage is 14,8V, and the total maximum current required is approximately 85A. The choice of battery lands on a lithium polymer (abbreviated as LiPo) battery, one of the most common types in RC environment, which offers outstanding performance and delivers high energy storage to weight ratios. In this project a TATTU 1300mAh 4s 75C is used.
In order to control the motors while providing electricity from the battery, the ESC (electronic speed controller), will be used. It takes a PWM signal from the Arduino, adds voltage and current and sends the signal to the motor in the form of an electric pulse. Four EMax BLHeli Series ESC’s were used, see appendix D, Table D, with a maximum current rating of 30A. This setup was considered sufficient to provide enough lift force for the quadrotor to fly.
3.2.3 Sensors and Radio
In this project an IMU sensor chip will be used. IMU sensors are electronic devices based on multiaxis combinations of precision MEMS (Micro-Electro-Mechanical Systems) sensors, most commonly gyroscopes, accelerometers, magnetometers, and pressure sensors. A GY-85 9DOF IMU Sensor Module, see Figure A.5, is chosen because of its low cost and a combination of a gyroscope, an accelerometer and a magnetometer, monitoring three axis each. It also supports the I2C communication protocol, which is suitable for use with an Arduino microcontroller. The purpose of the GY-85 is to provide necessary data for the PID regulator in order to stabilize the quadrotor.
Radio communication is employed to maneuver the Y4 quadcopter. It includes two parts: a stand along transmitter and a receiver which is attached to Arduino Uno board. Both devices work at 2.4GHz with PWM signals. A Futaba T7C 2.4GHz
12
3.3. THE FLIGHT CONTROLLER AND SOFTWARE
7-Channel Transmitter, see Figure A.6, is chosen because 5 or more channels will be used in the project. A Futaba R617FS 2.4GHz FASST 7-Channel Receiver is used since it with the transmitter and it exceeds the required specifications of short distance and compatibility with the Arduino. During the configuration of the flight controller, channel 1, 2, 3, 4 are respectively assigned to Roll, Pitch, Throttle and Yaw. Channel 7 is used for safety interruption, instantly shutting down all motors by using a switch.
3.3 The Flight Controller and Software
The Flight Controller as previously mentioned is in this case an Arduino Uno R3 board. In order to control and handle the input and output signals, an extensive program is required. Implementation of an existing open source code has been chosen for the purpose. Several of them are available on the internet, but two of them were considered to be acceptable candidates: Ben Ripley’s quadrotor code [Ben Ripley, 2014] and the MultiWii [MultiWii, 2015]. The last one is finally cho- sen due to the extensive amount of information available about modification and code structure. To incorporate the Y4 quadcopter a few modifications needed to be made, see chapter 3.3.2.
3.3.1 Arduino Uno
First introduced in 2005, the Arduino has gone through numerous iterations, revi- sions and improvements. The Arduino Uno (see Figure A.7), like its predecessors, is an all-in-one development board. It contains an Atmel ATmega328P microcon- troller running at 16 Mhz. The ATmega328P on the Uno board isn’t an empty chip;
it contains a small bootloader program for use with the Arduino development editor.
The five volt regulator provides up to 800 mA of current; the 3.3 volt regulator pro- vides up to 50 mA. Connection pins are provided for both 5V and 3.3V regulated outputs. These pins can be used to power low current components such as ultra- sonic sensors or accelerometers. Further more, there are six analog Input/Output (I/O) pins and 14 digital I/O pins on board. The analog pins are connected to an internal ten-bit Analog to Digital Converter (ADC), to read voltages from sensors and other devices. All I/O pins can be used as digital outputs, and can sink or source up to 40 mA.
3.3.2 Modified MultiWii
The MultiWii/WinGUI [MultiWii, 2015] is an open source software that incorpo- rates controls for airplanes, helicopters, multirotors and other flying vehicles. It’s
CHAPTER 3. DEMONSTRATOR
is distributed under the GPL license [Free Software Foundation, Inc., 2007]. It in- cludes support for a large number of flight controllers, sensors, navigational features to mention a few. In this project only the most basic features are used. The modi- fications made are listed bellow. The Code itself can be seen in appendix C.
First, the Y4 was defined in the def.h file. At line 235 where the various quadrotors are listed the Y4 was also added under the name Y4TOP. This part of code defines how many motors the copter have. Next the Y4 was added in the same document starting at line 1745, where the different types of multirotor are listed for the GUI.
Then, the Y4TOP was added in the config.h file where the options to be used for the current compilation resides. The Y4 was defined at line 41. The last definition to be added was the motor mix [Adam Polak, 2012] for the output of the PID con- troller. The mix is simply the position of the rotors in a two dimensional Cartesian plane relative to the flight controller in origo. This modification was made in the output.ccp file from line 1153 to 1157.
The next step was to configure the output to match the ESC’s and to handle the input from the transmitter correctly. In the multiwii.h file the MINCHECK and MAXCHECK values were changed from 1100 and 1900 to 1050 and 1820. These two values are used to limit incorrect input from the transmitter and since the val- ues received were outside this range it was impossible to arm and fly the quadrotor.
After that, the output to the ESC was decided. Therefor the values of MINTHROT- TLE, MAXTHROTTLE and MINCOMMAND in the config.h file had to be matched to the calibration of the ESC. Trial and error proved that the values 1010, 2000 and 1000 respectively seemed to match the calibration of the ESC.
Finally, an LED was incorporated in the build. To use it as an indicator for arming and disarming the copter line 884 in config.h was modified. Armed time warning is located there, a function to provide a buzzer on pin 13 for an alarm to be activated after the copter has been active for a certain amount of time. arming itself is the act of enabling the motors to rotate. The value was changed from 330 to one to ensure arm indication.
3.3.3 The Software Development
Using the modified MultiWii is probably not the ultimate solution, but a shortcut to test the Y4 quadrotor. However the work of designing a flight controller software has been initially started, a flow chart of it is already drawn and shown in appendix A, see Figure A.9 and Figure A.10. Some sketches has been completed in order to test the various systems on the Y4 quadrotor. In this section, the basic concept about these sketches will be described and the relations between code and hardware
14
3.3. THE FLIGHT CONTROLLER AND SOFTWARE
are be explained if required.
No matter which software will be used, the proper code to handle interrupts of ATmega328P must be written to process the signals sent by transmitter. There are only two external interrupt pins, INT0 and INT1, on the ATmega328p in the Arduino Uno board, but in practice four or more channels are needed to control the Y4 Qudrocopter. Two popular solutions are chosen in different software. To toggle any enabled PCINT[7:0] will trigger the pin change interrupt PCI0. It’s the first method and can be found in the manual of Atmega328P, see appendix C. Because the pins of Atmega328P and the pins on the Arduino Uno board are different, see Figure A.11, any programmer who decide to choose this method must know the relationship between them.
The second method to handle the input signals of receivers is using the PinChangeInt Library [arduino.cc, 2012]. The 3rd party Arduino library called PinChangeInt can be utilized to allow programmer to take advantage of “pin change” interrupts, which are available on all input and output pins of the Arduino Uno. It can be enabled on any or all of the Arduino Uno’s signal pins. They are triggered equally on RISING or FALLING signal edges, so it depends on the interrupt code to appoint the proper pins to receive interrupt signals, to determine what happened (which pin? ...did the signal rise, or fall?), and to handle them properly. The library is designed to handle the Arduino Uno’s pin change interrupts as quickly and reasonably as possible.
3.3.4 Sketches of Different Parts
The sketches of Receiver.ino and IMU.ino are developed first. They both have been tested and satisfied results have been obtained. There’re four channels to be oc- cupied in order to send signals of roll, pitch, throttle and yaw. A few basic but important factors must be explained before programming an IMU [Starlino, 2009].
Most accelerometers can be divided into two categories: digital and analog. The digital accelerometers give the information under serial protocol like I2C or SPI, while analog accelerometers send output as voltage level within a predefined range that will be converted to a digital value by using an ADC module. Three equations are needed to calculate the inclination of a device relative to the ground plane. The values from ADC readings to a force vector component are expressed in g. Finally a triplet often called Direction Cosine, which basically represents the unit vector that has same direction as vector R (see Figure A.12) can be obtained. The cosine of angle between vector R and X axis is shown below:
cos(Axr) = ((AdcRx∗ Vref/216− VzeroG)/Sensitivity)/R (3.1)
CHAPTER 3. DEMONSTRATOR
Where Axz is defined as the angle between the Rxz (projection of vector R on XZ plane) and Z axis (see Figure A.13), Vref is ADC reference voltage, VzeroG is zero-g voltage level. Sensitivity – is the sensitivity of the gyroscope expressed in mV /(deg/s) often written as mV /deg/s, it basically indicate how many mV the gyroscope output will be increased, if the rotation speed is increased by one deg/s.
Finally 216 implies a 16bit ADC module in IMU.
For the gyroscope, each channel measures the rotation around one of three axes.
For instance, a 2-axes gyroscope will measure the rotation around the X and Y axes, thus the rate of changes of the angle defined above will be measured. In other words, a value that is linearly related to the rate of change of the angle will be sent.
The rate of change between the Rxz (projection of R on XZ plane) and Z axis will be calculated as: RateAxz = (Axz1 − Axz0)/(t1− t0). If Axz is in degrees and time is in seconds, the value will be expressed in deg/s. This is what a gyroscope measures. In practice a gyroscope rarely give the output expressed in deg/s. Same as for accelerometer an ADC value is needed to convert to deg/s using a similar for- mula that be defined for accelerometer. The value of Axzin plan XZ is shown below:
RateAxz = (AdcGyroXZ ∗ Vref/216− VzeroRate)/Sensitivity (3.2)
Where AdcGyroXZ, is obtained from ADC module and it represents the channel that measures the rotation of projection of R vector in XZ plan. Vref is reference voltage and Vzerois the zero-rate voltage.
A sketch of MPU-6050 is shown in appendix B as demonstration. Because of read- ing speed at 250 Hz, the raw data will be multiplied by 0.0000611 = 1/(250 ∗ 65.5).
Here 65.5 is the Sensitivity Scale Factor (°/s) which can be found in the manual page 12, if FS_SEL has been set to 1. In practice, the registers of IMU must be set up correctly in order to activate their functions. The addresses of registers can be found in the manual of chips which is offered by different manufacturers. A comple- mentary filter [Pieter-Jan, 2013] will be adopted in order to improve the accuracy of IMU raw data. On the short term, the data from the gyroscope can be used, because it is very precise and not susceptible to external forces. On the long term, the data from the accelerometer can be used, as it does not drift. The most simple form of the filter looks as follows:
angle = 0.98 ∗ (angle + gyrData ∗ dt) + 0.02 ∗ (accData) (3.3)
16
3.3. THE FLIGHT CONTROLLER AND SOFTWARE
The sum of two constants (0.98 and 0.02) should equal 1, but they can absolutely be changed to tune the filter properly. Even another easier complementary filter has been used to calculate the angle of pitch:
pitch_output = 0.9 ∗ pitch_output + 0.1 ∗ angle_pitch (3.4)
A comparison of using complementary filter has been made and is shown in ap- pendix A, Figure A.14 and Figure A.15. Finally, the accurate values will be sent to the ESC’s and PID controller. The public PID library for Arduino Uno can be adopted directly in the software. As mentioned in chapte 2.4, the separate PID controller should be implemented for the roll, pitch, and yaw axes in order to get stabilization of Y4 Quadrotor.
3.3.5 The ESC Output
The different rotor velocity combinations in order to achieve hover, pitch, roll and yaw movement are shown in Figure A.16. The white arrow implies hover reference rotor velocity, the green arrow means increasing of rotor velocity and the red arrow means decreasing of rotor velocity.
The orientation system of aviation is shown in Figure A.17. It consists of 3 orthogonal axes (Roll, Pitch, and Yaw) and the arrows indicate the positive rotations of these axes. With the guidance of both figures, the codes of ESC’s output can be included in function of FlightControl, which is shown in appendix B.
Chapter 4
Discussion and conclusions
The goals of this project were to investigate how a quadrotor in Y4 configuration with a larger centered rotor could be constructed if paired with an Arduino Uno board, considering limited time and resources. As well as determining the most important factors in controlling the robot. A durable construction was obtained and its ability to lift has been successfully tested. Modifying the MultiWii software has also successfully been done.
The goals have been scaled down multiple times due to unforeseen events, the main issues are related to large delays due to shipment times and faulty deliveries. There- fore tuning of the PID parameters hasn’t been completed, and by extension no stable flight has been achieved and lifting capabilities couldn’t be observed. The choice of using an open source program was made to save time, however the time it took to understand, modify and debug it probably compares to that of writing a simple one from scratch.
An other highly time consuming aspect was that of the ESC’s, they were bought to match the motors but data sheets and specifications were otherwise scarce. This proved problematic when it came to feeding them a signal with the correct frequency.
Chapter 5
Recommendations and future work
The next step for this project is to achieve stable flight, and compare it to a regular quadrotor carrying the same power source. A question that we’ve heard frequently is about safety and what happens if one motor or ESC stops working during flight, something that will be investigated more in the future. Also adding other naviga- tional capabilities like GPS and altitude control will probably be done.
To anyone attempting a similar project a word of caution must be issued about the ESC’s. We bought cheap ESC’s and they have shown an unstable behaviour from time to time, to get a more reliable and stable platform to work with they might be worth a larger investment.
Bibliography
[Adam Polak, 2012] Adam Polak (2012). Custom MultiWii Motor Mixes. Available from: http://polakiumengineering.com/custom-multiwii-motor-mixes/
[cited 2017-04-27].
[arduino.cc, 2010] arduino.cc (2010). PinChangeInt Library. Available from: http:
//www.pieter-jan.com/node/11 [cited 2017-05-21].
[arduino.cc, 2012] arduino.cc (2012). ATmega168/328-Arduino Pin Mapping. Avail- able from: https://www.arduino.cc/en/Hacking/PinMapping168 [cited 2017- 05-21].
[Aviastar, 2007] Aviastar (2007). De Bothezat. Available from: http://www.
aviastar.org/helicopters_eng/bothezat.php [cited April 2017].
[Ben Ripley, 2014] Ben Ripley (2014). Quadcopter source code from scratch. Available from: http://www.benripley.com/development/
quadcopter-source-code-from-scratch/ [cited 2017-04-10].
[Driessens and Pounds., 2013] Driessens, S. and Pounds., P. E. I. (2013). The Trian- gular Quadrotor: A More Efficient Quadrotor Configuration. 2013 IEEE/RSJ In- ternational Conference on Intelligent Robots and Systems. Available from: http:
//ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=6696530 [cited 2017-05- 21].
[Duc et al., 2015] Duc, M. N., Trong, T. N., and Xuan, Y. S. (2015). The quadrotor MAV system using PID control. 2015 IEEE International Conference on Mecha- tronics and Automation (ICMA). Available from: http://ieeexplore.ieee.
org/document/7237537/ [cited 2017-03-10].
[emaxmodel.com, 2014] emaxmodel.com (2014). EMAX MT2208 Multirotor Motor - Cooling Series. Available from: https://www.emaxmodel.com/
emax-cooling-series-multicopter-motor-mt2208.html [cited 2017-06-04].
[Filatov et al., 2016] Filatov, D. M., Deviatkin, A. V., Minav, T. A., and Fridrix, A. I. (2016). Control system of unmanned aerial vehicle. 2016 XIX IEEE In- ternational Conference on Soft Computing and Measurements (SCM). Available from: http://ieeexplore.ieee.org/document/7519701/ [cited 2017-03-10].
BIBLIOGRAPHY
[Free Software Foundation, Inc., 2007] Free Software Foundation, Inc. (2007). GNU General Public License. Available from: https://www.gnu.org/licenses/
gpl-3.0.en.html [cited 2017-05-04].
[Hobden, Andrew, 2015] Hobden, Andrew (2015). Quadcopters: Orientation.
Available from: https://hoverbear.org/tags/#quadcopters [cited 2017-02- 09].
[Li and Li, 2011] Li, J. and Li, Y. (2011). Dynamic analysis and PID control for a quadrotor. 2011 IEEE International Conference on Mechatronics and Automa- tion. Available from: http://ieeexplore.ieee.org/document/5985724/ [cited 2017-03-10].
[MultiGP, 2017] MultiGP (2017). Drone Racing MultiGP. Available from: http:
//www.multigp.com/ [cited 2017-05-21].
[MultiWii, 2015] MultiWii (2015). MultiWii. Available from: http://www.
multiwii.com/wiki/?title=Main_Page [cited May 2017].
[Oscar Liang, 2013] Oscar Liang (2013). Types of multirotor. Available from:
https://oscarliang.com/types-of-multicopter/ [cited 2017-05-21].
[Pieter-Jan, 2013] Pieter-Jan (2013). Reading a IMU Without Kalman: The Complementary Filter. Available from: http://playground.arduino.cc/Main/
PinChangeInt [cited 2017-05-21].
[Radhesh, 2008] Radhesh (2008). PID Controller Simplified.
Available from: https://radhesh.wordpress.com/2008/05/11/
pid-controller-simplified/ [cited 2017-05-21].
[rcmart.com, 2010] rcmart.com (2010). Futaba (7C-2.4G) 7 Channel R/C Sys- tem For Airplane (MD 1). Available from: http://www.rcmart.com/
futaba-7c24g-channel-system-airplane-p-27626.html [cited 2017-05-21].
[rctigermotor.com, 2013] rctigermotor.com (2013). MT2814. Available from:
http://www.rctigermotor.com/html/2013/Professional_0912/46.html [cited 2017-06-04].
[Redshift Labs, 2012] Redshift Labs (2012). Understanding Euler Angles. Avail- able from: http://www.chrobotics.com/wp-content/uploads/2012/11/F18.
png [cited 2017-05-21].
[SainSmart, 2013] SainSmart (2013). GY-85 Sensor Modules Accelerom- eter Gyroscope Module. Available from: https://www.amazon.com/
SainSmart-Modules-Accelerometer-Gyroscope-HMC5883L/dp/B00KQDIU3U [cited 2017-05-21].
[sparkfun.com, 2015] sparkfun.com (2015). ARDUINO UNO - R3. Available from:
https://www.sparkfun.com/products/11021 [cited 2017-05-21].
24
[Starlino, 2009] Starlino (2009). A Guide To using IMU (Accelerometer and Gy- roscope Devices) in Embedded Applications. Available from: http://www.
starlino.com/imu_guide.html [cited 2017-05-21].
[sUAS News, 2017] sUAS News (2017). The market. Available from: https://
www.suasnews.com/category/the-market/ [cited 2017-05-21].
[Wei, 2016] Wei, X. (2016). Control system of unmanned aerial vehicle. 2016 13th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI). Available from: http://ieeexplore.ieee.org/document/7733984/
[cited 2017-03-15].
[Young, 1982] Young, W. R. (1982). The Helicopters. "The Epic of Flight". Time- Life Books.
Appendix A
Images and Drawings
Figure A.1. The Details of The Final Prototype.
APPENDIX A. IMAGES AND DRAWINGS
Figure A.2. This is the lower part of the frame, spacers and top disc not included.
Figure A.3. Here the entire frame is shown, with motors and propellers mounted for illustrative purpose.
28
Richard Sagrén & Alexander Yun Project 6 MF133X
KTH 2017
Figure A.4. To the left the bottom disc can be seen, in the middle the top disc and to the right the three arm motor holders. Made in Solid Edge ST8 and Adobe Illustrator CC.
Figure A.5. GY-85 9DOF IMU Sensor Module [SainSmart, 2013]
APPENDIX A. IMAGES AND DRAWINGS
Figure A.6. Futaba T7C Transmitter and R617FS Receiver [rcmart.com, 2010]
Figure A.7. Arduino Uno board [sparkfun.com, 2015]
30
Figure A.8. The General Connection between Arduino Uno and ESC Array, IMU Sensor. Made in Fritzing 0.9.3b.
APPENDIX A. IMAGES AND DRAWINGS
Figure A.9. The Flow Chat of Y4 Quadrotor Software Development.
32
Figure A.10. Software Overview
Figure A.11. ATmega168/328-Arduino Pin Mapping [arduino.cc, 2010]
APPENDIX A. IMAGES AND DRAWINGS
Figure A.12. Coordinate System of Accelerometer [Starlino, 2009]
Figure A.13. Coordinate System of Gyroscope [Starlino, 2009]
34
Figure A.14. The result of using complementary filter for Angle of Pitch shown in Arduino Serial Plotter.
Figure A.15. The result of using complementary filter for Angle of shown in Arduino Serial Plotter.
APPENDIX A. IMAGES AND DRAWINGS
Figure A.16. Rotor Velocity Combinations of Y4 [Driessens and Pounds., 2013]
Figure A.17. The Orientation System [Redshift Labs, 2012]
36
Figure A.18. The Draw of Main Rotor: T-Motor MT2814-19 KV400. [rctigermo- tor.com, 2013]
Figure A.19. The Data of Main Rotor: T-Motor MT2814-19 KV400. [rctigermo- tor.com, 2013]
APPENDIX A. IMAGES AND DRAWINGS
Figure A.20. The Draw and Data of Boom Rotor: Emax MT2208 KV2000. [emax- model.com, 2014]
38
Appendix B
Arduino Code
1 // //////////////////////////////////
// Richard S a g r n & Alexander Yun //
3 // Course : MF133X //
// TRITA MMK 2017:12 MDAB 630 //
5 // Date : 19 May 2017 //
// //////////////////////////////////
7
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
9 // ∗∗∗ PID CONTROLLER ∗∗∗
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
11
f l o a t computeRollPID (f l o a t set_Point ) {
13
/∗
15 −−−−−−−−−−−−−−Roll PID Function−−−−−−−−−−−−−−−−
General−purpose PID c o n t r o l l e r .
17 ∗/
19 s t a t i c unsigned long last_Time ; s t a t i c f l o a t last_Error , error_Sum ;
21 f l o a t e r r o r ;
s t a t i c f l o a t P, I , D;
23
unsigned long now = m i l l i s ( ) ;
25 i n t dt = (i n t) (now − last_Time ) ;
27 i f( dt >= PID_SMPL_TIME) { e r r o r = set_Point − r o l l ;
29 error_Sum += e r r o r ;
31 P = ROLL_PID_KP ∗ e r r o r ;
33 i f( dt > 10∗PID_SMPL_TIME) { D = 0 ;
35 error_Sum = 0 ;
}
37 e l s e{
APPENDIX B. ARDUINO CODE
D = ROLL_PID_KD ∗ ( e r r o r − last_Error ) ;
39 }
41 I = ROLL_PID_KI ∗ error_Sum ; // c o n s t r a i n I term to max s e t t i n g
43 I = c o n s t r a i n ( I , −I_MAX, I_MAX) ;
45 #i f d e f DEBUG_PID_ROLL
S e r i a l . p r i n t (P) ; S e r i a l . p r i n t (’ \ t ’) ;
47 S e r i a l . p r i n t ( I ) ; S e r i a l . p r i n t (’ \ t ’) ; S e r i a l . p r i n t l n (D) ;
49 #e n d i f
51 last_Error = e r r o r ; last_Time = now ;
53 }
return (P + I + D) ;
55 }
Listing B.1. The code for the Roll PID Function
1 // //////////////////////////////////
// Richard S a g r n & Alexander Yun //
3 // Course : MF133X //
// TRITA MMK 2017:12 MDAB 630 //
5 // Date : 19 May 2017 //
// //////////////////////////////////
7
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
9 // ∗∗∗ RECEIVER CONVERTER ∗∗∗∗
// ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
11 byte last_CH_A ; i n t rx_CH_A_In ;
13 unsigned long timer_1 ;
15 void setup ( ) {
PCICR |= (1 << PCIE0) ;
17 S e r i a l . begin (57600) ; }
19
void loop ( ) {
21 delay (150) ; p r i n t _ s i g n a l s ( ) ;
23 }
25 // This part i s c a l l e d every time Channel A changed s t a t e ISR ( PCINT0_vect ) {
27 // Channel A −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
i f (last_CH_A == 0 && PINB & B00000001 )
29 // Channel A changed from 0 to 1 {
31 last_CH_1 = A;
//Remember c u r r e n t input s t a t e
33 timer_1 = micros ( ) ;
40
// Set timer_1 to micros ( )
35 }
e l s e i f (last_CH_A == 1 && ! ( PINB & B00000001 ) )
37 // Channel A changed from 1 to 0 {
39 last_CH_A = 0 ;
//Remember c u r r e n t input s t a t e
41 rx_CH_A_In = micros ( ) − timer_1 ; // Channel A i s micros ( ) − timer_1
43 }
}
45
void p r i n t _ s i g n a l s ( ) {
47 S e r i a l . p r i n t (" Roll : ") ;
i f(rx_CH_A_In − 1460 < 0) S e r i a l . p r i n t ("<<<") ;
49 e l s e i f(rx_CH_A_In − 1500 > 0) S e r i a l . p r i n t (">>>") ; e l s e S e r i a l . p r i n t ("−+−") ;
51 S e r i a l . p r i n t (rx_CH_A_In) ; }
Listing B.2. The code for the Receiver Channel A
// //////////////////////////////////
2 // Richard S a g r n & Alexander Yun //
// Course : MF133X //
4 // TRITA MMK 2017:12 MDAB 630 //
// Date : 19 May 2017 //
6 // //////////////////////////////////
8 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
// ∗∗∗ IMU CONVERTER ∗∗∗∗
10 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
12 #i n c l u d e <Wire . h>
i n t gyro_x , gyro_y , gyro_z ;
14 long gyro_x_cal , gyro_y_cal , gyro_z_cal ; boolean set_gyro_angles ;
16
long acc_x , acc_y , acc_z , acc_total_vector ;
18 f l o a t angle_roll_acc , angle_pitch_acc ;
20 f l o a t angle_pitch , a n g l e _ r o l l ;
i n t angle_pitch_buffer , a n g l e _ r o l l _ b u f f e r ;
22 f l o a t angle_pitch_output , angle_roll_output ;
24 long loop_timer ; i n t temp ;
26
void setup ( ) {
28 Wire . begin ( ) ;
setup_mpu_6050_registers ( ) ;
30 f o r (i n t cal_int = 0 ; cal_int < 2000 ; cal_ int ++){
read_mpu_6050_data ( ) ;
32 gyro_x_cal += gyro_x ;
APPENDIX B. ARDUINO CODE
gyro_y_cal += gyro_y ;
34 gyro_z_cal += gyro_z ; delay ( 3 ) ;
36 }
gyro_x_cal /= 2000;
38 gyro_y_cal /= 2000;
gyro_z_cal /= 2000;
40 S e r i a l . begin (115200) ; loop_timer = micros ( ) ;
42 }
44 void loop ( ) {
read_mpu_6050_data ( ) ;
46 gyro_x −= gyro_x_cal ; gyro_y −= gyro_y_cal ;
48 gyro_z −= gyro_z_cal ;
50 //Gyro angle c a l c u l a t i o n s : 0.0000611 = 1 / (250Hz x 6 5 . 5 ) angle_pitch += gyro_x ∗ 0 . 0 0 0 0 6 1 1 ;
52 a n g l e _ r o l l += gyro_y ∗ 0 . 0 0 0 0 6 1 1 ;
54 // 0.000001066 = 0.0000611 x ( 3 . 1 4 2 / 180)
angle_pitch += a n g l e _ r o l l ∗ s i n ( gyro_z ∗ 0.000001066) ;
56 a n g l e _ r o l l −= angle_pitch ∗ s i n ( gyro_z ∗ 0.000001066)
58 // Accelerometer angle c a l c u l a t i o n s
acc_total_vector = s q r t ( ( acc_x∗acc_x )+(acc_y∗acc_y )+(acc_z∗acc_z ) ) ;
60
// 57.296 = 1 / ( 3 . 1 4 2 / 180) The Arduino a s i n f u n c t i o n i s in r a d i a n s
62 angle_pitch_acc = a s i n ( (f l o a t) acc_y/ acc_total_vector ) ∗ 5 7 . 2 9 6 ; angle_roll_acc = a s i n ( (f l o a t) acc_x/ acc_total_vector ) ∗ −57.296;
64
angle_pitch_acc −= 0 . 0 ;
66 angle_roll_acc −= 0 . 0 ;
68 i f( set_gyro_angles ) {
angle_pitch = 0.98 ∗ angle_pitch + 0.02 ∗ angle_pitch_acc ;
70 // See Chapter 3 . 3 . 5
a n g l e _ r o l l = 0.98 ∗ a n g l e _ r o l l + 0.02 ∗ angle_roll_acc ;
72 // See Chapter 3 . 3 . 5 }
74 e l s e{
angle_pitch = angle_pitch_acc ;
76 a n g l e _ r o l l = angle_roll_acc ; set_gyro_angles = true ;
78 }
80 //Complementary f i l t e r i s used to damp the p i t c h and r o l l a n g l e s angle_pitch_output = 0 . 9 ∗ angle_pitch_output + 0 . 1 ∗ angle_pitch ;
82 // See Chapter 3 . 3 . 5
angle_roll_output = 0 . 9 ∗ angle_roll_output + 0 . 1 ∗ a n g l e _ r o l l ;
84 // See Chapter 3 . 3 . 5
86 S e r i a l . p r i n t (" | Pitch Angle = ") ;
42
S e r i a l . p r i n t l n ( angle_pitch_output ) ;
88 S e r i a l . p r i n t (" | P o l l Angle = ") ; S e r i a l . p r i n t l n ( angle_roll_output ) ;
90
while( micros ( ) − loop_timer < 4000) ;
92 loop_timer = micros ( ) ; }
94
96 void setup_mpu_6050_registers ( ) { // Activate the MPU−6050
98 Wire . beginTransmission (0 x68 ) ; Wire . w r i t e (0x6B) ;
100 Wire . w r i t e (0 x00 ) ; Wire . endTransmission ( ) ;
102
// Configure the a c c e l e r o m e t e r (+/−8g )
104 Wire . beginTransmission (0 x68 ) ; Wire . w r i t e (0x1C) ;
106 Wire . w r i t e (0 x10 ) ; Wire . endTransmission ( ) ;
108
// Configure the gyro (500 dps f u l l s c a l e )
110 Wire . beginTransmission (0 x68 ) ; Wire . w r i t e (0x1B) ;
112 Wire . w r i t e (0 x08 ) ; Wire . endTransmission ( ) ;
114 }
116
void read_mpu_6050_data ( ) {
118 // Reading the raw gyro and a c c e l e r o m e t e r data Wire . beginTransmission (0 x68 ) ;
120 Wire . w r i t e (0x3B) ; Wire . endTransmission ( ) ;
122 Wire . requestFrom (0 x68 , 1 4 ) ;
124 while( Wire . a v a i l a b l e ( ) < 14) ; acc_x = Wire . read ( ) <<8|Wire . read ( ) ;
126 acc_y = Wire . read ( ) <<8|Wire . read ( ) ; acc_z = Wire . read ( ) <<8|Wire . read ( ) ;
128 temp = Wire . read ( ) <<8|Wire . read ( ) ; gyro_x = Wire . read ( ) <<8|Wire . read ( ) ;
130 gyro_y = Wire . read ( ) <<8|Wire . read ( ) ; gyro_z = Wire . read ( ) <<8|Wire . read ( ) ;
132 }
Listing B.3. The code for the MPU-6050
// //////////////////////////////////
2 // Richard S a g r n & Alexander Yun //
// Course : MF133X //
4 // TRITA MMK 2017:12 MDAB 630 //
// Date : 19 May 2017 //
APPENDIX B. ARDUINO CODE
6 // //////////////////////////////////
8 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
// ∗∗∗ ESC OUTPUT ∗∗∗∗
10 // ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
12
t h r o t t l e = receiver_input_channel_3 ;
14 i f ( s t a r t == 3) {
i f ( t h r o t t l e > 1950) t h r o t t l e = 1950;
16
motor_1 = t h r o t t l e + pid_out_yaw ;
18 // C a l c u l a t e the p u l s e f o r motor 1 ( Center : CCW)
20 motor_2 = t h r o t t l e − pid_out_pitch + pid_out_yaw ; // C a l c u l a t e the p u l s e f o r motor 2 ( Front : CW)
22
motor_3 = t h r o t t l e + pid_out_pitch − pid_out_roll − pid_out_yaw ;
24 // C a l c u l a t e the p u l s e f o r motor 3 ( Rear L e f t : CW)
26 motor_4 = t h r o t t l e + pid_out_pitch + pid_out_roll + pid_out_yaw ; // C a l c u l a t e the p u l s e f o r motor 4 ( Rear Right : CW)
Listing B.4. The code for the ESC Output
44
