DRONE AUTOPILOTÞórarinn Árni Pálsson

Final Report in Applied ElectricalEngineering B.Sc.

2017

Name: Þórarinn Árni PálssonKennitala: 240192-2849Supervisor: Baldur Þorgilsson

School of Science and EngineeringTækni- og verkfræðideild

ii

Heiti verkefnis:

Námsbraut: Tegund verkefnis:

Önn: Námskeið: Ágrip:

Höfundur:

Umsjónarkennari:

Leiðbeinandi:

Fyrirtæki/stofnun:

Dagsetning: Lykilorð íslensk: Lykilorð ensk:

Dreifing:opin lokuð til:

Háskólinn í Reykjavík Menntavegi 1, 101 Reykjavík sími: 599 6200 www.ru.is

DRONE AUTOPILOT

Applied Electrical Engineer-ing B.Sc.

Final Report in Tæknifræði B.Sc.

2017–3 RT-LOK1012

Þórarinn Árni Pálsson

Baldur Þorgilsson

4.12.2017 Sjálfsstýring, Bátur,Fiskileitartækni

Autonomous, Drone,Vessel

This products initial development is a partof a school class at Reykjavík Universitycalled RT-LOK1012 which amounts to 24ECTS. This report list the progress after onesemesters work of development.

This paper lists the process of making asteering mechanism for an autonomous boatwhich is to be able to sail by predefinedcoordinates wirelessly. The scope of thisproject was to research, test and assemble aprototype of an autopilot which could be usedto resemble the final product that will be usedin an autonomous boat.

X

iv

The undersigned hereby grants permission to the Reykjavík University Library to reproducesingle copies of this Final Report entitled Drone autopilot and to lend or sell such copies forprivate, scholarly or scientific research purposes only.The author reserves all other publication and other rights in association with the copyright in theFinal Report, and except as herein before provided, neither the Final Report nor any substantialportion thereof may be printed or otherwise reproduced in any material form whatsoever withoutthe author’s prior written permission.

date

Þórarinn Árni PálssonBachelor of Science

vi

Acknowledgements

This project is a bachelors thesis during the 7th semester at Reykjavík Uni-versity, department of applied electrical engineering. This bachelors thesis is acontinuation of an innovative project I worked on this summer, funded by NSN(Nýsköpunarsjóður Námsmanna). The idea is to design and make an unmannedsurface vehicle to help with fish finding. The work was originally proposed by JónSölvi Smárason who had begun research on this matter the summer before (2016).It has been an interesting and challenging journey, where I have gained valuableinsight into the maritime world. First and foremost I want to thank Jón Sölvi Sæ-varsson and María Helga Jónsdóttir my coworkers during last summer for leadingme to this path. My supervisor and teacher Baldur Þorgilsson who has guided mea lot during my stay at Reykavík university.

I also want to give special thanks to Indriði Sævar Ríkharðsson who has beenvery helpful with control theory and simulation, Joseph Timothy Foley and ÖrnHrafnsson for reviewing my report draft and Hannes Páll Þórðarsson, supervisor ofthe electronics lab at Reykjavík University for good advice.

viii

ix

Contents

Acknowledgements vii

Contents ix

List of Figures xi

List of Tables xiii

List of Abbreviations xv

1 Abstract 1

2 Introduction 32.1 Design Phases . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.2.1 Other complete solutions . . . . . . . . . . . . . . . . . . . . . . . . . 42.2.2 Controlling devices . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.2.2.1 PickHawk Autopilot . . . . . . . . . . . . . . . . . . . . . . 62.2.2.2 NAVpilot-711C . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Design 93.1 Control processor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.2 Wireless communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.2.1 Wixel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.2.2 XBEE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.3 The Vessel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.4 User Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.4.1 Routes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.4.2 Route Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.4.3 Communication Protocol . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.5 Heading and Location . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.5.1 Compass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.5.1.1 Calibration & Configuration . . . . . . . . . . . . . . . . . . 173.5.2 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.5.3 Rudder feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.6 Course keeping & Control theory . . . . . . . . . . . . . . . . . . . . . . . . . 223.6.1 First-Order Nomoto Model . . . . . . . . . . . . . . . . . . . . . . . . 233.6.2 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4 Modelling 25

x

4.1 Open loop experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 254.1.1 Rudder delay . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 274.1.2 Constants τ & K . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.2 The measuring circuit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 294.2.1 Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.2.2 Output signals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324.2.3 Controlling circuit . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5 Results & Discussion 355.1 Transfer function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 355.2 Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375.3 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.3.1 Parameter tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 385.4 AHRS system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405.5 User control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

6 Conclusion 416.1 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 416.2 Control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 426.3 AHRS system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 426.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 436.5 Wireless communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

Bibliography 45

A Pictures 49

B Datasheets 57

C Code 61

xi

List of Figures

2.1 How an active sonar system works [1] . . . . . . . . . . . . . . . . . . . . . . . . 42.2 Saildrone [2] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.3 Z-boat 1800 from Seaworld [3] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.4 PickHawk [5] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.5 Processor Unit NAVpilot-711C[6] . . . . . . . . . . . . . . . . . . . . . . . . . . 72.6 Actuator pump NAVpilot-711C[6] . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1 Block diagram of the prototype autopilot . . . . . . . . . . . . . . . . . . . . . . . 93.2 Pololu’s Wixel [8] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.3 XBEE ZB [9] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.4 Rockstar 48 [10] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.5 Active Route in TimeZeroTM Navigator . . . . . . . . . . . . . . . . . . . . . . . 123.6 Communication diagram for the autopilot . . . . . . . . . . . . . . . . . . . . . . 133.7 Information provided for home planning simulation . . . . . . . . . . . . . . . . . 133.8 Magnetic Heading is the deviation from vessels front end to magnetic north[12] . . 153.9 Pololu UM7 orientation sensor [13] . . . . . . . . . . . . . . . . . . . . . . . . . 163.10 A breakout board of the MPU9250 [14] . . . . . . . . . . . . . . . . . . . . . . . 163.11 X,Y and Z reading axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.12 Magnetic data points plotted in MATLAB . . . . . . . . . . . . . . . . . . . . . . 173.13 Explanation of the atan2 function . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.14 Before(blue) and After(red) bias calibration . . . . . . . . . . . . . . . . . . . . . 193.15 GP-20U7 receiver unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.16 Block diagram of modified closed loop configuration . . . . . . . . . . . . . . . . 203.17 SPMS601H Extra high torque servo motor [17] . . . . . . . . . . . . . . . . . . . 213.18 Measuring the extreme values of the servo motor . . . . . . . . . . . . . . . . . . 213.19 Boat’s reference system in the horizontal plane [19] . . . . . . . . . . . . . . . . . 223.20 Design in Simulink . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.1 Block diagram of the vessel’s system . . . . . . . . . . . . . . . . . . . . . . . . . 254.2 Step response of a first order system with K = 1 = τ = 1, θ = 0 . . . . . . . . . . . 264.3 Slew rate effect on a square wave . . . . . . . . . . . . . . . . . . . . . . . . . . . 264.4 Block diagram of LabView progam . . . . . . . . . . . . . . . . . . . . . . . . . . 274.5 Controlling interface in LabView . . . . . . . . . . . . . . . . . . . . . . . . . . . 274.6 Actual servo angle (red) vs. desired angle (blue . . . . . . . . . . . . . . . . . . . 284.7 Turning rate test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 294.8 Block diagram of the Measuring circuit . . . . . . . . . . . . . . . . . . . . . . . 294.9 Controlling interface in LabView . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.10 Numerical integral estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314.11 Area of the trapezoid . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

xii

4.12 Numerical integration in LabView . . . . . . . . . . . . . . . . . . . . . . . . . . 314.13 Ouput signal of the rudder in idle position, . . . . . . . . . . . . . . . . . . . . . 324.14 Block diagram of the GPS circuit . . . . . . . . . . . . . . . . . . . . . . . . . . . 334.15 Double pole double throw relay schematic [24] . . . . . . . . . . . . . . . . . . . 334.16 A fritzing diagram of the controlling circuit . . . . . . . . . . . . . . . . . . . . . 34

5.1 Measured angular velocity over time . . . . . . . . . . . . . . . . . . . . . . . . . 365.2 Measured angular velocity over time . . . . . . . . . . . . . . . . . . . . . . . . . 365.3 How angular velocity relates to directional velocity . . . . . . . . . . . . . . . . . 375.4 Block diagram of the controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375.5 P controller results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 385.6 PI controller results, Kp = 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 395.7 PID controller results, Kp = 1, Ki = 1 . . . . . . . . . . . . . . . . . . . . . . . . 395.8 Block diagram of the steering process . . . . . . . . . . . . . . . . . . . . . . . . 40

6.1 Minn Koda Power drive 70 [25] . . . . . . . . . . . . . . . . . . . . . . . . . . . 416.2 MTi-G-710 AHRS and positioning system [26] . . . . . . . . . . . . . . . . . . . 436.3 Furuno PG-700 Compass . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 436.4 NV08C-CSM-BRD GPS receiver . . . . . . . . . . . . . . . . . . . . . . . . . . . 436.5 Digi XBee-PRO 900HP [28] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

A.1 Output signal analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49A.2 Ouput signal of the rudder in idle position . . . . . . . . . . . . . . . . . . . . . . 50A.3 Ouput signal of the rudder at a maximum angle of 30° . . . . . . . . . . . . . . . . 50A.4 Ouput signal of the rudder at a minimum angle of -30° . . . . . . . . . . . . . . . 51A.5 Ouput signal of the throttle at idle . . . . . . . . . . . . . . . . . . . . . . . . . . 51A.6 Ouput signal of the throttle at max . . . . . . . . . . . . . . . . . . . . . . . . . . 52A.7 Ouput signal of the throttle at min . . . . . . . . . . . . . . . . . . . . . . . . . . 52A.8 Rudder angle measured . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53A.9 PCB board for the GPS compass . . . . . . . . . . . . . . . . . . . . . . . . . . . 53A.10 Schematic diagram of the GPS Compass . . . . . . . . . . . . . . . . . . . . . . . 54A.11 Switching relay circuit for boat control . . . . . . . . . . . . . . . . . . . . . . . . 54A.12 Measurement system mounted on the boat . . . . . . . . . . . . . . . . . . . . . . 55

B.1 MPU6050 Sensitivity values [29] . . . . . . . . . . . . . . . . . . . . . . . . . . . 57B.2 MTi-G-710 Specification [26] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57B.3 Furuno PG-700 Specifications[27] . . . . . . . . . . . . . . . . . . . . . . . . . . 58B.4 NV08C-CSM-BRD integrated GPS receiver specifications[30] . . . . . . . . . . . 58B.5 Digi XBee-PRO 900HPspecifications[28] . . . . . . . . . . . . . . . . . . . . . . 59

xiii

List of Tables

3.1 Output NMEA sentences from TimeZeroTM Navigator . . . . . . . . . . . . . . . . 133.2 GP-20U7 NMEA output sentences . . . . . . . . . . . . . . . . . . . . . . . . . . 203.3 Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

4.1 Servo motor slew rate measurements . . . . . . . . . . . . . . . . . . . . . . . . . 284.2 Measured output PWM of controlling servo motors . . . . . . . . . . . . . . . . . 33

5.1 Constants τ & K . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

6.1 Parameter results for the vessel controller . . . . . . . . . . . . . . . . . . . . . . 42

xiv

xv

List of Abbreviations

MCU Microcontroller UnitPCB Printed Circuit BoardNMEA National Marine Electronics AssociationROT Rate of turnPID A proportional–integral–derivative controllerAHRS Attitude and heading reference systemAPB Autopilot sentence B (NMEA protocol)XTE Crosstrack errorBTW Bearing to waypointDTW Distance to waypointTCP Transmission Control ProtocolUART Universal asynchronous receiver-transmitterIMU Inertial measuring unitUSV Unmanned surface vehicleIP Ingress protectionGPS Global positioning systemSiP System in packageMEMS Micro electrical mechanincal system

xvi

1

Chapter 1

Abstract

This products initial development is a part of a school class at Reykjavík University called RT-LOK1012 which amounts to 24 ECTS. This report list the progress after one semesters work ofdevelopment.

This paper lists the process of making a steering mechanism for an autonomous boat whichis to be able to sail by predefined coordinates wirelessly. The scope of this project was to re-search, test and assemble a prototype of an autopilot which could be used to resemble the finalproduct that will be used in an autonomous boat.

The listed requirements for the initial prototype are:

• Control - The boat is to be able to sail to a predefined location according to GPS coordi-nates set on a map and to be remotely controlled by hand.

• Steering - The boat should regulate the steering automatically by sensing heading andlocation.

• Tracking The boat may not get lost and has to include a safety mechanism to preventthat.

All tools needed for making an autonomous vessel steering mechanism are listed in thisreport.

When doing physical measurements on the vessel used, a decision was made to narrow thescope of the project and only design and simulate the controller in MATLAB. The proceduresused for measurements and designing of the controller will be done on future designs of theautonomous drone.

2

3

Chapter 2

Introduction

Fisheries have a long history in Iceland and are now one of the cornerstones of the economy.The increasing demand for autonomous cost effective vessels is due to expensive operations,increased complexity and harsh environments.

This project is a part of a bigger innovative project in the making. The long term goal is tomake an autonomous boat which can sail and send bathymetric(information about the oceanfloor) data in real time at its current location. In the future a fleet of unmanned surface vehicles(USV)s to do various operations but mostly send data about fish schools to a main ship to helpguide the fisheries process. These USVs are envisaged to have a application in commercial andmarine operations. Initial development of the whole project began last summer and has beenfunded by Rannís innovation fund. The project in whole is split in to four primary phases. Seebelow

2.1 Design Phases1. Design data measuring circuit which can send real time data over some distance.

2. Make a wireless steering mechanism (autopilot).

3. Build a hull of the boat.

4. Join all the parts together in a circuit and place them in the hull.

The main motivation for making this project in general is to make fish finding cheaper and moreefficient. Some other areas could also benefit from the use of this equipment such as search andrescue teams.

This report lists the research and design process of making the autopilot (phase 2).

First are discussed the tools needed for development, software and hardware-wise. Theoryand application of modeling is discussed. Measurements made on the vessel for controller de-sign.

4 CHAPTER 2. INTRODUCTION

2.2 Background

Ships have been able to obtain information about underwater objects and events by transmittingsound waves and observing the return echoes [1].

Figure 2.1: How an active sonar system works [1]

Today the measuring equipment is attached to the hull of the fishing ship so it limits thescope of of sight underneath the boat. Even though there are some products out there similarto the autonomous drone designed in this project, it has not been used in general to find fish.Figure 2.1 shows how this is done by most today. The scope of the boat is limited to its locationat each time.

2.2.1 Other complete solutions

After a little background research, there were few products out there that resemble the idea.One of them is the Saildrone 2.2 and the Z-boat 1800 from Subsea, see Figure 2.3. To be notedthese solutions are specially made for oceanography and climate research. The Saildrone is notavailable for sale to the common user.

2.2. BACKGROUND 5

Figure 2.2: Saildrone [2]

Figure 2.3: Z-boat 1800 from Seaworld [3]

The Z-boat is intended for shallow water inshore bathymetric surveys. It has autonomouswaypoint navigation which which offers real time communication to shore. This solution offersa lot of the functions needed for the autonoumous drone in this project. It is not specially de-signed for outer shore expeditions and not optimal for fish finding.

The Saildrone is mostly intended for fish stock analysis, data collection and environmentalhealth operations. They don’t offer solutions to the common market and work mainly with gov-ernments and organizations.

Even though the solution exists it is not intended for the same use as the autopilot dronediscussed here.

6 CHAPTER 2. INTRODUCTION

2.2.2 Controlling devices

Recently a lot of remotely controlled flying vehicles have come about and their controllingtechnique is something is that is of interest for this project. To prevent reinventing the wheelmethods and equipment that has been proven to work will be looked at.

Controlling an airborne drone is more complicated than controlling an autonomous boat dueto the fast regulating response they need to fly stable in harsh environments [4]. Flight con-trollers for airborne drones are available and were considered for this project. See Figure 2.4.

Autopilot steering mechanisms are not new and have been used for vessel control to keep a cho-sen course without constant human interaction for quite a while. Where autopilots are mostlyto be operated on board a sailing ship. The course and speed are operated by hand but notremotely. Below we’ll look at the NAVpilot-711C and PickHawk autopilot for demonstrationpurposes [5].

2.2.2.1 PickHawk Autopilot

Figure 2.4: PickHawk [5]

"PIXHAWK is a high-performance autopilot-on-module suitable for fixed wing, multi rotors,helicopters, cars, boats and any other robotic platform that can move. It is targeted towardshigh-end research, amateur and industry needs and combines the functionality of the PX4FMU+ PX4IO."2.4

The PickHawk is a microcontroller based computer which has certain inputs and outputs. Itcan be configured to control flying drones, RC boats or any kind of vehicle suited. It is widelyused in this field and will be considered as an option for this project. The PickHawk does notcome with an RC boat pre-programmed feature so all course keeping and autopilot functions

2.2. BACKGROUND 7

would have to be implemented and the PickHawk be programmed that way.

• 32bit STM32F427 Cortex M4 core with FPU

• 168 MHz

• 256 KB RAM

• 2 MB Flash

• 32 bit STM32F103 failsafe co-processor

It is also equipped with essential sensors such as accelerometer, gyroscopes, magnetometer andbarometer.

2.2.2.2 NAVpilot-711C

The NAVpilot from Furuno is a complete autopilot solution for onboard vessel control, Figures2.5 & 2.6. This solution is not feasible for this project because it requires onboard humancontrol. Its processor unit is hand controlled and not suitable for wireless operations. TheNAVpiloit-711C consists of the following units [6]. For autonomous control the processor unitwould have to be replaced by other custom made controlling circuit.

• Rudder Reference Unit

• NMEA2000 Rate Compensated Heading Sensor w/ 6M NMEA2000 Cable

• Processor Unit

• Control unit

Figure 2.5: Processor Unit NAVpilot-711C[6]

Figure 2.6: Actuator pump NAVpilot-711C[6]

The actuator pump, heading sensor and rudder reference unit can be of use. It would pos-sibly be a solution to use these parts with the PickHawk Autopilot as a processor unit. Neither

8 CHAPTER 2. INTRODUCTION

of these solutions were used for the project at this stage. The PickHawk would be a preferablechoice for the RC boat used here because the boat is controller by servo motors.

When implementing a an autopilot for larger vessels (6+ meters) the hydraulic actuatorpump in Figure 2.6 is of use. Only the PickHawk is of use for this project and will be consid-ered for the end product.

9

Chapter 3

Design

The first prototype will be made simple and robust. An microcontroller of some sort will beused and joined in a circuit with required sensors and other hardware.

Figure 3.1: Block diagram of the prototype autopilot

3.1 Control processor

An Arduino Nano is used for initial measurements and prototyping. Due to its size (18 x 45mm) and pinout, it’s a good candidate to meet the requirements needed (6 PWM pins and 8analog inputs). Specifications of the model board are listed below [7].

• ATmega328 processor

• 5V operating voltage

• 32 KB flash memory

• 16 MHz clock

• 8 Analog pins

• 6 PWM pins

• 22 digital I/O pins

10 CHAPTER 3. DESIGN

3.2 Wireless communication

3.2.1 Wixel

Wixel is a general propose programmable module used in this project for initial testing anddebugging. It features a 2.5 GHz radio and USB. A precompiled, open-souce program wasloaded onto the Wixel to trun it into a wrieless serial link. It was not considered as a viableradio link for the end product due to its limited range. Two Wixels were set up in a point topoint configuration and used for initial development of the vessels compass. As discussed laterdifferent modules were used [8].

• CC2511F32 processor

• 2.7 - 6.5V operating voltage

• 29 Kbytes flash memory

• 24 MHz clock

• 15 I/O lines

• 50ft (15m) range

3.2.2 XBEE

XBEE modules are an embedded solution from Digi international for wireless communication.Modules are set up in a point to point configuration using the used interface (XCTU). Thesemodules communicate using serial communication (UART) and in can act as a simple serialbridge. XBEE is used for measurement in this project and also considered to be a reliablechoice for the final product. Other more powerful versions of the XBEE modules are availablewith wider range XBEE

• 2mW output (+3dBm)

• 3.3V operating voltage

• Built-in antenna

• 6 10-bit ADC input pins

• 8 digital IO pins

• 400ft (120m) range

Figures 3.2 & 3.3 show the two wireless devices used in this project.

3.3. THE VESSEL 11

Figure 3.2: Pololu’s Wixel [8]Figure 3.3: XBEE ZB [9]

3.3 The VesselThe test boat is a Rockstar 48 gasoline powered Fiberglass boat. The reason for choosing thisboat was its simplicity in control. The throttle and rudder are controlled by servo motors whichare easy to control and can easily be implemented in an Arduino, ArduPilot or Raspberry Pisystem. It’s a small-scale version of the final product and can be easily manipulated and testeddue to its size. It has a removable composite tray which allows for easy access to steeringcomponents and a centrifugal clutch which helps prevent the prop from spinning while theengine is at idle. See Figure 3.4. Specifications can be seen below [10]

• Length: 1219.2mm

• Beam: 440mm

• Engine Size: 26cc

• Hull Material: Fiberglass

• Prop Size: 70mm

• 2-channel Radio

3.4 User InterfaceThe boat steering will be controllable in two ways, by hand and through a user interface. Thecomputer software is a state of the art shipping navigation software which can be operatedthrough a conventional PC. The TimeZero software is used mainly for boat’s steering but hasmany other useful features such as depth measuring and fish finding. The fish finding functionwill not be discussed here and the focus mainly on the autopilot function.

3.4.1 RoutesTimeZero Professional allows for the creation of an unlimited number of routes, only one acti-vated at a time. Routes are made by setting waypoints and when a route is activated it becomes

12 CHAPTER 3. DESIGN

Figure 3.4: Rockstar 48 [10]

red. The software provides information about bearing and cross track error which is necessaryinformation for the autopilot unit to be able to steer to the next desired waypoint. See Figure 3.5

Figure 3.5: Active Route in TimeZeroTM Navigator

3.4. USER INTERFACE 13

There are many functions available to modify and change the routes an will not be discussedin detail here. For now, the only parts of interest are the route and information the softwareprovides for autopilot steering. The communication protocol used in TimZero Navigator isNMEA-0183 which sets the communication standard for the whole system. This communica-tion protocol is discussed in further detail in Section 3.4.3 and the only output information ofinterest for us is the APB (autopilot sentence B).

To activate the Autopilot the software needs GPS and heading information (HDT, HDGand GPS sentences). On fig. 3.5 we can see an active route and the cross track error(XTE) iszero, distance to waypoint (DTW) is 428 meters and the bearing to the next waypoint is 268.8°(BTW). The GPS accuracy, satellites in use and the reliability of the GPS information provided.An Arduino AHRS system was used to provide necessary information to activate the autopilot.Description of that system is discussed in section 3.5. Figure 3.6 shows how the communi-cation structured. The output port has to be set in TimeZero Navigator, either TCP or Serialcommunication. By monitoring the set COM port it was easier to recruit how the softwareacted.

Figure 3.6: Communication diagram for the autopilot

In Table 3.1 the output sentences from the software are listed.

Table 3.1: Output NMEA sentences from TimeZeroTM Navigator

NMEA Record DescriptionAPB Information about next set waypointXTE Cross-track error

The main information of interest in the APB sentence is the bearing and heading to a way-point. See listing 3.3 in Section 3.4.3: Items number 11 - 14.

3.4.2 Route SimulationsAfter a route has been created it can be simulated. That makes for easier planning and isextremely useful for autopilot reliability testing. Information about distance, speed, headingand time is provided. The whole process can be seen visually in the program.

Figure 3.7: Information provided for home planning simulation

14 CHAPTER 3. DESIGN

3.4.3 Communication ProtocolThe NMEA 0183 (National Marine Electronics Association) standard defines an electrical inter-face and data protocol for communications between marine instrumentation. It is used widelyin the marine industry, TimeZeroTM Navigator software allows for either TCP/UDP or NMEA0183 communication. Due to NMEA-0183 simplicity and reliability that standard will be usedfor this project. The NMEA-0183 allows a single talker and several listeners on one line so it’sa one-way serial communication standard.NMEA-0183 uses plain ASCII (American Standard Code for Information Interchange) charac-ter code format so it’s human readable. Data is sent from a talker (could be a GPS or Compassin our case) to a listener (computer) in the form of sentences with the maximum length of 82characters. Each sentence begins with the starting sign delimiter "$" and is terminated with a<CR><LF> (carriage return, line feed) delimiter. There are three types of sentences: talker sen-tences, proprietary sentences, and query sentences. Here we are mostly working with talker’ssentences. See in Listing 3.1.

Listing 3.1: NMEA-0183 talker’s sentence cointaining magnetic heading

$ttsss,d1,d2,....<CR><LF>

The first two letters following the "$" are the talker identifier (tt). The next three characters(sss) are the sentence identifier which tells what information the sentence contains followedby a number of data fields separated by commas (CSV format). The sentence is concluded bya checksum and terminated by carriage return/line feed. An example of a talker’s sentence isshown in listing 3.3

Listing 3.2: General NMEA-0183 talker’s sentence

$HCHDM,238,M<CR><LF>

The first two letters "HC" indicate that this is a heading compass sentence, next three letter"HDM" specifies that a magnetic heading message follows. Next we have the degree headingin relation to Magnetic North followed by a checksum (M).

Listing 3.3: NMEA-0183 APB Autopilot Sentece "B"13 15

1 2 3 4 5 6 7 8 9 10 11 12 | 14|| | | | | | | | | | | | | | |

$--APB,A,A,x.x,a,N,A,A,x.x,a,c--c,x.x,a,x.x,a*hh1) StatusV = LORAN-C Blink or SNR warningA = general warning flags or other navigation systems when a reliablefixes not available2) StatusV = Loran-C Cycle Lock warning flagsA = OK or not used3) Cross Track Error Magnitude4) Direction to steer, L or R5) Cross Track Units, N = Nautical Miles6) Status

3.5. HEADING AND LOCATION 15

A = Arrival Circle Entered7) StatusA = Perpendicular passed at waypoint8) Bearing origin to destination9) M = Magnetic, T = True10) Destination Waypoint ID11) Bearing, present position to Destination12) M = Magnetic, T = True13) Heading to steer to destination waypoint14) M = Magnetic, T = True15) Checksum

3.5 Heading and LocationAn AHRS (Attidude and Heading Reference System) is used to determine rotation of a rigidbody in 3 dimensional space, though at this stage the yaw angle is only information of interest,which represents the rotation angle from the the vessels front end to magnetic north (MagneticHeading). Figure 3.8

Location is provided through GPS (global positioning system) and is a radio navigational sys-tem. It provides geological position and time through a satellite system to GPS receivers any-where where there is an unobstructed line of sight to four or more GPS satellites. The GPSdoes not require the user to send any data and works independent of any telephonic or internetmedium [11].

Figure 3.8: Magnetic Heading is the deviation from vessels front end to magnetic north[12]

3.5.1 CompassA triple-axis accelerometer/magnetometer compass module will be used to determine the head-ing angle. The accelerometer tells which direction is downwards and the direction of acceler-ation. The three-axis magnetometer senses where the strongest magnetic field is coming fromand determines the magnetic north. By combining these functions the heading can be deter-mined.

Two IMU’s (inertial measuring unit) were considered for this project. UM7 (CH Robotics,Figure 3.9) orientation sensor and the MPU9250 (InvenSense, Figure 3.10) were considered for

16 CHAPTER 3. DESIGN

Figure 3.9: Pololu UM7 orientation sensor [13]

this project.

The UM7 orientation sensor is a third-generation Attitude and Heading Reference System(AHRS) from CH Robotics that produces attitude and heading estimates from a tri-axial ac-celerometer, rate gyro, and magnetometer data. Unlike a typical inertial measurement unit(IMU), which only provides raw sensor readings, the UM7 features an inboard microcontrollerthat combines sensor data using a sophisticated Extended Kalman Filter (EKF) to generate ori-entation estimates 500 times a second [13].

Figure 3.10: A breakout board of the MPU9250 [14]

The SparkFun MPU-9250 IMU Breakout features the latest 9-axis MEMS sensor from In-venSense. The 9250 breakout on figure 3.10 is a SiP (system in package) which combinestwo chips the MPU-6500 which is a 3-axis gyroscope as well as a 3-axis accelerometer andthe AK8963, a 3-axis magnetometer. Unlike the UM7 the MPU-9250 only sends out raw datawhich has to be processed afterwards.

The UM7 seemed to best solution at first, but in chapter 5 are listed the reasons why that sensorwas not optimal for this project.

3.5. HEADING AND LOCATION 17

3.5.1.1 Calibration & Configuration

The magnetometer readings are plotted on a sphere with regards to each axis. See Figure 3.11.By plotting up the measured values, Fig 3.12 it can be confirmed that the sensor is workingcorrectly.In the absence of a strong external magnetic field the compass should reflect the earth’s mag-

Figure 3.11: X,Y and Z reading axis

Figure 3.12: Magnetic data pointsplotted in MATLAB

netic field (between 0 - 800 mGauss). By calculating the angle of the magnetic field withrespect to the XY-plane the sensor can give reliable compass information. The sensor readingsare converted into a 0 - 360 degree compass heading. The function "atan2()" [15] is used toget a degree heading from the Y-axis, it gives degree orientation from the X-axis and preventdivision by zero. See fig. 3.13.

18 CHAPTER 3. DESIGN

Figure 3.13: Explanation of the atan2 function

If we have a rotation below the X-axis the heading is normalized by adding 360 degreesto the angle. The main factor to consider to get accurate heading angle is mostly that themaximum and minimum readings are equally centered from the origin. Thus the axis had to beoffset calibrated.

Listing 3.4: Calibration Offset in C++, simplified1 void calculateBiases(void)2 {3 sensors_event_t magEvent;45 myIMU.getEvent(&magEvent);678 if (magEvent.magnetic.x < MagMinX) MagMinX = myIMU.mx;9 if (magEvent.magnetic.x > MagMaxX) MagMaxX = myIMU.mx;

1011 if (magEvent.magnetic.y < MagMinY) MagMinY = myIMU.my;12 if (magEvent.magnetic.y > MagMaxY) MagMaxY = myIMU.my;1314 if (magEvent.magnetic.z < MagMinZ) MagMinZ = myIMU.mz;15 if (magEvent.magnetic.z > MagMaxZ) MagMaxZ = muImu.mz;1617 int mXbias = MagMinX + MagMaxX)/2.0;18 int mYbias = MagMinY + MagMaxY)/2.0;19 int mZbias = MagMinZ + MagMaxZ)/2.0;20 }

Before axis and after axis offset calibration the readings are shown below. fig. 3.14.

3.5. HEADING AND LOCATION 19

Figure 3.14: Before(blue) and After(red) bias calibration

Listing 3.5: Heading calculations. Simplified function1 void calculateHeading (&heading){23 mX = (myIMU.mx -= mXbias);4 mY = (myIMU.my -= mYBias);5 mZ = (myIMU.mz -= mZBias);67 float heading = ((atan2(mY,mX) * 180) / Pi);89 // Normalize to 0-360

10 if (heading < 0)11 {12 heading = 360 + heading;13 }14 }

3.5.2 GPS

To provide position, timing and velocity information a GP-20U7 GPS receiver is used. It’s alow power consumption device (40mA) that outputs NMEA-0183 sentences which is very use-ful because. It’s the communication protocol the user interface (MaxSea) can understand sodata from the GPS doesn’t have to be manipulated. This GPS was used because it had provento work well in previous projects.

20 CHAPTER 3. DESIGN

Figure 3.15: GP-20U7 receiver unit

NMEA output sentences are shown below. See Table 3.2.

Table 3.2: GP-20U7 NMEA output sentences

NMEA Record DescriptionGGA Global positioning system fixed dataGLL Geographic position - latitude / longitudeGSA GNSS DOP and active satellitesGSV GNSS satellites in viewRMC Recommended minimum specific GNSS dataVTG Course over ground and ground speed

3.5.3 Rudder feedbackTo be able to determine the rudder angle at a given moment a feedback reference unit has toprovide the deflection angle of the rudder δ. The vessels throttle and direction are controlledby simple servo motors which are open loop devices. Open loop controls have no feedbackand require the input to return to zero before the output will return to zero. In order to have areference, the servo has to me modified to give feedback.

Figure 3.16: Block diagram of modified closed loop configuration

On larger scale vessels, a reference potentiometer is attached to the rudder to provide rudderposition. For demonstration purposes a smaller scale model controlled by a servo motors is usedhere and can an estimation of how to control a bigger vessel. Inside the servomotor there is apoint which indicates the position of the servomotor. By measuring the extreme values of thatpoint with respect to ground a reference voltage is acquired. See figure 3.18 for demonstrationof these measurements. A wire is then soldered to that point and fed back to the microcontrollerwhere the values are mapped to voltage and angle calculated [16].

3.5. HEADING AND LOCATION 21

Figure 3.17: SPMS601H Extra hightorque servo motor [17]

Figure 3.18: Measuring the extremevalues of the servo motor

22 CHAPTER 3. DESIGN

3.6 Course keeping & Control theory

Dynamics of the boat are complicated by the interaction of the boats hull with the water, rigidbody dynamics with the hydrodynamic effects. To make the initial analysis of the system easier,some working conditions are assumed nominal such as the boats velocity for linear control. Amodel describing the dynamics between rudder deflection angle and the yaw (heading) has beenconstructed called the Nomoto model [18].

Generally a boat has six DOFs (degrees of freedom) [19] but for, now only three of themare considered, surge, sway and yaw. The plane of interest is the XY plane and the coordinatesystem used for development are internal coordinate system approximated by the earth fixedreference frame and a body-fixed reference frame. Figure 3.19. The XOY axis represents theinertial earth frame while the boat has its own internal body fixed reference frame xoy. Whenthe yaw angle (heading) is zero the hull of the boat is aligned to north end therefore the coordi-nate systems also. See table 3.3 for symbol definitions.

Figure 3.19: Boat’s reference system in the horizontal plane [19]

3.6. COURSE KEEPING & CONTROL THEORY 23

Table 3.3: Nomenclature

Symbol Definition Unit Explanation

ψ Yaw angle degreesAngle between boat’s heading andnorth (either true north or magnetic north)

r Yaw rate rad/sRate at which the yaw angle is changing(angular velocity around its vertial axis)

α Side slip angle degreesThe angle between heading and actualmovement direction

V Velocity vector m/s Velocity in actual direction

β Velocity vector angle degreesThe angle between the actualmovement direction and north

u Surge velocity m/s Velocity in x direction (body frame)v Sway velocity m/s Velocity in y direction (body frame)δ Rudder deflection angle degrees Angle of rudder

3.6.1 First-Order Nomoto Model

A first order Nomoto model can be used to describe steering qualities of boats. In 1957 Nomotoproposed transfer functions for steering simulation and control. A lot of time can be sparedand work made easier by simulating natural systems before trying them out in the real world.Trying to predict the behavior of a system can allow experimentation without exposure to risk.

The first order gives us a response in yaw rate to a change in rudder deflection angle. SeeEq. 3.1. oefficients K , T1, T2 and T3 are functions of the hull, form, rudder size and otherparticulars of the ship. They are composed by the equations of motions [20].

r(s)

δ(s)=

K(1 + T3)s

(1 + T1s)(1 + T2s)(3.1)

By defining the equivalent time constant τ Eq. 3.2 we can derive the first order model in asimpler manner. See Eq. 3.3

τ = T1 + T2 − T3 (3.2)

r(s)

δ(s)=

K

sτ + 1(3.3)

24 CHAPTER 3. DESIGN

3.6.2 SimulationOne part of this project is to implement the controlling unit for the vessel. Like stated beforewater dynamics can be complicated and the vessel for this demonstration has a much fasterresponse than other boats or ships. By getting the dynamic parameters of the boat it’s possibleto simulate the behavior rather than perform physical tests over and over. A first-order model isdiscussed in the preceding sections.

By getting the gain factor, time constant and slew rate of the rudder servo the boat can bemodeled and a controller mechanism designed for the vessel. A PID controller will be used toregulate the desired path to steer. It might be useful to take this a step further and see how thetransfer is from the rudder angle to the velocity vector V but for now, only the first order modelis demonstrated.

Figure 3.20: Design in Simulink

A simulation program for MATLAB (MSS_toolbox) is available on the internet [21]. Itdoesn’t support simulation of such small vessels, the minimum length is 7 meters. The simu-lation program allows for parameter calculation without any physical test on the vessel. Timeconstant and gain vary with different velocities and initially the idea was to compare transferfunctions for different velocities. Chapter 5 lists why measurements were only for one velocity.Decision was to make tests only at one velocity and approximate the boats characteristic tomake a controller.

25

Chapter 4

Modelling

This chapter introduces experiments for system identification. By measuring certain parametersof the physical system the control theory can be simulated and developed. First, open-loopexperiments will be introduced to see how the system reacts. Here wind turbulence is neglectedand the focus is mainly on the vessels steering and maneuvering dynamics. Rudder dynamicsare considered to only cause time delay and the vessel will be identified by the first orderNomoto model discussed in section 3.6. A block diagram of the system is shown in figure 4.1

Figure 4.1: Block diagram of the vessel’s system

The software development environment used for measurements is LabView. It is a graphicalprogramming language used widely in industry [22] and academia as a tool for data acquisitionand control of instruments. Two programs are used for parameter measurements Figure 4.5.The aim of the software is to measure rotational speed, acceleration, velocity, and rudder angle.

4.1 Open loop experimentsStandalone experiments are made to determine the delay of the rudder. A desired angle is sentto the servomotor controlling the rudder and the time measured to get to a desired angle. Themodified feedback servo discussed in section 3.5.3 is used to measure the real angle of the con-trolling motor.

To measure the time constant and gain for the first order Nomoto model the rate of turnis measured in accordance to the actual controlling rudder angle. The transfer function of thevessel system is then eq. 5.1.

G(s) =K

sτ + 1e−θs (4.1)

Where τ is the time constant, K is the gain and θ the time delay in the system. Assumingthe step is applied at t = 0. For demonstration purposes the time delay is considered zero. Thenthe step response of the first order system is given by eq. 4.2 [23].

y(t) = K(1− e−t/τ ) (4.2)

26 CHAPTER 4. MODELLING

When the system reaches steady state eq. 4.3.

yss = K (4.3)

τ is 63.2 % of the steady state value. See figure 4.2

Figure 4.2: Step response of a first order system with K = 1 = τ = 1, θ = 0

Figure 4.2 shows visually how to determine the time constant and steady state gain or some-times referred to as the DC gain. This is not real data just for demonstration purposes.

The servo motor has a maximum rate at which the turning angle can change. By sendingto the motor a desired angle and measure actual angle we can determine the slew rate of themotor. Change in angle over a period of time. Figure 4.3 shows an example of how an outputcan behave due to slew rate effects. The slope of the output, degree rise over time is the slewrate. With that being said a formula for slew rate is listed below ed. 4.3.

Figure 4.3: Slew rate effect on a square wave

SR =∆δm∆t

(4.4)

4.1. OPEN LOOP EXPERIMENTS 27

4.1.1 Rudder delayThe modified rudder servo motor discussed in 3.5.3 was used to get feedback results. An inter-face was set up to control an Arduino UNO connected to the servo. Measurements and controlare done in LabView, See fig. 4.4.

Figure 4.4: Block diagram of LabView progam

The controlling interface in LabView can be seen in Figure 4.5. A rotary knob can be usedto control the servo angle as well as entered directly. The feedback is shown visually on a graphas well the exact value. A button is to save the results measured.

Figure 4.5: Controlling interface in LabView

Measurements are shown graphically on Figure 4.6. Time delay is little but there are slewrate effects. Table 4.1 shows measured slew rate.

Mean value of measured slew rate is 259.949 deg/s.

28 CHAPTER 4. MODELLING

Figure 4.6: Actual servo angle (red) vs. desired angle (blue

Table 4.1: Servo motor slew rate measurements

∆t[s] ∆δ[deg] SlewRate[deg/s]0.693 174.138 251.2810.639 177.051 277.0750.712 179.980 252.7800.694 179.237 258.266

4.2. THE MEASURING CIRCUIT 29

4.1.2 Constants τ & KFor design and tuning of the heading controller in Simulink constants τ and K are acquired bydoing physical tests on the vessel. A fixed turn degree is sent to the rudder servo and the turningrate measured. The time constant is acquired measuring the time the vessel takes to get to aconstant angular velocity value. The constant angular speed over the rudder angle is the gainconstant K. This maneuvering test is one of the oldest test used to determine the characteristicsof a vessel [20].

The data is logged to a text file with one line each sample and can also be visualized inreal-time on screen. After measuring and inspecting the raw data of the logged data, it is im-ported to Matlab for further analysis. A diagram of the measurement circuit is shown in fig. 4.8.

Figure 4.7: Turning rate test

4.2 The measuring circuit

Figure 4.8: Block diagram of the Measuring circuit

The InvenSense MPU-6050 contains a MEMS (Mechanical Electronic Measuring System)accelerometer and gyro in a single chip. Here a breakout is used because of its simplicity toset up. It contains a 16-bits ADC for each channel and therefore it captures x, y and y values

30 CHAPTER 4. MODELLING

all at the same time. The sensor uses the I2C communication protocol. The user interface setup in LabView is shown in fig. 4.9. The setup mounted on the vessel can be seen on fig. A.12appendix A

Offset biases are zeroed out automatically using the LabView library for the sensor. Onthe other hand the sensitivity needs to be set. Here the sensitivity is set to ± 250 °/s, highersensitivity range is not needed because the vessel will not be turning at extreme rates.The MPU 6050 gives out 131 for each degree turn per second so the output has to be dividedby that factor. See figure B.1.

Figure 4.9: Controlling interface in LabView

4.2.1 VelocityA rough estimation of velocity is retrieved from acceleration measurements. Acceleration is thesecond derivative of displacement with respect to time or the first derivative of velocity withrespect to time. See equation 4.5

a =dv

dt=d2x

dt2(4.5)

Velocity is the integral of acceleration over time. Because the MPU6050 sends out nu-merical values its integral has to be estimated using a numerical method. Here the trapezoidalmethod is used. By looking at figure 4.10 we see that the integral is approximated by the areaunder the curve formed by using straight line segments between points (xi−1, yi−1) and (xi, yi).The x intervals, in this case, are the sampling period of the accelerometer [22]. See figure. 4.10.

T =1

f= ∆x (4.6)

The area of the trapezoid is obtained by adding the area of a rectangle and a triangle. Seefigure 4.11, eq. 4.7.

4.2. THE MEASURING CIRCUIT 31

Figure 4.10: Numerical integral estimation

A = yo∆x+1

2(y1 − y0)∆x =

(y0 + y1)∆x

2(4.7)

Figure 4.11: Area of the trapezoid

By adding the area of the trapezoids the integral can be approximated which results in thetrapezoidal rule formula. Eq. 4.8∫ b

a

f(x)dx ≈ ∆x

2(y0 + 2y1 + 2y2 + ...+ 2n−1 + yn) (4.8)

Figure 4.12: Numerical integration in LabView

32 CHAPTER 4. MODELLING

Carrying out a trapezoidal integration in LabView can be done as shown in figure 4.12. Anoise filter is used for minor fluctuations in the signal (within 0.2g), then the acceleration isconverted to m/s2, summed up with the last value, divided by two, multiplied by the samplingloop time. That’s how the approximate trapezoidal integral is calculated. Then the previous ap-proximated velocity values are summed up with trapezoidal value of velocity caused by changein changes in acceleration.

As said before only a rough estimates of the velocity can be derived with this method due tothe long sampling time. Values of acceleration are changing much faster than can be accountedfor, so the solution to this problem would be to get a faster microcontroller which results inmore accurate velocity calculations.

To get a better velocity estimate a GPS is also used to determine the velocity. The GPSdiscussed in section 3.5.2 gives out a course over ground which can be used to estimate thevelocity.

4.2.2 Output signalsServo motors control the vessels rudder angle and throttle. Controlling sliders for these mo-tors can be seen in fig. 4.9. To acquire controlling signals a Rigol DS1104 oscilloscope wasconnected to the original controlling circuit in the vessel and extreme values documented.

Figure 4.13: Ouput signal of the rudder in idle position,

A screenshot of all measurements can be seen in appendix A. It can be seen that the positionof the Servomotors is controller by different duty cycles of the controlling signals. Results areshown in table 4.2. Minimum and maximum rudder values correspond to a ± 30° angle.

4.2. THE MEASURING CIRCUIT 33

Table 4.2: Measured output PWM of controlling servo motors

Rudder ThrottleMax 1800 ms 1880 msIdle 1520 ms 1440 msMin 1200 ms 1080 ms

As shown on figure 4.9 the throttle knob has 8 positions ranging from 0 - 7. Velocity of eachposition was measured with GPS receiver. A simple circuit with a wireless communication wasused to measure velocity. A block diagram of the circuit is shown below.

Figure 4.14: Block diagram of the GPS circuit

A schematic and PCB of the GPS receiver board is shown in Appendix A. The same boardwas used to make a prototype of the compass discussed in section 3.5 so it has auxiliary I2Coutputs.

4.2.3 Controlling circuitTo be able to control the vessel with both signals from the computer as well as from an RCremote a DPDT relay circuit is used. If the computer loses connection the remote will automat-ically take over.

Figure 4.15: Double pole double throw relay schematic [24]

The circuit was made on a prototyping board and can be seen in figure A.11 in Appendix A.A fritzing diagram shown below. fig. 4.16

34 CHAPTER 4. MODELLING

Figure 4.16: A fritzing diagram of the controlling circuit

35

Chapter 5

Results & Discussion

After trying out the boat it was clear that it wasn’t the optimal vessel for this project. It had tobe sailed at high speed (20 - 40 mph) to get enough cooling. A decision was made to re-scopethe project and make only measurements at one velocity and simulate a controller in MATLAB.

The listed new requirements for project were:

• Control - The boat is to be controlled by a computer software and by hand.

• Autopilot - Find all the tools for an autopilot system.

• Steering - Design a controller in MATLAB.

• Tracking The boat may not get lost and has to include a safety mechanism to preventthat.

5.1 Transfer functionA turning circle test was made on the vessel to determine the time constant τ and gain K. Theboat was turned at a constant speed of 3 km/h with the rudder excitation of 30°. Due to vibra-tions of the vessels motor a moving average is used to compensate these disturbances, then anexponential regression is used to get a good estimate of how the angular velocity changes overtime.

Only one measurement was useful and used here. The RC boat used for measurementsoverheated during the measurements so most of the measurements were not useful. See chapter6 for possible future solutions

It can be seen from figure 5.2 that the steady state value is 19.56°/s so the time constantis 63% of that value or 12.32°/s. Steady state gain is the ratio between the output and inputvariables. Time constant and gain can be seen in table 5.1

H(s) =r(s)

ω(s)=

0.652

1.612s+ 1(5.1)

36 CHAPTER 5. RESULTS & DISCUSSION

Figure 5.1: Measured angular velocity over time

Figure 5.2: Measured angular velocity over time

Table 5.1: Constants τ & K

ValueK 0.652τ 1.612 s

5.2. VELOCITY 37

5.2 VelocityThe turning radius was measured roughly 2.3m so using equation 5.2 calculated velocity fromangular velocity is 0.79 m/s (about 2.84 km/h) which almost matches the velocity at rudderexcitation. So GPS gives a fairly good estimate of the velocity. Parameter measurements wereonly acquired for this one velocity.

ω = vr =s

θ(5.2)

Figure 5.3: How angular velocity relates to directional velocity

5.3 ControllerA PID controller will be used to keep the vessel on a desired path. The controller will get aheading degree for a specified way point and regulate its heading continuously to stay on theright path. Controller is designed in MATLAB Simulink which saves a lot of time that wouldelse go into physical measurements. Slew rate of the servo motors discussed in section 4.1.1comes into the the system as a limiter. The output of the controller is summed up and fed intothe transfer function which gives out angular velocity that is integrated to get a heading changein degrees.

Figure 5.4: Block diagram of the controller

38 CHAPTER 5. RESULTS & DISCUSSION

5.3.1 Parameter tuningTuning of controlling parameters was done manually, gain constants increased to get the desiredvalues. An alternative approach would be to looking at the root locus plot but due to limitedtime it was not done here.

By looking at the step response of the whole system with the controller included we can seehow it responds to different parameters. The goal is minim the overshoot but also get a fairlygood response time.

Figure 5.5: P controller results

Step response for the P controller can be seen that increased gain results in higher overshootand more oscillations. The settling time is almost the same for all. Kp = 1 is considered the bestresult. Next we look at how adding an integrator to the controller effects the system response.

Keeping the proportional gain Kp at constant value of 1 and varying the integral gain from.1 to 10 results in damping of the oscillations. With Ki = 10 the system is overdamped andtakes quite the time to get to a steady value. Ki gain of 1 is giving a little overshoot but nothingserious so that is the optimal value for the integral gain.

Regarding the derivative gain it can be seen that the response reaches a desired value muchquicker but oscillates around the setpoint and takes much more time to settle. Ki gain of 30gives the best results here.

Choice of controller is discussed in chapter 6.

5.3. CONTROLLER 39

Figure 5.6: PI controller results, Kp = 1

Figure 5.7: PID controller results, Kp = 1, Ki = 1

40 CHAPTER 5. RESULTS & DISCUSSION

5.4 AHRS systemThe MPU9250 discussed in 3.5 provides heading information. The magnetic information it pro-vides is calibrated, processed and then converted to right data format (NMEA 0183) which issent to the controlling software. The code used is based on a program from "King Tide Sailing"and can be seen in figure C.3 in Appendix C

The UM7 also discussed in 3.5 was though to be a feasible heading sensor but due to itssensitivity in harsh environments it was not chosen. A binary parser from Mike Hoyer wasused for reference and adjusted for packet processing of raw binary information. See listing C.1in Appendix. A schematic diagram of the compass can be shown on figure A.10 in Appendix A.

5.5 User controlTimeZero Navigator is the user interface used to send commands to the controlling mechanism.The software sends out heading to steer (APB) that is the difference between the current anddesired heading. The software and its use is discussed in section 3.4.

A desired route is set in the user interface. Bearing is calculated, then it’s sent to the con-troller which processes the information and sends out an excitation angle to the steering rudder.See figure 5.8.

Figure 5.8: Block diagram of the steering process

41

Chapter 6

Conclusion

After narrowing the scope of the project and making measurements, a controller was simulated.All the tools for an autonomous controller are listed in this report. The vessel is controllablethrough a PC and by hand remotely. A safety mechanism is activated when the boat goes out ofreach. All of these features are useful for future work.

By making and assembling the equipment for measuring physical parameters of the boat alot of the functions needed for an autopilot had to be implemented such as wireless steering,controlling circuit, sensors and more.

6.1 Future workImplementing a controller for a little RC boat can resemble control of larger vessels. The boatused here is mainly intended for hobbyists and to be sailed at a much higher velocities. Themotor is water- and air cooled so when sailing at lower velocities it doesn’t get enough coolingwhich is probably the reason why it overheated during measurements.

The next step is to use an electric transom mount outboard motor. Possibly the MinnKoda Power drive because it’s electrically turned which gives room for easy control. It willbe mounted onto boat hull and the impulse response acquired.

Figure 6.1: Minn Koda Power drive 70 [25]

42 CHAPTER 6. CONCLUSION

Similar approach in measurements and simulations will be done with better AHRS and po-sitioning system.

6.2 Control theoryModelling a system to be controlled is key in designing a controller discussed here. To be ableto simulate the controller first and then implement it physically saves a lot of time and effort.

Experiments done in this project are initial steps in designing a controller for an autonomousvessel. To get a better estimation of how any vessel moves in water a more accurate approachcan be made by getting the impulse response and then taking the laplace transform of the output.That way the exact transfer function can be derived. This came clear when doing research aftermeasurements had been made.

Results for the controller parameters discussed earlier in section 5.3.1 show that a PI or aPID controller is the most applicable choice for this system. Table 6.1 shows the gain constantsfor the vessels controller.

Table 6.1: Parameter results for the vessel controller

Controller Kp Ki KdPI 1 1 -

PID 1 1 30

6.3 AHRS systemA more sophisticated tilt compensated compass could be used for the vessel to steer more accu-rately. A lot of marine heading and GPS sensors are available out there which are robust and canhandle harsh environments. Aluminum casing, waterproof connectors for lasting integration invibrating environments and meets IP 67 standards.

After a search on the internet the MTi-G-710 seems to be a good choice especially becauseof its robust casing and vibration rejection gyros. It has high IP rating and is considered forfuture development of this projects. Specifications of the MTi-G-710 can bee seen in AppendixB Figure 6.2.

An alternative approach would be to have the compass and GPS in two separate units. TheFuruno PG-700 is a marine compass which offer high precision (± 1°) and within a rugged case(IP 55 proved) [27]. Specifications of the Furuno PG-700 can bee seen in Appendix B FigureB.3.

The NV08C-CSM-BRD Card is an integrated circuit that provides precise location. It cansend out NMEA 0183 data which is useful because it doesn’t have to be manipulated for thecomputer software. Specifications for the NV08C-CSM-BRD can be seen in Appendix B Fig-

6.4. SOFTWARE 43

Figure 6.2: MTi-G-710 AHRS and positioning system [26]

ure B.4.

Figure 6.3: Furuno PG-700 Compass

Figure 6.4: NV08C-CSM-BRD GPSreceiver

6.4 Software

The software TimeZero Navigator is used. It combines both steering functions and depth mea-surements in one user friendly interface. The communication protocol (NMEA-0183) used issimple and can be easily manipulated. All the requirements for an autopilot are met with thissoftware. Chart plotter, real-time depth measurements and autopilot steering capabilities.

44 CHAPTER 6. CONCLUSION

6.5 Wireless communicationThe Digi XBee-PRO 900HP is a more powerful transceiver than discussed in section 3.2.2 andprovides better range. It’s easy to set up in XCTU interface. Similar to the XBee used here butmore powerful. Line-of-sight range is over 4km and transmit power up to 250 mW.Specificationsfor the XBee-PRO 900HP can be seen in Appendix B Figure B.5.

Figure 6.5: Digi XBee-PRO 900HP [28]

45

Bibliography

[1] 1. PRINCIPLES OF THE USE OF THE SONAR SYSTEM FOR FISH BIOMASS ESTI-MATES. [Online]. Available: http://www.fao.org/docrep/003/x6602e/x6602e02.htm#TopOfPage (visited on 09/21/2017).

[2] Saildrone - ocean data redefined. [Online]. Available: http://saildrone.com(visited on 09/21/2017).

[3] Oceanscience: Autonomy to z-boat’s capabilities, Subsea World News. [Online]. Avail-able: http://subseaworldnews.com/2014/08/15/oceanscience-autonomy-to-z-boats-capabilities/ (visited on 09/25/2017).

[4] How to make a drone / UAV - lesson 4: Flight controller, RobotShop Blog, May 25,2015. [Online]. Available: http://www.robotshop.com/blog/en/how-to-make-a-drone-uav-lesson-4-flight-controller-15191 (visited on09/25/2017).

[5] Pixhawk autopilot - pixhawk flight controller hardware project. [Online]. Available: https://pixhawk.org/modules/pixhawk (visited on 09/25/2017).

[6] NAVPILOT 711c. [Online]. Available: https://www.furunousa.com/en/products/navpilot_711c (visited on 11/30/2017).

[7] Arduino nano. [Online]. Available: https://store.arduino.cc/usa/arduino-nano (visited on 11/30/2017).

[8] Pololu - wixel programmable USB wireless module (fully assembled). [Online]. Avail-able: https://www.pololu.com/product/1336 (visited on 12/04/2017).

[9] Digi XBee and XBee-PRO zigbee RF modules - digi international. [Online]. Available:https://www.digi.com/products/xbee-rf-solutions/2-4-ghz-modules/xbee-zigbee#overview (visited on 11/30/2017).

[10] Rockstar 48-inch catamaran gas powered: RTR (PRB09003): Pro boat models. [Online].Available: http://www.proboatmodels.com/Products/Default.aspx?ProdID=PRB09003 (visited on 10/17/2017).

[11] What is GPS? everyday mysteries. [Online]. Available: http://www.loc.gov/rr/scitech/mysteries/global.html (visited on 10/30/2017).

[12] t. U. S. government the, The american practical navigator.

[13] Pololu - UM7-LT orientation sensor. [Online]. Available: https://www.pololu.com/product/2740 (visited on 10/03/2017).

[14] SparkFun IMU breakout - MPU-9250 - SEN-13762 - SparkFun electronics. [Online].Available: https : / / www . sparkfun . com / products / 13762 (visited on10/03/2017).

46 BIBLIOGRAPHY

[15] Atan2 - c++ reference. [Online]. Available: http://www.cplusplus.com/reference/cmath/atan2/ (visited on 12/04/2017).

[16] Servo feedback hack (free), Instructables.com. [Online]. Available: http://www.instructables.com/id/Servo-Feedback-Hack-free/ (visited on 10/20/2017).

[17] Extra high torque servo (SPMS601h): Pro boat models. [Online]. Available: http://www.proboatmodels.com/Products/Default.aspx?ProdID=SPMS601H(visited on 10/20/2017).

[18] C.-Y. Tzeng and J.-F. Chen, “Fundamental properties of linear ship steering dynamicmodels”, Journal of Marine Science and Technology, vol. 7, no. 2, pp. 79–88, 1999.

[19] Z. Yu, X. BAO, and K. Nonami, “Course keeping control of an autonomous boat usinglow cost sensors”, vol. 2, pp. 389–400, Jan. 2008.

[20] T. I. Fossen, Handbook of marine craft hydrodynamics and motion control =: Vademe-cum de navium motu contra aquas et de motu gubernando. Chichester, West Sussex:Wiley, 2011, 575 pp., OCLC: ocn699374017, ISBN: 978-1-119-99149-6.

[21] MSS GNC - download page. [Online]. Available: http://www.marinecontrol.org/download.html (visited on 12/04/2017).

[22] S. Troisi, E. Servidio, and A. Provost, “Movement and position detection processing datafrom an accelerometer: Motion simulation of an AUV with a inertial navigation systembased on myrio”, Instrumentation viewpoint, no. 16, pp. 40–41, 2014.

[23] H. Niemann and R. Miklos, “A simple method for estimation of parameters in first ordersystems”, Journal of Physics: Conference Series, vol. 570, no. 1, p. 012 001, Dec. 16,2014, ISSN: 1742-6588, 1742-6596. DOI: 10.1088/1742-6596/570/1/012001.[Online]. Available: http://stacks.iop.org/1742-6596/570/i=1/a=012001?key=crossref.3f1087b7fd7daaf8a206bce59160daf3 (visitedon 10/25/2017).

[24] Switch types and common terminology - national instruments. [Online]. Available: http://www.ni.com/white-paper/3960/en/ (visited on 11/26/2017).

[25] Minn kota motors. [Online]. Available: https://www.minnkotamotors.com/Products/NEW- POWERDRIVE- 70- LB- 54- INCH- SHAFT/ (visited on12/01/2017).

[26] (). AHRS attitude heading reference system, Xsens 3D motion tracking, [Online]. Avail-able: https://www.xsens.com/tags/ahrs/ (visited on 12/01/2017).

[27] INTEGRATED HEADING SENSOR PG-700 | compass | products | FURUNO. [Online].Available: http://www.furuno.com/en/products/compass/PG-700(visited on 12/04/2017).

[28] Long range 900 MHz OEM RF module - digi international. [Online]. Available: https://www.digi.com/products/xbee-rf-solutions/sub-1-ghz-modules/xbee-pro-900hp (visited on 12/03/2017).

[29] F. Vora, S. Kanojia, M. Sayad, and F. Sayyad, “GESTURE CONTROLLED POWER-POINT”,

[30] NV08c-CSM-BRD. [Online]. Available: http://www.nvs-gnss.com/products/oemsolutions/item/35-nv08c-csm-brd.html (visited on 12/04/2017).

BIBLIOGRAPHY 47

[31] M. Hoyer, UM7-arduino: Binary um7 parser for the arduino due microcontroller, original-date: 2015-04-04T16:20:20Z, Sep. 27, 2017. [Online]. Available: https://github.com/mikehoyer/UM7-Arduino (visited on 11/30/2017).

[32] How to make an NMEA-0183 tilt-compensated compass and rate-of-turn indicator. [On-line]. Available: http://kingtidesailing.blogspot.com/2015/09/how-to-make-nmea-0183-tilt-compensated.html (visited on 11/30/2017).

48

49

Appendix A

Pictures

Figure A.1: Output signal analysis

50 APPENDIX A. PICTURES

Figure A.2: Ouput signal of the rudder in idle position

Figure A.3: Ouput signal of the rudder at a maximum angle of 30°

51

Figure A.4: Ouput signal of the rudder at a minimum angle of -30°

Figure A.5: Ouput signal of the throttle at idle

52 APPENDIX A. PICTURES

Figure A.6: Ouput signal of the throttle at max

Figure A.7: Ouput signal of the throttle at min

53

Figure A.8: Rudder angle measured

Figure A.9: PCB board for the GPS compass

54 APPENDIX A. PICTURES

Figure A.10: Schematic diagram of the GPS Compass

Figure A.11: Switching relay circuit for boat control

55

Figure A.12: Measurement system mounted on the boat

56

57

Appendix B

Datasheets

Figure B.1: MPU6050 Sensitivity values [29]

Figure B.2: MTi-G-710 Specification [26]

58 APPENDIX B. DATASHEETS

Figure B.3: Furuno PG-700 Specifications[27]

Figure B.4: NV08C-CSM-BRD integrated GPS receiver specifications[30]

59

Figure B.5: Digi XBee-PRO 900HPspecifications[28]

60

61

Appendix C

Code

Listing C.1: UM7 header class [31]12 #include <stdlib.h>34 class UM7{5 public:6 short roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate;78 UM7();9

10 bool encode(byte c);1112 private:1314 int state;1516 enum ←↩

↪→{STATE_ZERO,STATE_S,STATE_SN,STATE_SNP,STATE_PT,STATE_DATA,STATE_CHK1,STATE_CHK0};1718 byte packet_type;19 byte address;20 bool packet_is_batch;21 byte batch_length;22 bool packet_has_data;23 byte data[30];24 byte data_length;25 byte data_index;2627 byte checksum1; // First byte of checksum28 byte checksum0; // Second byte of checksum2930 unsigned short checksum10; // Checksum received from packet31 unsigned short computed_checksum; // Checksum computed from bytes ←↩

↪→received3233 bool checksum(void);3435 void save(void);36 };

62 APPENDIX C. CODE

Listing C.2: UM7 Binary packet processing functions [31]1 #include "UM7.h"23 #define DREG_EULER_PHI_THETA 0x70 // Packet address sent from the UM7 ←↩

↪→that contains roll,pitch,yaw and rates.45 UM7::UM7() : state(STATE_ZERO){} // Default constructor67 bool UM7::encode(byte c){89 switch(state){

10 case STATE_ZERO:11 if (c == 's'){12 state = STATE_S; // Entering state S from state Zero13 } else {14 state = STATE_ZERO;15 }16 return false;17 case STATE_S:18 if (c == 'n'){19 state = STATE_SN; // Entering state SN from state S20 } else {21 state = STATE_ZERO;22 }23 return false;24 case STATE_SN:25 if (c == 'p'){26 state = STATE_SNP; // Entering state SNP from state SN. ←↩

↪→Packet header detected.27 } else {28 state = STATE_ZERO;29 }30 return false;31 case STATE_SNP:32 state = STATE_PT; // Entering state PT from state SNP. Decode ←↩

↪→packet type.33 packet_type = c;34 packet_has_data = (packet_type >> 7) & 0x01;35 packet_is_batch = (packet_type >> 6) & 0x01;36 batch_length = (packet_type >> 2) & 0x0F;37 if (packet_has_data){38 if (packet_is_batch){39 data_length = 4 * batch_length; // Each data packet is 4 ←↩

↪→bytes long40 } else {41 data_length = 4;42 }43 } else {44 data_length = 0;45 }46 return false;47 case STATE_PT:48 state = STATE_DATA; // Next state will be READ_DATA. Save ←↩

↪→address to memory. (eg 0x70 for a DREG_EULER_PHI_THETA ←↩↪→packet)

49 address = c;50 data_index = 0;51 return false;

63

52 case STATE_DATA: // Entering state READ_DATA. Stay in state until ←↩↪→all data is read.

53 data[data_index] = c;54 data_index++;55 if (data_index ≥data_length){56 state = STATE_CHK1; // Data read completed. Next state will ←↩

↪→be CHK157 }58 return false;59 case STATE_CHK1: // Entering state CHK1. Next state will be CHK060 state = STATE_CHK0;61 checksum1 = c;62 return false;63 case STATE_CHK0:64 state = STATE_ZERO; // Entering state CHK0. Next state will be ←↩

↪→state Zero.65 checksum0 = c;66 return checksum();67 }68 }6970 bool UM7::checksum(){71 checksum10 = checksum1 << 8; // Combine checksum1 and checksum072 checksum10 |= checksum0;73 computed_checksum = 's' + 'n' + 'p' + packet_type + address;74 for (int i = 0; i < data_length; i++){75 computed_checksum += data[i];76 }77 if (checksum10 == computed_checksum){78 save();79 return true;80 } else {81 return false;82 }83 }8485 void UM7::save(){86 switch(address){87 case DREG_EULER_PHI_THETA : // data[6] and data[7] are unused.88 if(packet_is_batch){89 roll = data[0] << 8;90 roll |= data[1];91 pitch = data[2] << 8;92 pitch |= data[3];93 yaw = data[4] << 8;94 yaw |= data[5];95 roll_rate = data[8] << 8;96 roll_rate |= data[9];97 pitch_rate = data[10] << 8;98 pitch_rate |= data[11];99 yaw_rate = data[12] << 8;

100 yaw_rate |= data[13];101 }else{102 roll = data[0] << 8;103 roll |= data[1];104 pitch = data[2] << 8;105 pitch |= data[3];106 }107 break;

64 APPENDIX C. CODE

108 }109 }

Listing C.3: Code for Arduino Compass (shortened)[32]12 //Code based on an example from ←↩

↪→http://kingtidesailing.blogspot.is/2015/09/how-to-make-nmea-0183-tilt-compensated.html34 #include "quaternionFilters.h"5 #include "MPU9250.h"6 #include <PString.h>7 #include <SoftwareSerial.h>89 SoftwareSerial mySerial(10, 11); // RX, TX

1011 const long INTERVAL = 300;12 #define SerialDebug false // Set to true to get Serial output for ←↩

↪→debugging13 #define degreesFx true1415 #define outputHDM true // magnetic heading16 #define outputHDT true // true heading1718 // Pin definitions19 int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext ←↩

↪→int pins20 int myLed = 13; // Set up pin 13 led for toggling2122 float magd = 13.0; // magnetic deviation; if it's East, it's negative ←↩

↪→(14E in San Francisco 2015)2324 MPU9250 myIMU;2526 byte checksum(char* str)27 {28 byte cs = 0;29 for (unsigned int n = 1; n < strlen(str) - 1; n++)30 {31 cs ^= str[n];32 }33 return cs;34 }3536 void setup()37 {38 Wire.begin();39 // TWBR = 12; // 400 kbit/sec I2C speed40 Serial.begin(9800);4142 // Set up the interrupt pin, its set as active high, push-pull43 pinMode(intPin, INPUT);44 digitalWrite(intPin, LOW);45 pinMode(myLed, OUTPUT);46 digitalWrite(myLed, HIGH);4748

65

49 // Read the WHO_AM_I register, this is a good test of communication50 byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);51 Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX);52 Serial.print(" I should be "); Serial.println(0x71, HEX);5354 mySerial.begin(9600);555657 if (c == 0x71) // WHO_AM_I should always be 0x6858 {59 Serial.println("MPU9250 is online...");6061 // Start by performing self test and reporting values62 myIMU.MPU9250SelfTest(myIMU.SelfTest);63 Serial.print("x-axis self test: acceleration trim within : ");64 Serial.print(myIMU.SelfTest[0],1); Serial.println("% of factory ←↩

↪→value");65 Serial.print("y-axis self test: acceleration trim within : ");66 Serial.print(myIMU.SelfTest[1],1); Serial.println("% of factory ←↩

↪→value");67 Serial.print("z-axis self test: acceleration trim within : ");68 Serial.print(myIMU.SelfTest[2],1); Serial.println("% of factory ←↩

↪→value");69 Serial.print("x-axis self test: gyration trim within : ");70 Serial.print(myIMU.SelfTest[3],1); Serial.println("% of factory ←↩

↪→value");71 Serial.print("y-axis self test: gyration trim within : ");72 Serial.print(myIMU.SelfTest[4],1); Serial.println("% of factory ←↩

↪→value");73 Serial.print("z-axis self test: gyration trim within : ");74 Serial.print(myIMU.SelfTest[5],1); Serial.println("% of factory ←↩

↪→value");7576 // Calibrate gyro and accelerometers, load biases in bias registers77 myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);7879 myIMU.initMPU9250();80 // Initialize device for active mode read of acclerometer, ←↩

↪→gyroscope, and81 // temperature82 Serial.println("MPU9250 initialized for active data mode....");8384 // Read the WHO_AM_I register of the magnetometer, this is a good ←↩

↪→test of85 // communication86 byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);87 Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX);88 Serial.print(" I should be "); Serial.println(0x48, HEX);8990 // Get magnetometer calibration from AK8963 ROM91 myIMU.initAK8963(myIMU.magCalibration);92 // Initialize device for active mode read of magnetometer93 Serial.println("AK8963 initialized for active data mode....");94 if (SerialDebug)95 {96 // Serial.println("Calibration values: ");97 Serial.print("X-Axis sensitivity adjustment value ");98 Serial.println(myIMU.magCalibration[0], 2);99 Serial.print("Y-Axis sensitivity adjustment value ");

66 APPENDIX C. CODE

100 Serial.println(myIMU.magCalibration[1], 2);101 Serial.print("Z-Axis sensitivity adjustment value ");102 Serial.println(myIMU.magCalibration[2], 2);103 }104105 } // if (c == 0x71)106 else107 {108 Serial.print("Could not connect to MPU9250: 0x");109 Serial.println(c, HEX);110 while(1) ; // Loop forever if communication doesn't happen111 }112 }113114 void loop()115 {116 // If intPin goes high, all data registers have new data117 // On interrupt, check if data ready interrupt118 if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)119 {120 (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS)121 // Serial print and/or display at 0.5 s rate independent of data ←↩

↪→rates122 myIMU.delt_t = millis() - myIMU.count;123124 if(SerialDebug)125 {126 //print out magnetic readings plus the offset biases127 Serial.print( (int)myIMU.mx + 416);128 Serial.print(",");129 Serial.print( (int)myIMU.my + 229);130 Serial.print(",");131 Serial.print( (int)myIMU.mz + 423);132 Serial.println("");133 }134135 if(degreesFx){136137 float Pi = 3.14159;138139 int mX = (int)myIMU.mx + 416;140 int mY = (int)myIMU.my + 229;141 int mZ = (int)myIMU.mz + 423;142143 float heading = (-(atan2(mY,mX) * 180) / Pi);144145 // Normalize to 0-360146 if (heading < 0)147 {148 heading = 360 + heading;149 }150 if(outputHDM){151 //output magnetic heading152 char hdmSentence [23];153 byte csm;154 PString strm(hdmSentence, sizeof(hdmSentence));155 strm.print("$HCHDM,");

67

156 strm.print(lround(heading)); // lround simply rounds out ←↩↪→the decimal, since a single degree is fine enough of ←↩↪→a resolution

157 strm.print(",M*");158 csm = checksum(hdmSentence);159160 if (csm < 0x10) strm.print('0');161 strm.print(csm, HEX);162 Serial.println(hdmSentence);163164 }165 if (outputHDT){166167 // HDT True Heading //168 float headingTrue = heading - magd;169 char hdtSentence [23];170 byte cst;171 PString strt(hdtSentence, sizeof(hdtSentence));172 strt.print("$HCHDT,");173 strt.print(lround(headingTrue));174 strt.print(",T*");175 cst = checksum(hdtSentence);176177 if (cst < 0x10) strt.print('0');178 strt.print(cst, HEX);179 Serial.println(hdtSentence);180 }//check if there is GPS information from the spftware serial181 if (mySerial.available() > 0) {182183 while(mySerial.available()){184185 Serial.write(mySerial.read());186 }187 Serial.println("");188 }189 }190191192 myIMU.count = millis();193 myIMU.sumCount = 0;194 myIMU.sum = 0;195 } // if (myIMU.delt_t > 500)196 }

Listing C.4: MATLAB code for controller parameter tuning12 %% - P control tuning3 %Gain and time constant45 figure(1)6 K = 0.651;7 T = 1.612;89 num = [K];

10 denom = [T 1];11 integrator = 1/s;

68 APPENDIX C. CODE

1213 %Plant Gp and feedback H.14 Gp = tf(num, denom)*integrator;15 H = [1];1617 M = feedback(Gp, H)18 step(M)19 hold on;2021 %%2223 Kp = 3;24 Ki = 0;25 Kd = 0;2627 %Controller28 Gc = pid(Kp, Ki, Kd);2930 %System with a controller31 Mc = feedback(Gc*Gp, H);32 step(Mc)33 grid on;34 %%3536 Kp = 10;37 Ki = 0;38 Kd = 0;3940 %Controller41 Gc = pid(Kp, Ki, Kd);4243 %System with a controller44 Mc = feedback(Gc*Gp, H);45 step(Mc)46 grid on;47 title('P control with different Gain constants ←↩

↪→Kp','Interpreter','latex','FontSize',14);48 legend({'Kp = 1','Kp = 3','Kp = 10'})49 %% - PI control Tuning50 figure(2)5152 Kp = 1;53 Ki = 0;54 Kd = .1;5556 %Controller57 Gc = pid(Kp, Ki, Kd);5859 %System with a controller60 Mc = feedback(Gc*Gp, H);61 step(Mc)62 hold on;6364 Kd = 1;6566 %Controller67 Gc = pid(Kp, Ki, Kd);6869 %System with a controller

69

70 Mc = feedback(Gc*Gp, H);71 step(Mc)7273 Kd = 10;7475 %Controller76 Gc = pid(Kp, Ki, Kd);7778 %System with a controller79 Mc = feedback(Gc*Gp, H);80 step(Mc)8182 title('PI control with Gain Kp = 1, different ←↩

↪→Ki','Interpreter','latex','FontSize',14);83 legend({'Ki = .1','Ki = 1','Ki = 10'})84 grid on;8586 %% PID controller87 figure(3);8889 Kp = 1;90 Ki = 1;91 Kd = 15;9293 %controller94 Gc = pid(Kp, Ki, Kd);9596 Mc = feedback(Gc*Gp, H);97 step(Mc)98 hold on;99

100 %Kd = 20101 Kd = 20;102103 %controller104 Gc = pid(Kp, Ki, Kd);105106 Mc = feedback(Gc*Gp, H);107 step(Mc)108109 %Kd = 20110 Kd = 30;111112 %controller113 Gc = pid(Kp, Ki, Kd);114115 Mc = feedback(Gc*Gp, H);116 step(Mc)117118 title('PID control with Gain Kp = 1, Ki = 1, different ←↩

↪→Kd','Interpreter','latex','FontSize',14);119 legend({'Ki = 15','Ki = 20','Ki = 30'})120 grid on;

70

School of Science and EngineeringReykjavík UniversityMenntavegur 1101 Reykjavík, IcelandTel. +354 599 6200Fax +354 599 6201www.ru.is