A ALBORG U NIVERSITY Department of Control Engineering Fredrik Bajers Vej 7 - 9220 Aalborg Øst - Telephone 96 35 80 80
c
TITLE:
Development and Navigation of an Autonomous UAV PROJECT PERIOD: September 5th - June 1st
PROJECT GROUP: 1034B
GROUP MEMBERS: Jacob T. Holmgaard Christian S. Jensen Stefan Lænner
SUPERVISORS: Anders la Cour-Harbo
NUMBERS PRINTED: 8 REPORT PAGE NUMBER: 135 TOTAL PAGE NUMBER: 225
SYNOPSIS: This thesis concerns development and navigation of an autonomous UAV platform, able to complete the levels 1 and 2 of IARC06. After a design of the UAV hardware and software platform, the applications needed to complete the levels are defined. To guide the UAV through the levels 1 and 2, a navigation system was developed. A map over the area in which the missions are to be executed has been derived. From this map the GPS coordinates for the flight path has been extracted, and through optimization algorithms a near optimal flight path has been found. Through grounded tests the navigation system qualitively proved to be able to complete levels.
Preface This thesis is made by group 1034B at the Department of Control Engineering at Aalborg University in the period between September 5th 2005 and June 1st 2006. The thesis has been done as a part of the development of an autonomous helicopter based UAV in-progress at the control department at Aalborg University. The report consists of four parts: "Mission Description and Problem Formulation", "Platform Design", "Mission Applications", and "Conclusion and Perspective". The four parts all begin with an introduction and consist of several chapters each containing a description of the content and, if necessary, a short summary. Supplementary subjects are placed in appendices in the back of the report and are numbered A, B and so forth. Enclosed on the inside of the back page is the project CD, containing M ATLAB code, S IMULINK models, C code, and a pdf copy of this report.
Reading Instructions To help guide the reader through the report, a reading guide is presented. throughout the thesis different text syntaxes are used. When writing the name of a variable, the text is emphasized and when writing the name of an interprocess communication message, the text is capitalized. An example of a variable is variable and an example of a message is MESSAGE. Figures and tables are numbered consecutively starting over from each chapter, for example the 4rd Figure in the 3th chapter would be Figure 3.4. When referring to an equation, the number is simply in parenthesis, such that if a reference is made to the 7th equation in the 2th chapter it would say: "as seen in (2.7)". References to literature will throughout the thesis be written in square brackets containing the author’s surname and the year of the III
publishing. This refer to the bibliography, where further information about the source can be found. The reference to a source might also contain a page number if necessary, e.g. [Fossen 2002, p. 5].
Jacob T. Holmgaard
Stefan Lænner
IV
Christian S. Jensen
Contents Introduction
1
I Mission Description and Problem Statement
5
1 Mission Overview 1.1 Description of Levels . . . . . . . . . . . . . . . . . . . . 1.2 Mission Approach . . . . . . . . . . . . . . . . . . . . . .
7 7 8
2 Problem Formulation
11
II Platform Design
13
3 Hardware Platform 3.1 Platform Description . . . . . . . . . . . . . . . . . . . . 3.2 Hardware Implementation . . . . . . . . . . . . . . . . .
15 15 19
4 Software Platform 4.1 Software Environment . . . . . . . . . . . . . . . . . . . 4.2 Software Architecture . . . . . . . . . . . . . . . . . . . .
23 23 23
III Mission Applications
31
5 Navigator 5.1 Analysis of Navigator . 5.2 Design of Navigator . . 5.3 Simulation of Navigator 5.4 Test of Navigator . . . . 5.5 Tuning of Navigator . . 6 Vision
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
33 33 40 52 59 65 69 V
CONTENTS 6.1 6.2 6.3
Analysis of Vision . . . . . . . . . . . . . . . . . . . . . . Design of Vision . . . . . . . . . . . . . . . . . . . . . . . Test of Vision . . . . . . . . . . . . . . . . . . . . . . . . .
69 70 72
7 Mission Control 7.1 Design of Mission Control . . . . . . . . . . . . . . . . . 7.2 Test of Mission Control . . . . . . . . . . . . . . . . . . .
75 75 78
8 Flight Data Logger 8.1 Analysis of Flight Data Logger . . . . . . . . . . . . . . 8.2 Design of Flight Data Logger . . . . . . . . . . . . . . .
81 81 82
9 Ground Station 9.1 Analysis of Ground Station . . . . . . . . . . . . . . . . 9.2 Design of Ground Station . . . . . . . . . . . . . . . . . 9.3 Test of Ground Station . . . . . . . . . . . . . . . . . . .
83 83 85 88
10 Map Generation for UAV Navigation 10.1 Analysis of Map Generation . . . 10.2 House Map . . . . . . . . . . . . . 10.3 Waypoint Placement . . . . . . . 10.4 Coordinate Transformation . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
93 93 94 95 96
11 Generation and Optimization of Level 2 Path 11.1 Basic Graph Theory . . . . . . . . . . . . . 11.2 Description of Path Generation Problem . 11.3 Methods . . . . . . . . . . . . . . . . . . . 11.4 Comparison of Heuristics . . . . . . . . . 11.5 Path Generation for Level 2 . . . . . . . . 11.6 Evaluation of Generated Path . . . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
109 110 111 112 117 118 123
. . . .
. . . .
. . . .
. . . .
IV Conclusion and Perspectives
127
12 Conclusion
129
13 Perspectives
133
Bibliography
134
VI
CONTENTS
Appendix
135
A RTAI Linux Platform
137
B Comparison of Route Generation Heuristics
139
C Helicopter code C.1 Mission Control Code . . . . . . C.2 Navigator Code . . . . . . . . . . C.3 Controller Code . . . . . . . . . . C.4 Ground Station Code . . . . . . . C.5 Logger Code . . . . . . . . . . . . C.6 Vision Code . . . . . . . . . . . . C.7 Platform definitions Header File
. . . . . . .
143 143 148 165 169 189 196 205
D Route Generation Code D.1 Optimal Route Algorithm . . . . . . . . . . . . . . . . . D.2 Nearest Neighbor Heuristic . . . . . . . . . . . . . . . . D.3 2-OPT Optimization Heuristic . . . . . . . . . . . . . . .
213 213 213 214
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
E Map Generation Code 217 E.1 Map Transformation Algorithm . . . . . . . . . . . . . . 217 E.2 Relative House Coordinates . . . . . . . . . . . . . . . . 219 E.3 Relative Waypoint Coordinates . . . . . . . . . . . . . . 219 F Listing of Interprocess Communication Messages
225
VII
Introduction The motivation for this thesis is to participate in the International Aerial Robotics Competition 2006 (IARC06) at Fort Benning, Georgia, USA. It is a competition organized by the Association for Unmanned Vehicle Systems International (AUVSI), which is the world’s largest non-profit organization devoted exclusively to advancing the unmanned systems community. The purpose of the competition is to make an Unmanned Aerial Vehicle (UAV) perform a defined mission. AUVSI have defined three mission examples, which all require the same behavior and all would be dangerous for people to carry out. One mission example is a hostage situation, where a group of people have been detained by a terrorist group at an embassy. 3 km from the embassy a submarine is situated, in which a rescue team is ready to launch a UAV which is intended to determine how many terrorists are guarding the hostages. The embassy can be identified by the national seal over the main entrance, and can be entered through some open windows. The mission is to make the UAV navigate the 3 km to the embassy, and then recognize the building visually by identifying the national seal on the wall. Furthermore the UAV has to investigate the building further to locate an entrance large enough for deploying a vehicle into the building. This vehicle must provide a visual link to the hostage situation, enabling the rescue team to identify the number of terrorists. The mission has to be carried out in less than 15 minutes in order for the rescue team not to loose the advantage of the element of surprise. A full description of the missions can be found on the website [AUVSI 2006]. The content of the UAV missions in the other scenarios are similar to the mission described above and forms four levels to complete in the IARC06. The McKenna MOUT Site at Fort Benning in Georgia, USA, at which the IARC06 is held, is a military training facility used by the U.S. Army Battle Lab. The facility is used in the conduct of training exercises, experimentation with aerial and ground unmanned platforms, and testing of new equipment and concepts for the U.S. Army and other Armed Forces. A view of the area is shown in Figure 1. The facility forms a realistic urban environment which features 15 European style buildings forming a small city. This makes the site well suited for experimental UAV missions and therefore also for hosting 1
INTRODUCTION
Figure 1: Elevated view of the facility at the McKenna MOUT Site.
the IARC06. In this thesis the project group have developed a fully functional autonomous UAV platform. The majority of the components have been designed and implemented from the ground up, and all of the the following topics have been considered by the project group in this project: • Development of hard real-time multi thread software environment. • Real-time constrains in relation to control, navigation, and communication. • Flexible navigation strategies and methods. • Physical modifications of model helicopter. • Flight path generation and optimization. • Software/hardware interfacing. • Automated building of maps for navigation. • Thorough ground test of hardware, real-time software, and algorithms. • Visual recognition and categorization. • Development of on-board electronics. • In-flight error handling of unforseen events. • Aerial test of hardware, real-time software, and algorithms. 2
Due to the extensive scope of the development of the UAV other people of the local UAV group has also been involved in the development process. Since several simultaneous projects uses the UAV platform, the following tasks has not been handled by this group: • Development of helicopter simulation math model, sensor-fusion, observer, and controller. • Parts of the on-board electronics. • Wire harness and power supply. • Flight approval test. • Vision recognition algorithms. • Sensor server software. This briefly describes the scope of this project. In the next chapter, the mission description and problem statement will be presented.
3
Part I
Mission Description and Problem Statement This first part of the project report contains two chapters concerning the rules and content of the IARC06, and the group’s preliminary considerations of this project. The first chapter gives an introduction to the IARC06 and defines the tasks of the competition, followed by a description of the approach intended by the group. The second chapter forms the overall problem formulation for this project.
5
Mission Overview
1
The IARC06 level 1, 2, 3, and 4 requirements, and mission approach presented in this chapter, forms the basis for the problem formulation in chapter 2.
1.1 Description of Levels Based on the mission mentioned in the introduction on page 1, AUVSI has defined 4 levels for the autonomous UAV mission of the IARC06. The four levels of the competition each consists of autonomous tasks that has to be completed by the UAV within specific rules. In each level the autonomy required to complete the level increases making each level more difficult to complete. In level 1 the scope is to autonomously fly a route of approximately 3 km in less than 15 minutes. This route will consist of a set of GPS coordinates (waypoints) placed in the landscape surrounding the city. The coordinates are provided immediately prior to the flight. In level 2 the UAV must fly over a city in search of a marker in form of the sign shown in Figure 1.1. This marker is placed on a wall on one of the buildings in the city. The UAV must first locate the marker and then proceed with a further investigation of the house, on which the marker is found, to locate entrances larger than 1x1 m. These entrances could be either windows or doors, the marker is not necessarily placed on the same side of the building as the best suited entrance. For level 2 to be successfully completed, the mission must be carried out in under 15 minutes. Once the entrance is located level 2 is complete. In level 3 the UAV must deploy another autonomous vehicle through the located entrance. This vehicle must be autonomous, but not necessarily aerial. When inside the building, the vehicle must photograph all the walls and transmit the pictures to the helicopter, this must be done in under 15 minutes. In level 4 the level is completed successfully if level 1, 2, and 3 are carried out in less than 15 minutes. 7
CHAPTER 1. MISSION OVERVIEW
Figure 1.1: Illustration the marker the UAV must recognize in level 2.
As the framework of the IARC06 now has been presented, the mission approach will be described in the next section.
1.2 Mission Approach This project focuses on completing level 1 and 2 of the IARC06. The first step is to define the strategy for completing these levels. Numerous considerations are involved in this. However, only the result of these, the so-called mission approach, is shown here. The mission approach describes the steps needed to complete the levels 1 and 2. In Figure 1.2 an overview of the mission approach formed by the group is illustrated. Firstly a path must be derived on-board and in real-time on the UAV based on the waypoints given for the level 1 flight. It is necessary to be able to fly as fast as possible due to the mission time constraints and to have as much time as possible for investigation of the city in level 2. When the path has been flown, level 1 is complete. When level 1 is completed, the UAV will be placed near the city, hence, level 2 is initiated immediately after level 1 completion. In the level 2 flight, the marker needs to be identified, and an entrance larger than 1 × 1 m must be located. It has been chosen to investigate the city by a number of predefined waypoints, placed throughout the city, in which the helicopter will hover and take the pictures. The predefined picture waypoints are located in positions such that all the walls at which the marker or a house entrance could be placed is photographed by the UAV. Two of the advantages of waypoint-based investigation of the city are: 1) knowledge of which house is being photographed and 2) hereby knowledge of on which house the marker is 8
1.2. MISSION APPROACH
Level 1
Determine path from waypoints
Follow path
Level 2 Fly to picture waypoint
Take and analyze pictures
Marker found?
no
yes Identify house
Photograph walls
Identify entrances
Figure 1.2: Overview of the mission approach
found. In this project, it is assumed that hover forms the best conditions for taking pictures, i.e. it minimizes picture distortion and motion blur. An alternative approach for investigating the city could be continuous trajectory flight where the houses are photographed when the UAV is in motion. However, as the navigation strategy in waypointbased investigation complies well with navigation strategy needed for level 1, it is possible to use the same navigation system for both level 1 and level 2. Therefore the waypoint-based approach is found to be the best suited approach for completing level 2 in this project. All of the picture waypoints will be derived from a map of the city generated off-line before the competition. The helicopter then flies from waypoint to waypoint taking pictures of the houses and analyzing them in real-time in search of the 9
CHAPTER 1. MISSION OVERVIEW marker. When the marker is identified on a house the next step is to photograph each wall of this house in search of an entrances larger than 1×1 m. This search is performed in the same way as when searching for the marker, using the picture waypoints related to this specific house. From these pictures a suitable entrance is chosen, and this concludes level 2. The scope of this thesis is to make the UAV complete level 1 and 2. This choice has been made since it can be achieved with the use of only one autonomous vehicle. Level 3 is out of scope due to the limited amount of payload of the helicopter platform and completion of level 3 will also include development of a second autonomous vehicle that has to be deployed from the UAV. After having defined the approach to each level of the competition a problem formulation can be formed to specifically define the tasks involved in this project.
10
Problem Formulation
2
The scope of this thesis is to develop a fully functional UAV platform capable of completing level 1 and level 2 of the IARC06. Based on the mission approach described in the previous section the tasks to be completed by the group in this project can now be defined. Each task and its content are described in the list below. The first 5 tasks are based directly on the mission approach and are crucial to the completion of level 1 and level 2 of the IARC06. The last 2 tasks in the list are required in the implementation and test of the first 5 tasks and in the general use of the UAV platform. Real-time software environment for scheduling of in-flight software tasks. To solve this task a hard real-time multi thread software environment will be fully implemented on an on-board computer configuration, able to handle all real-time and non realtime software subsystems, running during flight. Navigation system for path planning and navigation during level 1 and level 2. The navigation system will be fully designed and implemented on the UAV, and capable of real-time planning of a path for level 1 based on any given waypoints, and perform a waypoint based level 2 flight. Map and Path Generation for optimal navigation during level 2 flight. An algorithm for generation of GPS coordinates for the waypoints of level 2 will be designed and implemented based on a relative map extracted from an aerial photo. Furthermore route optimization algorithms will be investigated and applied to ensure a near optimal flight path in level 2. Vision system for recognition of the marker and house entrances. For this purpose an on-board camera will be implemented on the UAV and a software subsystem will be designed and implemented able to communicate with the camera hardware and perform real-time analysis of the visual input during flight. UAV hardware platform for full autonomous flight. The hardware 11
CHAPTER 2. PROBLEM FORMULATION platform includes implementation of an on-board computer, sensors and actuators for navigation and control, hardware interface for interfacing of sensors and actuators to the on-board computer. Ground station for mission control and visual presentation of helicopter data in real-time. A software system will be designed and implemented on a computer on the ground capable of wireless in-flight communication with the UAV, visual real-time presentation of mission status and helicopter data, and data logging for later off-line processing flight data. Helicopter modifications to accommodate the needed space for the on-board electronics on the stock model helicopter. The modifications includes airframe modifications and rearrangement of components. The completion of level 1 and level 2 of the IARC06 is done in cooperation with group 1034A who are responsible for modelling and control in relation to the UAV development. Due to this all development in this thesis will be done under the assumption of a stable UAV platform able to fly according to any given reference input. The UAV developed in this project functions as a multiple purpose UAV platform at the Control Department at Aalborg University and it will be developed in cooperation with group 1034A, Associate Professor Anders la Cour-Harbo, and Ph.D. Student Morten Bisgaard.
12
Part II
Platform Design This part contains two chapters and concerns the structure of the hardware and software platform for this project. The first chapter describes the on-board electronics of the UAV and its implementation on the model helicopter. The second chapter defines all the necessary software subsystems and the communication between them.
13
Hardware Platform
3
For this project the hardware platform has partly been given in advance. A Bergen Industrial Twin helicopter has been acquired as the base of a multiple purpose UAV platform developed at the Control Department at Aalborg University. Due to this, the project is bounded by the capabilities and limitations of this specific helicopter. The intention is to mechanically modify the stock helicopter in order to fit sensors enabling the determination of position, attitude, rotational velocity, acceleration, and vision from the helicopter. Furthermore an OBC, and a hardware input-output interface for sensors and actuators, will be fitted to process the sensor data and control the helicopter.
3.1 Platform Description In this section the constructed hardware platform will be described. The platform has been developed in cooperation with Associate Professor Anders la Cour-Harbo and Ph.D. Student Morten Bisgaard. This section provides an overall description of the developed hardware design. The helicopter airframe used as the basic mechanical platform is a Bergen Industrial Twin which has the following specifications • Length: 1663.0 mm • Height: 558.8 mm • Weight: 8.165 kg • Main Rotor Diameter: 1940 mm • Tail Rotor Diameter: 340 mm • Payload: 11 kg approx. • Engine: Twin Cylinder Bergen/Zenoah - 52 cc 15
CHAPTER 3. HARDWARE PLATFORM
Figure 3.1: Stock Bergen Industrial Twin without modifications.
An overview of the different hardware parts and their interaction are shown in Figure 3.2. Each part will be described in the following section. Servos are the actuators of the system. The collective pitch servo, the pitch servo and the roll servo are of the type JR NES 4131 and the throttle servo is a JR NES 4721. These are all controlled by a 50 Hz PWM signal from Servo Board. The tail rotor servo controlling the yaw motion of the helicopter is a Futaba S9256 which is a digital high-speed servo controlled by Gyro Controller. This control signal is a 330 Hz PWM signal. All the servos are supplied with 6 V from Radio Receiver. When flying manually all the servo control-signals run directly from Radio Receiver through Servo Board to each servo with the exception of the signal to the throttle servo which runs directly from the governor, and the tail servo which runs to Gyro Controller. When flying autonomous the servo signals are all generated by Servo Board based on the servo positions received from Onboard Computer. Gyro Sensor and Gyro Controller are the GY611 from Futaba consist16
3.1. PLATFORM DESCRIPTION Servo Yaw
Gyro Controller
Gyro Sensor Radio Receiver
Servo Pitch Servo Roll
Servo Board
Servo Col.
Governor
Tachometer
Servo Throttle Wireless Network Onboard Computer Camera
GPS Antenna
GPS
Gyro / Accelerometer
Serial Interface
Compas
Figure 3.2: Overview of the hardware interaction.
ing of a yaw angular-velocity sensor and a controller that stabilizes the yaw angular velocity to 0 rad/s but still enables the pilot to give a yaw angle input to maneuver the helicopter. The GY611 is supplied with 6 V from Radio Receiver Radio Receiver is a Graupner JR smc 20DS which is a 10 channel PCM receiver. The receiver is controlled by a Graupner MX-22 transmitter and supplied with 6 V from a 4200 mAh NiMH accumulator. Onboard Computer is a DELL Latitude D400 which is a 1.8 GHz Pentium M with 512 MB DDR RAM. Onboard Computer is connected to Servo Board via a RS232 connection which is vital for control of the helicopter since this connection has some real-time demands in order to control the helicopter at a fixed frequency. The two blocks Serial Interface and Camera are USB and USB 2.0 connections respectively. WLAN is the Wireless Network 17
CHAPTER 3. HARDWARE PLATFORM connection, the build-in wireless adapter is used in this case. Serial Interface is an Edgeport/8r from Digi International which is a USB-to-serial converter converting one USB port to eight serial com ports which are necessary for interfacing GPS, Compass and Gyro/Accelerometer to Onboard Computer Gyro/Accelerometer is a Falcon MX consisting of three gyroscopes and accelerometers measuring angular rates and accelerations in three dimensions. Gyro/Accelerometer communicates with Serial Interface through a RS232 connection and is being supplied with 5 V from the PSU. Compass is a Honeywell HMR2300 which is a three-axis digital magnetometer measuring the helicopters attitude in three dimensions in relation to the earths magnetic field. Compass is supplied with 11.1 V from the PSU and connected to Serial Interface via a RS232 connection. GPS and GPS antenna is a Novatel OEM4-G2L GPS card with an active antenna. It has a 20 Hz refresh rate. The GPS is supplied with 3.3 V from the PSU and connected to Serial Interface through a RS232 connection. Governor is a Futaba GV-1 which controls the rotor RPM to a fixed value regardless of the drag of the rotor blades. Governor is connected to Tachometer which provides the rpm-feedback to the governor. Governor is also connected to Receiver in order for the pilot to be able to give an input. Camera is a UI-1410-M from Ueye which is a CMOS-based monochrome VGA-camera. Camera is connected to Onboard Computer via a USB2.0 connection. Servo Board is the interface between the onboard computer and the actuators of the system. Servo Board is based on a PIC18F458 from Microchip which is used to generate PWM signals to the servos based on the servo positions supplied via RS232 by the onboard computer. Servo Board is also used for sampling of the servo positions when flying manual. During manual flight the PWM signals from Radio Receiver passes through Servo Board to each servo. 18
3.2. HARDWARE IMPLEMENTATION Tachometer is a GGS-23 from Stator Gator which is a governor sensor that sends a puls to Governor and Servo Board each time the sparkplugs gives a spark. This is necessary in order to calculate the main-rotor RPM. As the on-board electronics has now been described, the following section will elaborate on the modifications made to the helicopter in order to fit the on-board electronics.
3.2 Hardware Implementation In order to implement the necessary electronics described in the previous section some modifications had to be made to the stock helicopter. In this section the modifications will be described and supported by pictures. A logical place for mounting the needed flight electronics is as close as possible to center of mass in order to affect the flying capabilities as little as possible. Therefore the flight electronics is often placed right under the rotor shaft between the landing gear. However, this was not possible on this UAV since this space was reserved for equipment in relation to another project where the UAV also has to be used. It was decided to mount the flight electronics in front of the rotor shaft instead. To make this possible some modification and rearrangement was necessary. As is shown in Figure 3.3 the gas tank and accumulator have been moved to the tail section in order to make space for the aluminum box and the laptop placed in the nose of the helicopter. The equipment placed in the tale also counter weights the extra mass added in the the nose to maintain the center of mass close to the rotor shaft, as intended on the stock Bergen helicopter. Also a cut-away in the helicopter carbon-fiber frame was made to make the necessary space. The aluminum box is mounted on two aluminum brackets which is mounted direct to the motor block. The aluminum box and the brackets have no direct contact but are connected through a set of rubber mounts to reduce vibrations on the electronics inside the box. The rubber mounts are shown in Figure 3.4 together with the mounting of the camera. In Figure 3.5 the on-board computer (OBC) is shown in the left side of the picture and the flight electronics inside the aluminum box in the 19
CHAPTER 3. HARDWARE PLATFORM
Figure 3.3: Modifications and rearrangement of stock helicopter. Stock helicopter in top of picture and modified helicopter in bottom of the picture.
Figure 3.4: Close up of aluminum brackets and rubber mountings for the aluminum box.
20
3.2. HARDWARE IMPLEMENTATION right side. The OBC is held in place by two velcro straps mounted on the sides of the aluminum box.
Figure 3.5: Top view of OBC and flight electronics inside the aluminum box
As shown in Figure 3.6 the compass and GPS antenna are both mounted on the tail and not together with the other electronics inside the aluminum box. This is done to avoid the main rotor to shield the GPS antenna causing limited satellite tracking ability, since the main rotor is made of carbon fiber which is a conducting material. The compass has to be placed as far away from other electronics as possible to avoid noise and bias on the sensor data caused by electromagnetic radiation.
Figure 3.6: Mounting of GPS antenna and compass on the tail
This concludes the description of the implementation of hardware and modifications made to the helicopter. The UAV is considered complete as a development-UAV hardware platform at this point. Due to involvement of the UAV in other projects, ongoing changes in the 21
CHAPTER 3. HARDWARE PLATFORM configuration of the helicopter will be made to improve the current implementation and in order to adapt the platform to other specific applications. Further changes to the set-up will not be covered in this thesis. During test flights the described set-up has proven to perform satisfying and has gained flight approval from RC-Unionen, Denmark.
22
Software Platform
4
In this chapter the software environment, under which the execution of the software subsystems takes place, will be chosen. Furthermore the purpose of the software subsystem and their real-time demands for them will be stated. Afterwards a description of the communication design between the software subsystems is elaborated and lastly the dataflow between the subsystems will be presented.
4.1 Software Environment As stated in the problem formulation on page 11 a real-time operating system is needed, under which real-time tasks can be executed. In this section the choice of the real-time operating system is made, and the installation is described. A Real-Time Application Interface, better know as RTAI, which is an extension for the common Linux Kernel, has been chosen as the real-time operating system. It is based on a regular Linux distribution with a modified kernel. The modified kernel enables real-time task execution, and is a regular Linux kernel with a RTAI patch applied enabling this execution. The advantage of using RTAI is that a hard real-time software environment is provided on a normal Linux development platform, which gives the flexibility of Linux during development. A Debian Linux 3.1 distribution has been chosen, and patched with RTAI version 3.2. In order to patch the kernel with RTAI a new kernel must be compiled, which is a complicated process and thus will not be mentioned here, but can be found in Appendix A.
4.2 Software Architecture In the previous section, it was chosen to implement the software under RTAI Linux. In this section, the software subsystems will be presented, and will be categorized as being either real-time or non real23
CHAPTER 4. SOFTWARE PLATFORM time tasks. Afterwards the communication design and a description of the dataflow between the software subsystems will be elaborated.
4.2.1 Software Subsytems and Real-Time Demands It has been chosen to divide the system into two layers. A software layer and a hardware layer. An overview of this division showing the software subsystems is given in Figure 4.1, where the hardware Mission Control
Map Information
Navigator
Flight Data Logger
Groundstation server
Vision
Software
Sensor Server
Controller
Sensors
Servoboard
Servos
Camera
Hardware
Figure 4.1: Overview of the subsystems, illustrating the high/low-level division.
layer corresponds to the description in chapter 3. In the following, the tasks of the software subsystems are briefly described and it is selected whether the tasks are defined to be real-time tasks or non real-time tasks. Sensor Server has the task of collecting data from the sensors, used by some of the other subsystems. This subsystem is developed solely by Morten Bisgaard and the development will not be dealt with in this thesis, but still it is taken into account when designing the system architecture, as it has to be placed in the overall architecture. It is not necessary to implement the sensor server in real-time even though it acquires the sensor data, since the sensor data is not requested by the OBC, but is send with a periodic frequency from the respective sensor to the OBC. Since the sensors does not operate with the same frequencies the result is a asynchronously sensor data flow into the OBC. For a non realtime task the ability to read from the sensors using a software 24
4.2. SOFTWARE ARCHITECTURE interrupt is available, therefore it has been chosen to define the sensor server as a non real-time subsystem. Controller provides a stabilizing control of the helicopter. It is a hover controller which also can be used for forward flight up to 3 m/s. Furthermore, the Controller provides an interface for the navigation carried out by this group and it feeds the flight data to the flight data logger. The environment in which the Controller is implemented, together with the interfaces, are developed by this group, but the development and implementation of the control algorithm, the sensor fusion, and the state observer are carried out by group 1034A. The controller needs to be able to execute the control algorithm with a fixed frequency in order to guarantee stability, which is why the controller is defined to be a real-time subsystem. Since only the controller environment will be developed by this group it has been chosen not to deal with the design in this thesis. Still it has been implemented during the development of the other subsystems as it is needed in the overall architecture. The implementation of the controller is shown in Appendix C.3. Map Information is a subsystem with the task of providing information about level 1 and level 2. It contains the waypoints used for level 1 flight, and for level 2 it contains the waypoints where the helicopter must stop to search for the marker and for a house entrance. Furthermore it contains the coordinates of the buildings in the city. Since the only task of the map information is to provide the map data for the other subsystems it is defined to be a non real-time subsystem. Navigator consists of tasks divided into two levels. In level 1 the navigator must navigate the helicopter through the waypoints defined by the level 1 waypoints in the map information. In level 2 the navigator must navigate the helicopter to a predefined set of waypoints defined by the level 2 waypoints in the map information to search for the marker. When the marker has been found, the house is identified and the navigator investigates the house further in search for the best suited entrance. The navigator uses the controller to move the helicopter by sending periodic references. To ensure that the execution frequency is constant for the navigator it has been chosen to define the navigator as a realtime subsystem. 25
CHAPTER 4. SOFTWARE PLATFORM Flight Data Logger has the task of logging the flight data during flight. This is necessary in order to be able to analyze the flight data off-line. In order to log the flight data it must be stored to a file which preferable was chosen to be readable by M ATLAB, and with a structure that corresponds to the sensor data was chosen. As it is not possible to write data to files from real-time tasks, since a data write to a file is a non real-time operation, it is required that the flight data logger is a non real-time task. Ground-station Server has the tasks of transmitting the flight data to a ground-station PC, which displays the flight data during flight using a ground-station client. The flight data received on the ground-station PC is also logged in order to have the data in case of a crash resulting in the OBC is destroyed. The ground-station server does not require real-time execution since the display of data in real-time on the ground-station client is not required, and thus the ground-station server is defined to be a non real-time subsystem. Vision has the task is to search the pictures acquired by the camera to determine the likelihood of the marker being present within the picture. Furthermore it is also able to detect if a house entrance is present within the picture. When the marker or house entrance are detected, the vision subsystem notifies the mission control which decides the action to be taken. The vision subsystem is developed by this group but the detection algorithms are developed by Anders la Cour-Harbo. The driver provided along with the camera used for acquiring images is a precompiled driver which limits the usage of the driver to non real-time only, and hence the vision subsystem is defined to be a non realtime subsystem. Mission Control manages the execution of the mission parts and is responsible for accomplishing the central helicopter missions. It is expandable, if other systems, such as a collision-avoidance system, is needed. If such systems are implemented the mission control needs to be implemented in real-time, and hence the mission control is defined as being a real-time subsystem. In summary, this leads to having defined Controller, Navigator and Mission Control as the three real-time tasks that are present within the system, while the other subsystems are defined as non real-time tasks. 26
4.2. SOFTWARE ARCHITECTURE
4.2.2 Software Structure and Communication Design After defining which tasks require real-time execution and which does not, each of the subsystems can be placed in an overall structure. The structural diagram has been modified during the development process of the different subsystems and resulted in the diagram shown in Figure 4.2, which also shows the communication between the different tasks and the interaction with the hardware layer. One of the Hardware
Non Real-Time
Real-Time
Hardware
Sensors
Actuator positions
Sensor Server
WLAN
Gound Station Server
Shared Memory
Controller
Actuators
M.Box
Flight Data Logger
FIFO
Navigator Map Information M.Box
Camera
Vision
M.Box
Mission Control
Figure 4.2: Software structural and communication diagram.
most central blocks within the structural diagram shown in Figure 4.2 is the shared-memory block. The shared-memory block enables the software subsystems to acquire or store data to be exchanged with the other subsystems. This memory space is unbuffered which means that only the latest value stored are available. The structure of the memory space has been chosen as shown in Table 4.1. In Table 4.1, the variables Locked, Sensorsvalid and Samplenumber are variables while the elements Gyro, Compass, GPS, Servo and Tacho are data structures containing a set of variables corresponding to those the sensors read. The data structure Navigator contains information used to follow the status of 27
CHAPTER 4. SOFTWARE PLATFORM Element locked SensorsValid samplenumber Gyro Compass GPS Servo Tacho System
Estimator Controller Navigator Map Information
Description Semaphore used for signaling a write to the shared memory Flag that signals that the sensor data is valid Samplenumber for the current data Gyroscope data Compass data GPS data Servo positions Engine tachometer System information data (WLAN strength, battery level) Estimated state values Controller output data Information about the state of the navigator Struct containing map information data
Table 4.1: Shared-memory structure.
the navigator when sent back to the ground station, and is also used for writing information for debugging during development. The data struct Map Information contains all the information known about the map of the city and is used for level 2 flight. The elements of the mentioned data structures are shown in more detail in Appendix C.7. The appendix contains the implementation of a platform definitions header file which, among others, contains the definitions of the data structures. For enabling interprocess communication (IPC) between the software subsystems in Figure 4.2 a commonly known IPC type has been used called mailboxes. The mailboxes are indicated by M.Box in Figure 4.2, and are basically a shared memory area as the one previously described. In the box M.Box in Figure 4.2 a mailbox exists for each direction the communication travels, but they are shown as one box for the sake of simplicity. Through the mailboxes the tasks are able to communicate by synchronizing through a variable and exchanging one or more variables located within the mailbox. In Table 4.2 an example is shown of the structure the mailboxes use. Besides the mailboxes, a FIFO buffer has been used between the controller and flight data logger as a buffer for the data passed continuously by the controller to the flight data logger, since the flight data logger is a non 28
4.2. SOFTWARE ARCHITECTURE Element new message message
Description Synchronization byte indicating a new message is valid within the mailbox Variable used for containing the message Table 4.2: Shared memory structure.
real-time task. The size of the FIFO is defined as the amount of data it can store, and this size must be dimensioned. It is assumed that the size of the FIFO buffer suits its needs if it is able to store flight data from the last five seconds. When logging with a frequency of 50 Hz the size in bytes is found to 250 times the size of the shared memory area, which will ensure 5 seconds of buffer time for the flight data logger. This concludes the structural and communication design of the software. In the next section the dataflow between the different subsystems will be elaborated.
4.2.3 Dataflow Between Software Subsystems In this section the dataflow between the subsystems is described to provide the reader an idea of how the software subsystems interact with each other at low level. The sensor server, controller, flight data logger and ground-station server has a continuous loop in which sensor data is passed. This loop will be described in the following. Initially the sensor server, which is connected to the sensors and actuator-position sensors in the hardware layer, reads the data from the sensors. If the helicopter is operating in manual mode the actuatorposition sensors are read, and if operating autonomously the actuator signals are generated and thus the actuator position-sensors are not read. When the data has been read the sensor sever toggles locked in shared memory and writes the data to the fields previously described, and toggles locked again. In parallel with the sensor server, writing sensor and actuator information to the shared memory block, the controller reads from the shared memory. Before reading the shared memory, the controller checks whether locked is set, and if it is not the controller reads the data. When the controller has read the data a copy of it is stored in the FIFO buffer from which the flight data logger reads and stores the data in a file. In parallel with the flight data logger the ground-station 29
CHAPTER 4. SOFTWARE PLATFORM server reads the data available within the shared memory if locked is not set, and transmits this data back to the ground-station client running on the ground-station PC. Besides the continuous loop just explained, the navigator, mission control, vision and map information runs in parallel, but these subsystems are decoupled from the sensor data passing process. There are no data flow between these subsystems, only communication between the subsystems is passed. However, the navigator uses the GPS positions and map information data located in the shared memory, and thus it must be connected to this. The purpose of the map information subsystem is to load data, stored within a file, into the shared memory when the Map Information is initialized. After having defined the software platform, the real-time demands for the software subsystems, and the communication between them, can be developed separately.
30
Part III
Mission Applications This part contains 7 chapters and concerns the development of all applications, necessary for this project, individually. The first four chapters in this part describes the development of the software subsystems running on the OBC. This includes the navigation system, vision system, mission control, and flight data logger. The fifth chapter describes the development of the ground-station software. In the two final chapters the off-line map and path generation are covered.
31
Navigator
5
To make the helicopter fly through a set of coordinates, a navigator is needed. In this chapter, the analysis and design of the navigator is described. After the design process, the navigator is implemented in ANSI C in such way that the same C file is used in simulation in M ATLAB and on the final implementation on the helicopter. This way, if a change is made in simulation it is also included in the navigator implemented on the helicopter. The simulation and test of the navigator is also documented in this chapter. Lastly, the navigator will be tuned to minimize the flight time.
5.1 Analysis of Navigator As stated in Chapter 1, the purpose of this project is to carry out level 1 and 2 of the IARC06. In level 1, the purpose is to fly through all the waypoints as fast as possible. In level 2 the helicopter is supposed to stop and hover in each waypoint, facing a predetermined direction, to take pictures of a specific wall to find the marker. The house on which the marker is found is then examined in the same way for an entrance. To accomplish this, it is necessary to be able to navigate the helicopter between waypoints. Furthermore, for level 2, it is necessary to be able to set a specific heading of the helicopter, to control what the on-board camera is taking pictures of. Hence, a navigator is designed to calculate velocity and yaw references and send them to the controller to make the helicopter behave as desired. Firstly some variables, which are used in the following sections, are defined. Figure 5.1 illustrates some of the variables, associated with the helicopter navigation, which is described as WPN The waypoint the helicopter is flying towards, WPN+1 is the waypoint the helicopter will fly towards when having reached WPN . VC The vector describing the desired course, that is the vector between W PN−1 and W PN 33
CHAPTER 5. NAVIGATOR
Helicopter Vp
Vn
Vo
Vc
WPN-1
WPN
Figure 5.1: Illustration of the helicopter between two waypoints and the variables associated with this.
VN The vector from the helicopter to W PN VP The vector from W PN−1 to the helicopter VO The velocity reference vector, that is the output from the navigator to the controller Vcross The cross-product vector of VC and VN
5.1.1 Path Computation The input to the navigator is a set of waypoints which consists of a position given in absolute coordinates. Hence, an earth fixed reference frame (EF) is defined as having the y-axis pointing in northern direction and the x-axis pointing in eastern direction. In the EF, angles are defined from the y-axis and is positive counter clockwise as illustrated in figure 5.2.
N θ
E
Figure 5.2: Definition of the EF and angles in this frame.
34
5.1. ANALYSIS OF NAVIGATOR The navigator needs to be generic in the way that it has to work with an undefined number of waypoints and with any given controller. Hence, the navigator must be prepared to handle different velocities, different accuracy etc.. This requires a variable hysteresis around each waypoint to support the varying accuracies. Therefore, a hysteresis circle, called waypoint circle, is placed around every waypoint, and when the helicopter enters this waypoint circle it is considered as having reached the waypoint. The waypoint circle might not have the same radius in level 1 and 2 as the two levels sets different demands. Before navigation is possible a path must be computed. This path must combine all the waypoints, such that the helicopter will enter each waypoint circle. This could for instance be done by deriving a mathematical function which runs through all of the waypoints. However as this approach could become very complex as the order of the function is most likely to increase with the number of waypoints, a more simple approach is considered. Another approach could be to simply combine the waypoints with vectors between them. This approach has the advantage of being quite simple to implement, but lacks the ability to avoid a known obstacle without placing an extra waypoint. This is not a problem in this project as the helicopter can be set to fly at higher altitude than the altitude of the tallest obstacle. This approach is chosen as it is easy to implement and is adequate for completing level 1 and 2. The chosen approach for deriving a path between the waypoints can be combined with different ways of flying through the waypoints, and some of these ways are illustrated in Figures 5.3(a)-5.3(c).
(a) Example 1
(b) Example 2
(c) Example 3
Figure 5.3: Examples of path connections inside the waypoint circle.
Figure 5.3(a) shows the least complicated way of connecting the waypoints. Here the helicopter flies towards WPN and comes to a halt. When the helicopter has stopped, it yaws until the nose of the he35
CHAPTER 5. NAVIGATOR licopter points in the direction of WPN+1 , then it begin to fly towards this waypoint. This reduces the path-following complexity to simple forward flight. This solution is, obviously, not the fastest way to fly a route, but it can be extended to be more efficient. Figure 5.3(b) shows a scenario where the helicopter flies towards WPN and when it enters the waypoint circle it immediately receives a new reference pointing towards WPN+1 and flies towards this waypoint without stopping. This will cause the flight path of the helicopter to become smooth inside the waypoint circle. It could also be a possibility not to steer directly towards the waypoint, but towards a point on the associated waypoint circle and then follow a specific curve through the circle, such that the helicopter would overfly the waypoint more accurately. This is illustrated in Figure 5.3(c). As the helicopter is considered as being inside the waypoint when entering the waypoint circle, the waypoint connection method illustrated in figure 5.3(b) is chosen. As the helicopter will get a new reference when entering the waypoint circle, it will most likely have to make a turn. To perform this turn it might be necessary to lower the helicopter speed. This means, that the helicopter velocity reference must be decreased before the helicopter enters the waypoint circle. As the navigator has information of the path, whereas the controller has not, the navigator should consider this problem. Hence, a circle with a larger radius than the waypoint circle is placed around all the waypoints to define when the helicopter speed must be decreased, this circle is called the velocityreference circle. This means, that when the helicopter approaches a waypoint, it will fly as illustrated in Figure 5.4. The figure illustrates that the speed of the helicopter is decreased when it enters the velocity reference circle. When the helicopter enters the waypoint circle, it is considered as being in the waypoint, and a new reference pointing towards the next waypoint is used. The helicopter will then turn while flying with the slow speed, and when it leaves the velocity reference circle the speed will be increased again to make the helicopter reach the next waypoint as fast as possible.
5.1.2 Deviations Another consideration which needs to be made regarding the following of a path is how to handle a potential deviation from the course. This deviation could be caused by many things, wind, model error, controller error etc.. 36
5.1. ANALYSIS OF NAVIGATOR Velocity Reference Circle Waypoint Circle
Increase speed New reference
Decrease speed
Figure 5.4: Illustration of how the helicopter acts when approaching a waypoint.
If it does not matter whether the helicopter deviates or not, the navigator could simply use the vector from the current position of the helicopter to WPN , which is the vector Vn , as reference for the velocity and heading. If this method is used, the path between the waypoints is completely neglected and the only thing the navigator considers is the waypoints. This could potentially cause the helicopter to deviate from Vc , but the helicopter will eventually reach the waypoint. In some applications such a deviation does not matter, but in the case of controlling a helicopter, it is very important to minimize deviations from the planned path. If the helicopter deviates to much from Vc in the vertical or horizontal directions, it might collide with the ground or any obstacles present and crash. Hence, it is necessary to steer the helicopter not just towards WPN , but also back on the initial course Vc , if the helicopter deviates from it. This gives the ability to follow a course between two waypoints instead of just flying randomly between them.
5.1.3 Navigation Method To accomplish the ability of following a course, a navigation method must be chosen. The common methods used for navigation are based on control of marine vehicles, as autopilots were first developed to relieve the pilots of smaller boats on routine cruising [Fossen 2002, p. 5]. The desire regarding the deviation, when autonomously navigating a marine vehicle, is to make the vehicle approach the original course in the same way as the helmsman of the vehicle would have done. That is, if the deviation, which is called the cross-track error, ect , gets larger, the navigation has to steer more after the course than 37
CHAPTER 5. NAVIGATOR the waypoint. This should ensure that the vehicle will have a smooth path back towards the course. This is illustrated in Figure 5.5. The
Vehicle Desired approach Deviation Course Figure 5.5: Illustration of how a deviating vehicle should approach the course.
navigation method which is commonly used to ensure the approach illustrated in Figure 5.5, is line-of-sight (LOS) navigation [Fossen 2002, p. 168]. Basically the LOS navigation method works by placing a circle with a suitable radius around the vehicle. The vector from the vehicle to the intersection between the course and the circle around the vehicle is then used to calculate the heading and velocity reference of the vehicle, as illustrated in Figure 5.6. The LOS navigation method
dlos
Vo Vc Figure 5.6: Illustration of how the line-of-sight navigation method works.
is developed and intended for marine vehicles, but for navigating a model helicopter it has some disadvantages. If ect gets larger than the LOS-distance, then the navigation does not produce an output as no intersection of the circle and Vc is present, and if the circle is made larger the helicopter is navigated slower towards Vc . Furthermore it is not possible to change how the angle between Vc and Vo develops as a function of ect . In LOS navigation, the angle develops as ect π (5.1) Θ = − arccos 2 dlos 38
5.1. ANALYSIS OF NAVIGATOR which is illustrated in Figure 5.7. It would be preferable to be able to Θ
e ct
Figure 5.7: Illustration of how the angle between Vc and Vo evolves as a function of ect when using line-of-sight navigation.
adjust how the angle develops as a function of ect , to make the navigator as independent of the underlying controller as possible. Where one controller might be able to use the rate of angle change LOS provides, this rate might make another controller become unstable. Instead of using a circle to determine the direction of Vo , it could be advantageous to calculate Vo by rotating VN . By using VN as basis for calculating Vo , the actual heading towards WPN is always taken into account. In this way of navigating, VN is rotated by an angle, Θhelm , which is a function of ect , to produce Vo . By designing this function properly the vehicle will follow the desired approach illustrated in Figure 5.5. The actual function used in this project will be designed in section 5.2. As this is the approach a helmsman would use, this navigation method will be called helmsman navigation. If the vehicle does not deviate from the course, the helmsman navigation will simply use VN as heading. If a deviation occurs, the helmsman navigation will rotate VN about the cross product of Vc and VN , Vcross . The reason why VN is rotated around the cross product, is that the correction needs to be done in all three dimensions, as the helicopter can deviate in three dimensions. If ect gets larger, Θhelm increases and VN is rotated further. This is illustrated in Figure 5.8. To conclude, it is chosen to create a path from the waypoints by placing vectors between them. It is also chosen to use the connection shown in figure 5.3(b) where the helicopter flies towards WPN until it enters the waypoint circle, then it gets WPN+1 as reference. To make sure that the helicopter speed is slow enough for it to be able to turn within the waypoint circle, it is chosen to place a velocity-reference circle outside the waypoint circle. Lastly it is chosen that the heli39
CHAPTER 5. NAVIGATOR Course Flight direction Helicopter
e ct
Figure 5.8: Illustration of how the algorithms steer the helicopter towards the desired course, as a function of the deviation from it ect .
copter must minimize the deviation by use of helmsman navigation and thereby follow the vector between two waypoints instead of just flying randomly between them. The design of a navigator able to perform these choices is described in the following.
5.2 Design of Navigator As the helicopter motion is different in level 1 and 2, the navigator must be able to steer the helicopter in both ways. The difference between the two ways is the action taken when the helicopter enters a waypoint. In level 1, it is only necessary to define the waypoints as a set of coordinates in three dimensions. But as a heading and a house number is needed in level 2, these two entries are added to the list of waypoints. Hence, they do not only consist of coordinates but a yaw angle and a house number are also attached. The yaw angle defines which way the helicopter must point when taking pictures of the houses, and when the marker is found, the house number is returned to the mission con40
5.2. DESIGN OF NAVIGATOR trol. The list of waypoints is therefore constructed as x1 y1 z1 yaw1 house_number1 x2 y2 z2 yaw2 house_number2 .. . . xn yn zn yawn house_numbern
(5.2)
In Figure 5.9 an example of a path made from three waypoints is illustrated. Making this path is one of the tasks for the navigator, and additionally the navigator must also steer the helicopter along the path, and make sure it does not deviate from it. As illustrated in Figure 5.9, Velocity-reference circle
Waypoint circle
Course
Figure 5.9: Example of a path with three waypoints.
two circles are placed around each of the waypoints, the waypoint circle and the velocity-reference circle. When the helicopter enters the velocity-reference circle, the velocity reference will decrease. Then, when it enters the waypoint circle, it is considered as having reached the waypoint, and the next waypoint is given as reference. The circles might not have the same radius in level 1 and level 2. To simplify the design of the navigator, it is divided into two parts, a path planner and a path follower. The purpose of the path planner is to receive a message from the mission control, described in chapter 7, with a set of waypoints and activate the path follower with the two waypoints W PN−1 and W PN . From these two waypoints, the path follower calculates a velocity-reference vector for the helicopter, and depending on the deviation of the helicopter, this velocity reference is altered to make sure the helicopter flies towards the desired course. The communication in the navigator is illustrated in Figure 5.10. The messages from the mission control consists of a command to the navigator and a vision result. Before the navigator receives the first message from the mission control, it is set to hover. In Figure 5.11, the 41
CHAPTER 5. NAVIGATOR
Navigator
Path Planner
Path Follower
Waypoint list
Loop
Previous and next waypoint
Velocity reference
Velocity and yaw references and house number
Figure 5.10: Sequence diagram of the navigator.
flow chart of the navigator is presented. And as it shows, the first thing the navigator does, is to check if a new message has been received from the mission control. If no new message is present, the navigator uses the previous command. This command could be either INIT 1, INIT 2, LEVEL 1 or LEVEL 2. An init command is sent to ready the navigator for flying level 1 or level 2. The level commands is then send to execute a flight of level 1 or level 2. If a new message is present, the navigator reads the contents of the message and stores the command and vision result, which are included in the message. If the command stored is an init command, the navigator checks if the initialization has already been done. This is necessary as the navigator is a loop and the command might not be updated between two loops. If the initialization has already been performed once, the navigator stops and waits until the scheduler starts it again. If the initialization has not been performed, then the navigator adds the current position of the helicopter as the first coordinate in the waypoint list. This is to make the navigator generic in the sense that it can be started in a random position and seek the first waypoint from this location. When this is done, the navigator informs the mission control that the initialization has been performed, and that the navigator is ready to fly the level. If the command stored is a level command instead of an init command, the navigator calls the path planner, which will be described later. The path planner returns several values, velocity and yaw references, a house number and a path planner state. The returned references are sent to the controller, and then the navigator checks the path 42
5.2. DESIGN OF NAVIGATOR
New message
No
Yes
Read message from Mission Control
Command
Level x
Init x
(Command, Vision Result)
Yes
Initialization executed once
Delay
Call Path Planner
No Add current position in level x waypoint list
Send calculated references to Controller
Initialize level x
Send ”init x ok” to Mission Control
Reset ”sent”variable
Ongoing
Path Planner state
Inside waypoint
Level_1_done Level_2_done_not_found Level_2_done_found
Reset ”sent”variable
Set ”sent”-variable
No
”sent”-variable set
Yes Path Planner state
Level_1_done Level_2_done_not_found
Send state of Navigator to Mission Control
Level_2_done_found Send state of Navigator and House number to Mission Control
Send state of Navigator to Mission Control
Figure 5.11: Flow chart of the entire navigator.
43
CHAPTER 5. NAVIGATOR planner state. It is based on this state that the navigator determines if and what to tell the mission control. The different path planner states are shown in table 5.1 Path Planner State ONGOING LEVEL_1_DONE LEVEL_2_INSIDE_WAY-POINT LEVEL_2_DONE_FOUND LEVEL_2_DONE_NOT_FOUND
Description Helicopter between waypoints Level 1 is flown Helicopter inside level 2 waypoint Level 2 done - return house number Level 2 flown
Table 5.1: Description of the path planner states.
When the helicopter enters a waypoint circle in level 2, the mission control must be informed of it. Hence, if the path planner state is LEVEL_2_INSIDE_WAYPOINT, then the navigator sends a message informing the mission control that the helicopter has entered a waypoint unless it has already been sent. If the path planner state is either LEVEL_1_DONE or LEVEL_2_DONE_NOT_FOUND, the navigator sends a message to the mission control that the level is flown and no more waypoints are present. The same is done if the path planner state is LEVEL_2_DONE_FOUND, then a house number is attached to the message telling the mission control on which house the marker has been found. In the following sections, the path planner and path follower will be designed. The design of the path planner is divided into two parts, one for level 1 and one for level 2. This is not the case for the path follower as it can be designed generic such that it can be used in both level 1 and 2.
5.2.1 Path Planner Depending on which level the helicopter is attempting to fly, there is a difference in the procedure of the path. Hence, in the following sections, two instances of the path planner are described, one for each level. Path Planner During Level 1 Flight In Figure 5.12 the flow chart describing the procedure of the path planner during level 1 flight is presented. Firstly, the path planner state is set to ONGOING. This state is only changed if the last waypoint in 44
5.2. DESIGN OF NAVIGATOR
Path planner state = PP_ONGOING
Start path-follower with waypoints
V_out from pathfollower
Calculate yaw angle from V_out
Helicopter inside waypoint circle
No
Yes
Last waypoint?
Hover Yes
No Path planner state = PP_LEVEL_1_DONE
Next waypoint
Figure 5.12: Flow chart of the path planner during level 1 flight.
level 1 is reached. Then the path-follower algorithm is called with two waypoints as inputs, WPN−1 and WPN . The path follower returns a velocity-reference vector Vo to the path planner which uses it to calculate a yaw-angle reference. The yaw angle reference is defined as being 0 rad when pointing in northern direction and rising counterclockwise. The length of the vector, consisting of the two first elements of Vo , that is the velocity reference in the lateral and longitudinal direction, is calculated. This vector is illustrated in Figure 5.13. The yaw reference can now be calculated by use of Pythagoras as yaw = arccos
∆x ∆h
−
π 2
(5.3)
There is a potential problem with this way of calculating the yaw reference. It does not matter if ∆y is positive or negative, (5.3) will yield the same result in both cases as it does not depend on ∆y. Hence, if 45
CHAPTER 5. NAVIGATOR
∆x
Vo(2)
∆y ∆h
yaw
Vo (1) Figure 5.13: Illustration used in calculation of the yaw reference.
∆y is negative, the yaw reference is calculated as ∆x π yaw = − arccos − ∆h 2
(5.4)
The yaw references calculated in (5.3) and (5.4) is an absolute value π between −3π 2 rad and 2 rad. This value needs to be an unbounded value between −∞ rad and ∞ rad as the controller does not support π the fact that −3π 2 rad and 2 rad are the same angle. If for instance the previous yaw reference were 350◦ and the next is 5◦ , this would make the controller try to force the large number down to the smaller one, hence rotating clockwise 345◦ instead of rotating counter-clockwise 15◦ . That is, if the yaw reference change becomes larger than ±π rad, the controller will rotate the helicopter the wrong way. Hence, a function is made to make the yaw reference unbounded. The path planner checks the distance from the helicopter to the next waypoint to examine whether or not the helicopter has reached a waypoint circle. If it has not reached the waypoint circle, the algorithm will call the path follower with the same two waypoints again. If it has reached the waypoint circle, the helicopter is considered as being in the waypoint and the path follower is called with the next set of waypoints. This loop continues until the last waypoint is reached. When it has been reached, the path planner will return a message to the mission control telling it that the waypoints has been flown. Path Planner During Level 2 Flight The first part of the path planner is identical in level 1 and 2. This is the case until Vo is returned by the path follower, whereafter there are some differences. In Figure 5.14 a flow diagram of the path planner 46
5.2. DESIGN OF NAVIGATOR
Path planner state = PP_ONGOING Start path-follower with waypoints V_out from pathfollower
Distance between Long waypoints
Short
Variable ”heli_inside_wp” = 1?
No
Calculate yaw from V_out
Yes
Fetch yaw from waypoint list
No
Hover=0
Helicopter inside waypoint circle? Yes
Fetch yaw from waypoint list
Path planner state = PP_LEVEL_2_INSIDE_WP
Hover=1
Is message from mission control valid?
No
Set variable ”heli_inside_wp”=1
Yes Set variable ”heli_inside_wp”=0
Path planner state = PP_LEVEL_2_DONE_FOUND
Found
Check value of message
Not found
Last waypoint?
No
Next waypoint
Yes Hover=0
Path planner state = PP_LEVEL_2_DONE_NOT_FOUND
Path planner state = PP_ONGOING
Figure 5.14: Flow chart of the path planner during level 2 flight.
47
CHAPTER 5. NAVIGATOR during level 2 flight is given. In level 1, the yaw reference is always pointing in the flight direction, during level 2 flight the decision regarding the yaw angle is somewhat more complex. It will probably be necessary for the helicopter to photograph a long wall and hence necessary for it to take several picture while pointing in almost the same direction. In this case it would make little sense to fly after the nose for a short distance and then rotate the helicopter back in the same direction again. Even so, it would still be preferable to be able to fly after the nose if the helicopter has to fly from one side of the town to the other. Hence, if the distance between two waypoints is larger than 2 times the radius of the velocity-reference circle, the helicopter will fly after the nose. If the distance between WPN−1 and WPN is shorter, then the yaw-angle value attached to WPN will be used while flying. This way the helicopter is ready to take pictures the instance it enters the waypoint circle, and thus it does not have to yaw first. If the helicopter is outside a waypoint, it can either be because it is flying between two waypoints or it could be because it has drifted outside the waypoint it was supposed to be located in. It is very important for the helicopter to be located inside the waypoint while the pictures are being taken, else the algorithm does not know what the helicopter is taking pictures of. Hence, a variable HELI_INSIDE_WP is set whenever the helicopter enters a waypoint and reset when it is supposed to leave the waypoint again. That is why, in the top of Figure 5.14, when asking for the distance between the waypoints, it is checked if the helicopter is supposed to be inside a waypoint. If it is, then the yaw reference must be set to the yaw value attached to the waypoint the helicopter should be in, if it is not, then the helicopter is flying between two waypoints which are far apart, and the yaw must be calculated by use of (5.3) and (5.4). Then it is checked whether or not the helicopter is inside a waypoint circle, and if not, the hover variable is reset. This is because of the drifting problem. In case the helicopter drifts outside the waypoint circle, it must fly back inside it. If the helicopter is inside the waypoint circle, the yaw reference is set to the value attached to the waypoint, and the hover variable is set. The helicopter must hover until the mission control tells the navigator that the marker is found or not found. Hence, the path planner examines if a message from the mission control is valid. If it is not, the variable HELI_INSIDE_WP is set to indicate that the helicopter is supposed to be inside a way48
5.2. DESIGN OF NAVIGATOR point. If a valid message is present, it is examined whether the message is FOUND or NOT_FOUND. If the message equals FOUND, then the path planner sets the state to LEVEL_2_DONE_FOUND. When the path planner enters this state, the navigator will send the house number of the house the helicopter was photographing to the mission control. If the message equals NOT_FOUND, it is checked if the current waypoint is the last. If it is not the last, then the helicopter will fly towards the next waypoint and hence set the state to ONGOING. If it is the last waypoint, The path planner state is set to LEVEL_2_DONE_NOT_FOUND. When this state is entered, the navigator tells the mission control that all the waypoints have been flown.
5.2.2 Path Follower The purpose of the path follower is to receive input from the path planner in form of two waypoints, and then calculate a helicopter velocity reference. This reference is of course dependent on the course between the two waypoints, but it is also dependent on how much the helicopter deviates from this course. In the following, the path follower will be described based on the flow chart in Figure 5.15. The first task for the path follower is to check if the hover variable is set. It can be set from the mission control or it can be set by the navigator either if the last waypoint is reached in level 1 or if any waypoint is reached in level 2. If this variable is set, the algorithm sets the velocity reference, Vo , to zeros and then return it to the path planner. If the hover variable is not set, the path follower calculates the cross-track error ect . ect is defined as being the perpendicular distance from the helicopter to the course vector Vc , as illustrated in Figure 5.16. As both Vc and Vp are known, ect can be calculated by use of the cross product of the two vectors as ect =
|Vc × Vp | |Vc |
(5.5)
Next, the vector from the current position of the helicopter to the next waypoint, Vn , is calculated. To be able to control the speed of the helicopter, Vn , is transformed into being a unit vector Vnunit . If the helicopter deviates from Vc , Vn must be rotated, to determine vo , in such way that the helicopter will fly towards the desired 49
CHAPTER 5. NAVIGATOR
Waypoints from path planner
Yes V_out=[0 0 0]
Is hover=1
No Calculate cross-track error (e_ct)
Calculate V_n
Calculate V_cross (V_n x V_c)
Using e_ct, rotate V_n around V_cross to calculate V_out_unit
Helicopter inside velocity-reference circle
Yes
Multiply V_out_unit with turn-velocity factor to calculate V_out
No Multiply V_out_unit with cruise-velocity factor to calculate V_out
Return V_out to path planner
Figure 5.15: Flow chart of the path follower.
50
5.2. DESIGN OF NAVIGATOR Helicopter Vp
WPL
e ct Vc
WPN
Figure 5.16: Illustration to be used in calculating ect .
course. This is done using helmsman navigation. As described in section 5.1, a function describing the angle Θhelm must be chosen, and it would be preferable if it is possible to alter the chosen function in both amplitude and evolution. The function must increase Θhelm the larger the deviation is, but it needs to have a definable limitation. This is to secure that the angle which Vn is rotated, is not to large. As an example it makes no difference if Vn is rotated with 2π rad. A function which has these features are the hyperbolic tangent function. By inserting two parameters, X and Y , it is possible to control both how large the maximum amplitude can get as well as how fast the function evolves to this maximum. Hence, Θhelm is calculated as Θhelm = X · tanh (Y · ect ) .
(5.6)
A plot of the function in (5.6) is illustrated in Figure 5.17. To be sure F( e ct )
Θ helm
ect
Figure 5.17: Illustration of the function − tanh (ect ).
the rotation makes the helicopter fly towards the course in all three dimensions, the rotation vector should be perpendicular to both Vn and Vc . Hence, the cross product of these is calculated, this cross product is called Vcross . To rotate Vn about Vcross , Euler’s eigenaxis rotation is used. This function can rotate a vector or reference frame around another vector. 51
CHAPTER 5. NAVIGATOR The eigenaxis rotation matrix, C, is described by [Bak 2002, p. 19] as T C(Vcross , Θhelm ) = cos(Θhelm )I + (1 − cos(Θhelm ))Vcross Vcross
− sin(Θhelm )E, where E is described as 0 −Vcross (3) Vcross (2) 0 −Vcross (1) . E = Vcross (3) −Vcross (2) Vcross (1) 0
(5.7)
(5.8)
Vn is now rotated Θhelm radians around Vcross , and the result is the unit vector Voutunit . The rotation is performed by Voutunit = C(Vcross , Θhelm ) · Vn .
(5.9)
When this is done, it is checked whether or not the helicopter is inside a velocity-reference circle. This information is used to determine the size of the velocity reference Vo . If it is inside a velocity-reference circle, Voutunit is multiplied with a factor describing the speed during turns, and if it is outside the velocity-reference circles, Voutunit is multiplied with a factor describing the speed during cruise flight. The velocity-reference vector, Vo , is now returned to the path planner and the path follower terminates.
5.3 Simulation of Navigator To verify that the navigator works, it has been simulated using M ATLAB and S IMULINK . These simulations are made to show that the navigator qualitatively works as intended. Simulations of level 1 and 2 flights will be presented to show that the navigator is able to fly these levels. Also, a simulation showing whether or not the yaw angle reference is calculated properly and a simulation showing if the helicopter speed is changed when entering and leaving a velocity-reference circle, are presented. All of the simulations are performed on a full helicopter model controlled by an optimal controller which is implemented in S IMULINK , this is elaborated in [Hald et al. 2006]. To minimize the possibility for errors, the navigator has been implemented in ANSI C in such a way that the same C file is used in simulation and on the final implementation on the helicopter. This way, if a change is made in simulation it is also included in the navigator implemented on the helicopter. 52
5.3. SIMULATION OF NAVIGATOR
5.3.1 Simulation of Level 1 In level 1, the navigator is supposed to fly a route defined by a set of waypoints. When the helicopter reaches a waypoint circle it is supposed to fly towards the next waypoint. To simulate this, the helicopter is set to fly the set of waypoints listed in Table 5.2. A 2Waypoint number 1 2 3
Coordinate [m] (0, 0, 0, N/A, N/A) (15, 8, 0, N/A, N/A) (30, 0, 0, N/A, N/A)
Table 5.2: Waypoints for the level 1 example flight.
dimensional plot of the simulation of this flight is shown in Figure 5.18. As Figure 5.18 shows, the helicopter flies from waypoint number 1-3
Northern Distance [m]
Level 1 example flight simulation
10
5
0
−5 −5
0
5
10 15 20 Eastern Distance [m]
25
30
35
Figure 5.18: 2-dimensional plot of a level 1 example flight simulation.
without noticeable deviations between the waypoints. The only deviation present, is the deviation at the moment the helicopter enters the waypoint circle of waypoint 2. This deviation is expected as the helicopter flies towards waypoint 3 the instance it enters the waypoint circle of waypoint 2. The distance flown in this simulation is much shorter than the actual level 1 distance, Hence, a new simulation is performed of a more realistic level 1 flight. In this simulation, the waypoints flown are listed in Table 5.3 The route made from these waypoints are 3 km long, and are situated realistically with reference to how the way53
CHAPTER 5. NAVIGATOR Waypoint number 1 2 3 4 5
Coordinate [m] (0, 0, 0, N/A, N/A) (10, 0, 0, N/A, N/A) (410, 920, 0, N/A, N/A) (1410, 920, 0, N/A, N/A) (1018, 0, 0, N/A, N/A)
Table 5.3: Waypoints for the realistic level 1 flight.
points would be situated in the competition. A 2-dimensional plot of the simulation of this flight is shown in Figure 5.19. This simulation Level 1 realistic flight simulation 1000 900 800 Northern Distance [m]
700 600 500 400 300 200 100 0 0
200
400
600 800 Eastern Distance [m]
1000
1200
1400
Figure 5.19: 2-dimensional plot of a level 1 realistic flight simulation.
showed that the navigator is able to navigate the helicopter along a 3 km long route in the desired way.
5.3.2 Simulation of Level 2 In level 2, the navigator is supposed to make the helicopter fly to a waypoint and rotate the helicopter to a heading defined by the waypoint list. To simulate this, the helicopter is set to fly the waypoints 54
5.3. SIMULATION OF NAVIGATOR presented in Table 5.4. When the helicopter enters waypoint 2, a mesWaypoint number 1 2 3
Coordinate [m] (0, 0, 0, 0, 0) (15, 8, 0, π2 , 1) (30, 0, 0, 0, 2)
Table 5.4: Waypoints for the level 2 example flight.
sage from the mission control will be send to the navigator telling it that the the marker is not found. This should make the helicopter fly towards waypoint 3, and when it enters this waypoint, the navigator will receive a message telling it that the marker is found. This should make the navigator return the house number attached. A 2dimensional plot of the simulation of this flight is shown in Figure 5.20. The plot in Figure 5.20 shows that the helicopter flies through the way-
Northern Distance [m]
Level 2 example flight simulation
10
5
0
−5 −5
0
5
10 15 20 Eastern Distance [m]
25
30
35
Figure 5.20: 2-dimensional plot of a level 2 example flight simulation. The arrows are the heading of the helicopter when inside the waypoints
points 1-3. The interesting things during the level 2 flight is how the yaw angle acts. Hence, a plot of the evolution of the yaw angle is shown in Figure 5.21 The first part of the plot in Figure 5.21 has a value of approximately −1 rad. This fits the heading of the helicopter during the flight between waypoint 1 and 2. When the helicopter enters the waypoint circle of waypoint 2, the yaw reference is set to the value listed in the waypoint list which is π2 rad. When the message telling the helicopter that the marker was not found is received, the helicopter flies towards waypoint 3 and the yaw reference rises to ap55
CHAPTER 5. NAVIGATOR Yaw angle during level 2 example flight simulation 7 6 5
Angle [rad]
4 3 2 1 0 −1 −2
0
40
80
120
160 200 Time [s]
240
280
320
360
Figure 5.21: Plot of the evolution of the yaw angle during the level 2 example flight simulation.
proximately 4 rad, this is the angle of the flight between waypoint 2 and 3. When the helicopter enters the waypoint circle of waypoint 3, it changes the heading to the yaw reference attached to this waypoint, which is 0 rad or 2π rad as the plot shows. This yaw reference is held until the navigator receives message that the marker is found, then the navigator returns the house number 2, which was attached to the waypoint.
5.3.3 Simulation of Unbounded Yaw This simulation serves the purpose of showing that the yaw reference given to the controller is not bound by some period of 2π rad. To simulate this the helicopter is set to fly a level 1 route which is constructed in such way that the yaw angle always will rise. The helicopter is set to fly the waypoints listed in Table 5.5. A 2-dimensional plot of the flight using the waypoints listed in Table 5.5 is illustrated in Figure 5.22. This flight should make the yaw reference rise further than 2π rad, and should therefore show if the yaw reference is bounded. The evolution of the yaw reference during the flight is plotted in Figure 5.23. As Figure 5.23 shows, the yaw increases whenever the helicopter enters a waypoint circle. 56
5.3. SIMULATION OF NAVIGATOR Waypoint number 1 2 3 4 5
Coordinate [m] (0, 0, 0, N/A, N/A) (25, 25, 0, N/A, N/A) (0, 50, 0, N/A, N/A) (-25, 25, 0, N/A, N/A) (0, -50, 0, N/A, N/A)
Table 5.5: Waypoints for the unbounded yaw simulation. Path flown during unbounded yaw simulation 50 40
Northern Distance [m]
30 20 10 0 −10 −20 −30 −40 −50 −20 0 20 Eastern Distance [m]
Figure 5.22: Plot of the helicopter flight during the unbounded yaw simulation.
5.3.4 Simulation of Helicopter Speed Change The purpose of this simulation is to show that the speed of the helicopter is changed whenever the helicopter enters or leaves a velocityreference circle. This is done to have the helicopter fly a route made from the waypoints listed in Table 5.6. This route makes the heliWaypoint number 1 2
Coordinate [m] (0, 0, 0, N/A, N/A) (50, 0, 0, N/A, N/A)
Table 5.6: Waypoints for the helicopter speed change simulation.
copter fly along the x-axis, and then it is shown by the evolution of 57
CHAPTER 5. NAVIGATOR Yaw angle during unbounded yaw simulation 3.5 3 2.5
Angle [rad]
2 1.5 1 0.5 0 −0.5 −1
0
10
20
30
40 Time [s]
50
60
70
80
Figure 5.23: Plot of the evolution of the yaw reference during the unbounded yaw simulation.
the position in the x-direction if the helicopter speed changes. This evolution is illustrated in Figure 5.24. In this simulation the velocity reference circle has a radius of 6 m, and it is clear from the plot in Figure 5.24, that the slope of the position evolution changes at approximately 6 meters out of the x-axis and at approximately 44 meters along the x-axis. This concludes the simulation of the navigator. The simulations made to verify that the navigator works as intended, has all given the expected results. It is important to notice that no outliers or abnormalities occur in the simulations. If such were to occur, it might make the UAV unstable and possibly cause a crash. As the simulations all gave positive results, the navigator can now be tested on the actual UAV.
58
5.4. TEST OF NAVIGATOR Evolution of distance in speed change simulation 50 45 40
Distance [m]
35 30 25 20 15 10 5 0
0
5
10
15
20
25
Time [s]
Figure 5.24: Plot of the evolution of position in the x-direction during the helicopter speed change simulation.
5.4 Test of Navigator After the navigator is fully implemented in the real-time software environment on the helicopter, it must be tested in real-time in cooperation with all other software subsystems which the navigator depends on during flight. This requires the tests to be carried out on the fully implemented UAV platform. Conduction of grounded tests are important prior to any in-flight tests, in order to test functionality and reliability of the software implemented on the UAV without any risk of damaging the UAV itself as result of a crash or cause danger to any nearby humans. To perform the grounded tests, the helicopter has been placed on a table cart, and has been pushed around by a human being in a parking lot, substituting a controller during flight. Hence, in some of the tests, an abnormal deviation from the path might occur, but the tests has been performed as well as possible under the given circumstances. A picture of the equipment used during the tests are shown in Figure 5.25. During these grounded tests several subsystems are used. The mission control subsystem is running and thereby controlling how the sequence of the different tests are carried out. The ground station subsystem is also running to give a visual output of positions, velocities 59
CHAPTER 5. NAVIGATOR
Figure 5.25: Picture of test set-up with UAV and ground station placed on table cart used for test of navigator.
etc. and the tests are started through WLAN connecting the groundstation PC and the OBC. The parts of the navigator which will be tested are: • Cross-track Error Check whether or not ect increases with larger deviation of the helicopter. • Rotation Angle: Check if the rotation angle, Θhelm, evolves as Θhelm = −tanh(ect ) when the helicopter deviates from the course. • Waypoint Change: Check that when the helicopter enters a waypoint circle, it changes its destination from W Pn to W Pn+1 . • Speed: Check that the speed decreases when entering the velocityreference circle and increases when leaving the circle. • Level 1 Done: Check that the helicopter behaves correctly when entering the final waypoint in level 1. • Level 1 Yaw Reference: Check that the yaw reference points in the direction of the velocity reference when flying between the waypoints. • Level 2 Yaw Reference: Check that the correct yaw reference is applied when the helicopter enters a waypoint, and that the yaw 60
5.4. TEST OF NAVIGATOR reference points in the direction of the velocity reference when outside the waypoint circle. • Level 2 Message Handling: Check that the helicopter behaves correctly when the message "Marker found" is given to the navigator. • Level 2 Message Handling: Check that the helicopter behaves correctly when the message "Marker not found" is given to the navigator.
5.4.1 Cross-Track Error In order to verify that the cross-track error is calculated properly, the helicopter is placed on the course and then moved perpendicularly away from it. To be successful, the test should show that the crosstrack error is equivalent to the distance the helicopter is moved. The cross-track error should be positive no matter which side of the course the deviation is situated. In Figure 5.26, the result of the test is illusTest result from the cross−track error test 9 Actual distance 8
Calculated Cross−track error
7
Distance [m]
6 5 4 3 2 1 0
0
2
4
6
8 Time [s]
10
12
14
16
Figure 5.26: Plot of calculated ect and the deviation in northern direction.
trated. The result shows, that the value of ect is smaller than the actual distance. This is the case because of the way the test was performed. The desired test path was meant to follow the x-axis, that is directly towards east. Then the perpendicular deviation would simply be the offset of the helicopter in northern direction. The reason for the bias 61
CHAPTER 5. NAVIGATOR of ect is probably that the test path was a bit off. The perpendicular distance to the test path will always be shorter than the distance in northern direction if the test path was just a few degrees of. Despite the bias it is clear that the trend of ect is the same as the trend of the deviation. Hence, it is concluded that ect is calculated correctly. Also, the test has showed that there are no outliers which could potentially cause instability.
5.4.2 Rotational Angle To test that the rotational angle Θhelm is calculated properly, the helicopter is placed on the course and then moved perpendicularly away from it. To be successful, the test should show that the evolution of Θhelm is equal to π Θhelm = −tanh (ect ) . 4
(5.10)
A plot of the tanh()-function is shown in Figure 5.17 on page 51. Test result from the rotational angle test 0 −0.1 −0.2
Angle [rad]
−0.3 −0.4 −0.5 −0.6 −0.7 −0.8
0
2
4
6
8
10
12
14
Time [s]
Figure 5.27: Plot of Θhelm when moving the helicopter perpendicularly away from the test path.
As Figure 5.27 shows, the trend of Θhelm is similar to the trend of the −tanh()-function. This is a very important result as it shows that the correction angle Θhelm is actually limited by the value π4 as intended. 62
5.4. TEST OF NAVIGATOR
5.4.3 Waypoint Change To be successful, this test must show that when the helicopter enters the waypoint circle around WPn , it changes from WPn → WPn+1 . This test was performed by observing the waypoint ID number on the ground-station PC while moving the helicopter inside a waypoint circle. The test showed that the waypoint ID changed immediately when the helicopter entered the waypoint circle.
5.4.4 Speed The speed of the helicopter must change when the helicopter enters or leaves a velocity-reference circle. This is tested by moving the helicopter into and out from a velocity-reference circle and visually check that |vout | changes from the value of cruise speed to the value of turn speed. The visual check is made by printing |vout | on the groundstation PC during flight. The test showed that the speed changed immediately when entering or leaving a velocity-reference circle.
5.4.5 Level 1 Flight A full level 1 path has been performed to test both the yaw reference during the flight of this level and also to test if the navigator acts correctly when all the waypoints in level 1 has been flown. To test the yaw reference, the helicopter is pushed in the direction the yaw reference points and it is examined if the helicopter reaches all waypoints. And when entering the last waypoint, it is checked if the navigator state changes to LEVEL_1_DONE. In Figure 5.28 a plot of the level 1 flight is illustrated. As Figure 5.28 shows, the helicopter has flown through all the waypoints. This means that the yaw reference is correct as the UAV was pushed in the direction of the yaw reference. Furthermore it was observed on the ground-station PC under test, that when entering the final waypoint circle, the navigator changed the state from ONGOING to LEVEL_1_DONE and went into hover mode.
5.4.6 Level 2 Flight A level 2 test flight has been performed to test that the correct yaw references are used in level 2. Furthermore it is tested if the navigator acts correctly to the message from the mission control telling it whether or not the marker is found. 63
CHAPTER 5. NAVIGATOR Test result from the full level 1 flight 45 40
Northern distance [m]
35 30 25 20 15 10 5 0 −5 −10
0
10
20 30 Eastern distance [m]
40
50
Figure 5.28: Plot of a level 1 test flight.
The test was performed by pushing the helicopter in the direction of the yaw reference when it is outside a waypoint circle. When the helicopter enters the first waypoint circle it is checked that the value of the yaw reference is the same as the one listed in the waypoint list. This is done by checking the yaw reference on the ground-station PC. After 20 seconds, the navigator will receive the message that the marker was not found and it must seek the next waypoint. When entering the next waypoint, it is checked again that the correct yaw value is used, and after 20 seconds, the navigator is told that the marker is found.
Northern distance [m]
Test result from the level 2 test flight 10 5 0 −5 0
10
20 30 Eastern distance [m]
40
50
Figure 5.29: Plot of a level 2 test flight.
The test has been performed,and the level 2 test flight is illustrated in Figure 5.29. The yaw references was correct both inside and out64
5.5. TUNING OF NAVIGATOR side the waypoint circles, and the navigator acted as planned on the messages received from the mission control. The first message telling the navigator that the marker was not found resulted in the helicopter seeking the next waypoint. The message telling that the marker was found resulted in the navigator changing state to LEVEL_2_DONE_FOUND together with the navigator going into hover mode and returning the house number to the mission control. To conclude, the tests were all performed and the results were positive. Besides showing if the navigator works at all, it was very important that the tests showed that no abnormalities occurred. These abnormalities could be crucial to the helicopter and could potentially cause a crash. Further aerial tests has not been conducted as the group responsible for the controller design was unable to deliver a controller able to stabilize the helicopter.
5.5 Tuning of Navigator The purpose of level 1 of the IARC06 is to fly through a set of waypoints as fast as possible. The total time it will take to fly level 1 is dependent on the radius of the waypoint circle as well as the radius of the velocity-reference circle. The dependency of the velocity-reference circle is obvious, the speed of the helicopter is lower inside a velocity-reference circle than outside. Based on this, the radius of the velocity-reference circles must be as small as possible, but if they are made to small, the helicopter might overshoot the turns. If this overshoot becomes large, it might be better to increase the radius of the velocity-reference circles. By doing this, the distance the helicopter flies is shorter, but a bigger part of the distance is flown with the slow speed. Hence, as the overshoot is determined by the performance of the underlying controller, it can be concluded that the simulation flight-time is dependent on this controller. More information of the controller used in this simulation can be found in [Hald et al. 2006]. The total flight time is also dependent of the waypoint circles. If the waypoint circles become larger, the total flight path gets shorter. This is due to the fact that the helicopter is considered as having flown the entire course, the instance it enters the last waypoint circle. If the radius of this circle is large, the helicopter enters it sooner than if the radius is small. 65
CHAPTER 5. NAVIGATOR A simulation using the full helicopter model has been made to examine the total flight time as a function of the radii of the velocityreference and waypoint circles. The radius of the waypoint circles are changed from 0.2 m to 5 m with an interval of 0.2 m. And for every radii of the waypoint circle, the radius of the velocity-reference is changed from the radius of the waypoint circle to 10 m with an interval of 0.5 m. This yields 385 simulations of the flight path illustrated in Figure 5.19 on page 54. The simulations were conducted and the result is illustrated in Figure 5.30. It is shown in Figure 5.30 that the surface of the plot has small Time of 3km long flight 1139
1138
1137
Time [s]
1136
1135
1134
1133
1132
1131 5
4.5
4
3.5
3
2.5
2
1.5
Size of the waypoint circle
1
0.5
0
2
4
6
8
10
Size of velocity−reference circle [m]
Figure 5.30: 3-dimensional plot of the flight time as a function of the radii of the velocityreference and waypoint circles.
variations, it is not smooth. This is probably because the simulation, that the plot was made from, was conducted on the full and complex helicopter model. To examine the 3D plot further, a 2D plot have been made where the radii of the waypoint circles varies. The radii of the velocityreference circles are in this plot as small as possible. This means that they always are equal to the radii of the waypoint circles as it makes no sense to have a velocity-reference circle which is smaller than the waypoint circle. The plot is shown in Figure 5.31. The plot in Figure 5.31 shows that when the radius of the waypoint circle is increased, the time rises at first, but then decreases. The increase at first is caused by 66
5.5. TUNING OF NAVIGATOR Time of 3km long flight with the radii of the waypoint circle varying 1133.5
1133
Time [s]
1132.5
1132
1131.5
1131
0
0.5
1
1.5 2 2.5 3 3.5 Size of the waypoint circle [m]
4
4.5
5
Figure 5.31: Plot of the flight time as a function of the radii of the waypoint circle.
the fact that the helicopter flies with the turn speed longer when the waypoint circle is increased. This is only the case because the radii of the waypoint and velocity-reference circles are the same. The reason the time decreases with larger waypoint circle radius is that the entire path shorter. This is because the path stops the instance the helicopter enters the final waypoint circle. So if the waypoint circle is larger, the helicopter enters it sooner, hence the decrease in time. It was expected that an increase of the radius of the velocity-reference circle could lead first to a decrease of the flight time and then when increased further it would cause a increase in the flight time. To examine the trend of the simulation as a function of the radius of the velocity-reference circle, a plot is made where the radius of the waypoint circle is constant. This is illustrated in Figure 5.32. As the plot in Figure 5.32 shows, the time is monotonically rising when the radius of the velocity-reference is increased. The reason it is monotonically rising, and not first falling and then rising is the controller available for this simulation. For the helicopter to overshoot when making a turn, the helicopter must fly with a fast speed outside the velocity-reference circles and a slower speed inside. The controller available to this simulation is designed as a hover controller and if it gets a speed reference larger than 3 m/s, it becomes unstable. Hence the simulation was conducted with a speed of 3 m/s outside the velocity-reference circles and a speed of 0.5 m/s inside 67
CHAPTER 5. NAVIGATOR Time of 3km long flight when waypoint circle radius is 0.2m 1144
1142
Time [s]
1140
1138
1136
1134
1132
0
1
2
3 4 5 6 7 Size of velocity−reference circle [m]
8
9
10
Figure 5.32: Plot of the flight time as a function of the radii of the velocity-reference circle.
them. However, the controller is stable enough to be able to turn with 3 m/s, and the deceleration from 3 → 0.5 m/s is done almost instantly. Hence, it is not necessary to use the velocity-reference circles as the turns are made without overshoot when flying 3 m/s. It is concluded that using this controller, there are no use for the velocity-reference circles as the helicopter has no need to decrease the helicopter speed to make a turn. It can also be concluded based on Figure 5.30 that, with this controller, there is a difference of 2 seconds if the radius of the waypoint circle is either at its smallest or at its highest. A difference of 2 seconds out of approximately 1130 seconds is not really an optimization worth mentioning as a gust of wind may cause a bigger difference than that. Still it must be pointed out that if the controller was different, i.e. flew fast enough to cause an mentionable overshoot during turns, the velocity-reference circle would become more important. It would both be able to minimize this overshoot and thereby decrease the complete flight time and it also might make an unstable turn stable. Hence, the velocity-reference circle makes the navigator more generic for any underlying controller.
68
Vision
6
In this chapter the vision system used for marker detection and entrance identification is elaborated. Firstly an analysis of the vision subsystem is carried out, followed by a design from which the implementation of the subsystem can be made. Lastly a test of the vision subsystem verifies that it is able to detect the marker.
6.1 Analysis of Vision As stated in the problem formulation on page 11, a vision system, which is able to examine the pictures for the marker, must be implemented. Together with the marker detection it must also be able to detect if a house entrance is present in the picture. This yields that the vision system can be running in two modes, either it is searching for the marker or else it is searching for entrances. The mathematical detection algorithms are developed by Anders la Cour-Harbo, and will not be presented here. The algorithms are assumed to be black boxes with a picture as input and an output representing the statistically probability of the marker is present, or the dimensions of the house entrance if one is present within the picture. During the development phase of the detection algorithms it is necessary to be able to collect pictures of the marker for off-line analysis. Hence a development mode must also be implemented in the vision system. It is not desirable to take pictures constantly when searching for either the marker or entrances, thus the mission control must be able to set the vision system in either idle, running or shutdown mode. When the vision system is running the mission control must be able to control the rate at which the pictures are collected and analyzed by the detection algorithms. Furthermore the vision system must be able to notify the mission control when the marker or entrance has been detected. In order for the mission control to be able to control the Vision System, and for the vision system to notify the mission control, a 69
CHAPTER 6. VISION proper interprocess communication must be created between the mission control and the vision subsystem.
6.2 Design of Vision With background in the above analysis, the design of the vision system has been chosen according to the flowchart in Figure 6.1. Initially the vision system starts the camera, and loads the values for the runtime parameters set by the mission control in an allocated memory space. The parameters located in this memory space are shown in Table 6.1. In order to secure data contention in this memory space Parameter State Algorithm ID npics cam_rate locked
Description Defines the state that the vision system must be set to. Defines which detection method that must be used. Defines the number of pictures to be taken. Defines the rate at which the camera must collect the pictures. Semaphore used to ensure data contention.
Table 6.1: Parameters located in the vision system shared memory space .
semaphores are used, which is necessary since the vision system not only reads from this memory space but also writes to it, which is showed later in the flowchart. The next step in the flowchart is to take action according to the read state parameter. If state equals IDLE, the vision subsystem polls the runtime parameters with a frequency of 5Hz until state is changed. If state is changed to STOP the vision subsystem will shut down. If state however is changed to RUN a picture is taken with the camera. According to the read algorithm_id the vision algorithm will be chosen. If algorithm_id is equal to either MARKER or ENTRANCE the vision algorithms will be used accordingly. However if algorithm_id equals DEVELOPMENT no vision algorithm is used. When the picture is stored to the hard-disk drive it follows a naming convention. A folder is created named according to the vision algorithm with a date and time stamp. The same applies for the pictures, which are named according to the vision algorithm with a date 70
6.2. DESIGN OF VISION
Initialize camera
Read runtime parameters
Sleep 200 ms
IDLE
STOP
state RUN
Take picture
Call marker detection algorithm
MARKER
algorithm_id
Call entrance detection algorithm
ENTRANCE
DEVELOPMENT (No vision algorithm) Store picture to harddisk
state = IDLE
yes
npics = img_cnt no
Send result to mission control
yes
cam_rate = -1
no
Sleep 1/cam_rate
Figure 6.1: Flowchart of vision system.
71
CHAPTER 6. VISION and time stamp but also a number which tells which picture, in the collected picture sequence, it is. If the number of picture, npics, has been reached, the Vision System sets state to IDLE and the result of the vision algorithm is sent to the mission control through a FIFO. If the number of pictures has not been taken, the cam_rate parameter is checked. If cam_rate is equal to −1 the vision system rereads the runtime parameters and starts over instantly. If vcam_rate differs from −1 the vision system is delayed 1 cam_rate before continuing. This concludes the design of the vision system, and the implementation of it is shown in Appendix C.6. In the next section the vision system will be tested.
6.3 Test of Vision In this section, the test of the vision system will be carried out. The vision system is designed to read a set of runtime parameters from shared memory and act according to the values of these. In this section the vision system will be tested to ensure that it reacts according to these runtime parameters. A test will demonstrate that the state of the vision system can be controlled, that is, set it to either IDLE, STOP or RUN mode. The test of the development mode that stores pictures to the hard-disk drive of the OBC is included in this test. Two other test will demonstrate the functionality of the detection algorithms for marker detection and entrance identification.
6.3.1 Test Development Mode and State Change In the IDLE-state, the vision system is expected to do nothing except to check the state-parameter with a frequency of 5 Hz. When state changes, the vision system enter the state of the one specified in state. To carry out the test, the Vision System is initialized in the IDLEstate in the DEVELOPMENT-mode, whereafter the state is changed to RUN. In order to test if the vision system executes as expected, it is set to output debug information, and a dump of this debug information is shown in Figure 6.2. As Figure 6.2 shows, the vision system first executes an initialization process. Then it reports that the pictures are stored in the folder “development_pics” with the time stamp for the current date and time the vision system was started. Afterwards the vision systems idles for 15 seconds whereafter the state is changed 72
6.3. TEST OF VISION
01:
Vision:
Subsystem initialized
02:
Vision:
Camera initialized
03:
Vision:
development_pics-2006.05.08-14.26.58
04:
Vision:
Idle...
05:
Vision:
Idle...
06:
Vision:
Idle...
07:
Vision:
Image acquired
08:
Vision:
Output from camera written in cam.bmp
09:
Vision:
Image acquired
10:
Vision:
Output from camera written in cam.bmp
11:
Vision:
Subsystem terminating
12:
Vision:
Allocated memory freed
13:
Vision:
Camera shutdown
Figure 6.2: Test output for demonstration of state change of vision system, together with the demonstration of the development mode.
to RUN, and the vision system starts taking pictures as it should in the development mode. When state is changed to STOP the vision system shuts down.
6.3.2 Test of Detection Algorithm The purpose of this test is to demonstrate that the vision system can process an image, through the marker detection algorithm. This is done by initiating the vision system in the RUN-state and in the mode MARKER. To verify that the vision system has detected the marker, a cross is inserted in the picture where the marker was detected. In Figure 6.3 the result of the test is shown, which demonstrates that the vision system is able to make use of the provided marker detection algorithm. To conclude, the vision system is able to detect the marker in the acquired image. However, the test of the entrance detection cannot be carried out, due to the fact that the development of the entrance detection algorithm was not finished.
73
CHAPTER 6. VISION
Figure 6.3: Picture used for test of vision system. The blue cross indicates the position at which the marker has been detected.
74
Mission Control
7
The mission control subsystem is the subsystem that controls the activities the helicopter performs. By use of the navigator and the vision subsystem, the mission control is able to carry out the two levels previously described. Hence, the mission control has the responsibility of making the helicopter perform the required actions to successfully complete the mission levels, and thereby it is the subsystem that makes the required decisions. In the following the design of the mission control is described together with a test verifying that it takes the required actions to complete the mission levels.
7.1 Design of Mission Control The mission control has been made generic in the sense that it can be set to fly only level 2 or only try to locate an entrance of a specific house etc.. But in this section, a full level 1 and 2 flight is described to illustrate how the mission control works. The order at which the different parts of the mission must be carried out, was outlined in the flowchart in Figure 1.2 on page 9. The design of the mission control subsystem is thereby simplified to the order at which the other subsystems must be activated to carry out a given task specified by Figure 1.2. The order of the tasks and thereby the interprocess communication order between the different software subsystems, is shown in the UML sequence diagram in Figure 7.1. As the sequence diagram illustrates, the major part of the interprocess communication to and from the mission control is with the navigator, and this interprocess communication is essential for the helicopter, to be able to fly the levels. The interprocess communication is built up by predefined messages listed in Appendix F, which are illustrated as capitalized text at the arrows in the sequence diagram in Figure 7.1. The first message which is sent is INIT_LEVEL_1. This message is sent to tell the navigator to initialize for level 1 flight. When the navigator is done initializing, it returns the message INIT_LEVEL_1_DONE. When 75
CHAPTER 7. MISSION CONTROL
Mission Control
Navigator
Vision
INIT_LEVEL _1
INIT_LEVEL _1_OK LEVEL _1
LEVEL _1_DONE INIT_LEVEL _2
INIT_LEVEL _2_OK LEVEL _2
INSIDE_WAYPOINT
Loop until marker found
SCAN_FOR_MARKER
Probability of marker detected MARKER_NOT_FOUND [Marker not found ] MARKER_FOUND [Marker found ]
HOUSE_NUMBER LEVEL _2_ENTRANCE [Housenumber ]
INSIDE_WAYPOINT Loop until marker found
SCAN_FOR_ENTRANCE
Entrancesize ENTRANCE_NOT_FOUND [Entrance not found ] ENTRANCE_FOUND [Entrance found ]
Figure 7.1: Sequence diagram of the mission control.
this message has been received, the mission control sends the message LEVEL_1 to the navigator to make it fly level 1. The navigator will then make the helicopter fly the defined level 1 path and when it has flown through all the waypoints, the navigator will make the helicopter hover and then it will return the message LEVEL_1_DONE. When the mission control receives this message, it will tell the navigator to initialize for a level 2 flight. And when the navigator has initialized for level 2, it returns a message telling the mission control that it is initialized. When this message is received, the mission control tells the navigator to start flying level 2. The navigator will now fly the helicopter to the waypoint it is supposed to be in next. When 76
7.1. DESIGN OF MISSION CONTROL this waypoint is entered, the navigator sends a message to the mission control telling it that the helicopter has reached the waypoint. Here the helicopter is set to hover and wait on a response from the mission control. Before responding, the mission control sends a message to the vision system, telling it to take and analyze pictures to search for the marker. When this has been done, the vision system returns a probability stating how likely it is that the marker is in the pictures. Based on this probability, the mission control must take a decision of whether or not to believe that the marker has been found. Hence, a threshold is defined to decide if the marker is found or not. If the marker has not been found, the mission control sends the message MARKER_NOT_FOUND to the navigator. The navigator will then fly to the next waypoint in the waypoint list and when entering this waypoint, send a message to the mission control telling it that the helicopter is inside a waypoint. The loop continues until either there are no more waypoints, or until the probability returned from the vision system is higher than the defined threshold. If the last waypoint is reached and no marker is found, the helicopter is set to hover and the mission has failed. But, if a probability higher than the threshold is received from the vision system, the message MARKER_FOUND is sent to the navigator telling it that the marker has been found. The navigator will then return the house number of the house it is taking pictures of to the mission control. When the housenumber is returned by the navigator, the mission control sends LEVEL_2_ENTRANCE, with the house number as a parameter, to the navigator. The navigator will then navigate the helicopter to the waypoints associated with the given house number. When the helicopter is navigated to a waypoint to search for the entrance the navigator sends INSIDE_WAYPOINT to the mission control after which the mission control initiates scanning for a entrance by sending SCAN_FOR_ENTRANCE to the vision subsystem. The vision subsystem returns the message ENTRANCE with the identified entrance size as a parameter to the mission control. If the entrance does not live up to the expected demands for the entrance size, the process is repeated by sending ENTRANCE_NOT_FOUND to the navigator, or in the case where the entrance does live up to the expectations the mission control sends the message ENTRANCE_FOUND to the navigator which then brings the helicopter to hover. In the mission control, a simple fault handling is implemented. Ba77
CHAPTER 7. MISSION CONTROL sically it works by sending a hover message to the navigator if a faulty message is received. It is obviously not enough to secure that the helicopter does not crash upon an error in the algorithms, but it might save the helicopter if an unexpected or faulty message is received. This concludes the design of the mission control. The implementation of it is shown Appendix C.1.
7.2 Test of Mission Control As mentioned in the introduction to this chapter, the mission control must be tested to verify that the order of the execution of the tasks are correct. Furthermore this test verifies that the interprocess communication works as intended in the design. The mission control has been tested by adding debugging information to the code such that all the messages sent to and from the mission control are logged. A test flight of level 1 and 2 has then been performed and the result of this test is shown in Figure 7.2. As Figure 7.2 shows, the test has been performed and the messages are sent and received correctly. In the test, the subsystems vision and navigator is not actually run. They are called and are told to return the needed message instantly. This is done partly because it makes no difference for the communication between the mission control and the other subsystems whether or not they perform an algorithm or are told to answer immediately. But also partly because the vision have not been fully developed. The entrance detection part of the mission control has not been tested. This is due to the entrance detection algorithm not having been developed. This concludes the test of the mission control. The test showed that the interprocess communication between the subsystems functioned as intended. Furthermore the sequential order at which the mission tasks were initiated corresponded to the one defined in Figure 1.2.
78
7.2. TEST OF MISSION CONTROL
01:
Controller :
02:
Controller :
Shared Memory Allocated
03:
Navigator :
04:
Mission Control:
IPC’s OK
05:
Mission Control:
Vision System modes set
06:
IPC : Navigator -> Controller (HOVER)
07:
IPC : Mission Control -> Navigator (INIT_LEVEL_1)
08:
IPC : Navigator -> Mission control (INIT_LEVEL_1_OK)
09:
IPC : Mission Control -> Navigator (LEVEL_1_START)
10: . . .
IPC : Navigator -> Controller :
(Controller ref’s)
11:
IPC : Navigator -> Controller :
(Controller ref’s)
12:
IPC : Navigator -> Mission control (LEVEL_1_DONE)
13:
IPC : Mission Control -> Navigator (HOVER)
14:
IPC : Mission Control -> Navigator (INIT_LEVEL_2)
15:
IPC : Navigator -> Mission control (INIT_LEVEL_2_OK)
16:
IPC : Mission Control -> Navigator (LEVEL_2_START)
17: . . .
IPC : Navigator -> Controller :
(Controller ref’s)
18:
IPC : Navigator -> Controller :
(Controller ref’s)
19:
IPC : Navigator -> Mission control (INSIDE_WAYPOINT)
20:
IPC : Mission Control -> Vision (SCAN_FOR_MARKER)
21:
IPC : Vision -> Mission Control (MARKER 40%)
22:
IPC : Mission Control -> Navigator (MARKER_NOT_FOUND)
23: . . .
IPC : Navigator -> Controller :
(Controller ref’s)
24:
IPC : Navigator -> Controller :
(Controller ref’s)
25:
IPC : Navigator -> Mission control (INSIDE_WAYPOINT)
26:
IPC : Mission Control -> Vision (SCAN_FOR_MARKER)
27:
IPC : Vision -> Mission Control (MARKER 60%)
28:
IPC : Mission Control -> Navigator (MARKER_FOUND)
29:
IPC : Navigator -> Mission control (HOUSE_NUMBER)
30:
Mission Control:
31:
Navigator :
32:
Controller :
Helicopter set to manual mode (0) Shared Memory Allocated
Subsystem shut down
Subsystem shut down Subsystem shut down
Figure 7.2: Test output for testing mission control execution order and interprocess communication. In this test the debugging information showing that the navigator sends references to the controller is also shown.
79
Flight Data Logger
8
In this chapter the Flight Data Logger will be elaborated. Firstly the flight data logger is analysed in terms of purpose and based on that a design of the data logger will be presented.
8.1 Analysis of Flight Data Logger As stated in the problem formulation page on 11, a flight data logger, able to log flight data for off-line analysis, is needed. There are several methods for data logging but an analysis of these is out of scope for this thesis. Instead, a simple method is applied, which consists of writing selected sensor and program information to a file, since this method fulfills the demands. The data to be logged by the flight data logger is primarily to be used in M ATLAB applications, and therefore it has been chosen to write the data to a file in a syntax that M ATLAB uses. Comma separated value files, or csv-files, is based on a raw data format where the values are separated by commas. This is a format that M ATLAB can import, hence the csv format is chosen as the data format the flight data logger stores in. The information to be logged is passed to a FIFO buffer by the controller subsystem as described in chapter 4. The data located in the FIFO has the same structure as that of the shared memory. A log file for each of the elements of the structure is created and the data of the elements are written to the log file separated by commas. When the log files are stored, the folder they are stored in is named with time and date. During test flights, experiences showed that the hard-disk drive in the OBC created timeouts when trying to access it during data writes to the log files. These timeouts were probably caused by the g-loads from the vibrations of the engine or the manoeuvres during the flight. Therefore it has been selected to store the log data in the RAM of the OBC until the helicopter has landed, and then move them to the harddisk drive of the OBC. In the case of a crash or malfunction the data 81
CHAPTER 8. FLIGHT DATA LOGGER will not be copied to the hard-disk drive if the OBC is destroyed, but in this case the the same data has been logged in the ground station PC. This is described in chapter 9.
8.2 Design of Flight Data Logger Based on the analysis described in the previous section and the flight experiences, the design of the flight data logger is shown in Figure 8.1. The flight data logger initially opens the files in which the log data is
Create folder named by date and time
Read sample of shared memory from FIFO
Create csv-files for log data in RAM
Write data to files
yes Move files to HDD of OBC
no Close files
End = 1?
Figure 8.1: Flowchart of the flight data logger.
to be written. Then a sample of the shared memory from the FIFO is read, where the number of bytes read from the FIFO corresponds to the data size of shared memory structure. Afterwards each element of the data are stored in its own csv file. The program runs in a loop which terminates when end is set either by a user or a task. Upon termination the files are closed and moved to the hard-disk drive of the OBC and the flight data logger terminates. This concludes the design of the flight data logger. The implementation of the logger is shown in Appendix C.5.
82
9
Ground Station
To be able view follow flight data while the helicopter is flying, a ground station is developed. In this chapter the analysis, design and test of the groundstation server and ground-station client will be elaborated.
9.1 Analysis of Ground Station The purpose of the ground station is to be able to display the helicopter flight data on a client computer during flight. Besides displaying the data, the data must also be logged, on the client computer, since the data is valuable when investigating the cause of the error in case of a crash or malfunction. The data must be sent from the OBC during flight. This demands the OBC to run a server which collects the flight data and send these to the client in the ground-station PC through WLAN. In Figure 9.1 the different elements of the ground staOBC
Ground station PC
Server
Shared Memory
WLAN (TCP)
Client
Logger
User Interface
Figure 9.1: Overview of the ground station
tion is shown. The server collects the flight data located in the shared memory and sends this back to the client, through its WLAN link. When the data is received by the client the data is both logged and displayed on the screen. The server must be able to handle if either the connection to the client, is lost or closed by it. The communication 83
CHAPTER 9. GROUND STATION can also travel from the client to the server, to be able to synchronize them. It has been chosen to built up the transmission between the server and client using TCP sockets. A TCP socket provides an application protocol interface (API) to the network protocol at the transport layer, which enables the programmer to implement his own higher level protocol layers, see Figure 9.2 [Tanenbaum 2003]. At the transport Application formats (e.g. HTML, XML) Layer 5-7: session, present & application (e.g. SSL, HHTP)
User Application
Socket Interface
API
Layer 4: Transport (TCP or UDP)
Layer 3: Network (e.g IP)
Layer 2: Data (e.g Ethernet)
Layer 1: Physical (e.g. Twisted Pair)
Figure 9.2: The network protocol layers. The socket interface is placed on top of the transport layer giving an API for user applications.
layer two specific protocols can be used; either the transmission control protocol (TCP) or the user datagram protocol (UDP). The difference between TCP and UDP is that, while TCP is a stream protocol UDP is a datagram protocol. The stream protocol (TCP) establishes a continuous connection between client and server, over which bytes can be transmitted. The order of the bytes is received the same way as they were sent while the connection is established. However, the bytes has no built-in structure which requires higher level layers to delimit the data bytes by themselves. In the UDP protocol there is no demand of having a continuous 84
9.2. DESIGN OF GROUND STATION connection established, it simply transmits a message between addresses. The great advantage compared to TCP is that the data is selfdelimiting, which means that it is defined exactly where the received data begins and ends. On the other hand no guarantee is provided regarding the order in which the datagrams are received, if they even arrive. The choice has fallen on the TCP protocol, since this assures that the data packages are received correct. Furthermore, a data delimiter on the client is easy to implement when using standard structs in C to give the data stream a structure.
9.2 Design of Ground Station The design of the ground station consists of a design of the server located at the OBC, and the client located at the ground-station PC. The client furthermore consists of two other parts, which is the logging part and the user interface part. To be able to synchronize the client and server it is necessary to define the data-flow sequence that the client and server follows sending data between them. The data-flow sequence is shown in Figure 9.3. When the client connects it sends the message GSRUN to the server. Client
Server GSRUN Data stream GSOK
GSRUN Data stream
{loop}
GSOK
Client shutdown
GSSTOP
Figure 9.3: Protocol for data exchange between ground-station client and ground-station server
When the server receives this message it sends its data from the shared memory to the client. When the client has received this, it sends 85
CHAPTER 9. GROUND STATION GSRUN again as a request for new data. This continues until the client disconnects by sending GSSTOP to the server. The design of the server and client are elaborated in the following subsections respectively.
9.2.1 Ground-station Server The design of the ground-station server is shown in Figure 9.4. Initially
Attach to shared memory
Wait for client connection
Create server TCP socket
Create client TCP socket
Bind to server socket
Receive syncbyte from client
Listen on the server socket
syncbyte
GSRUN GSERROR
clientstatus
GSSTOP
Close client socket
GSRUN Send content of shared memory to client
GSSTOP
Figure 9.4: Flowchart for the server part of the ground station
the server attaches to the shared memory created by the controller subsystem. The shared memory contains all the flight data that must be send back to the client. Next, the TCP socket is created and the server binds to its own socket. The server is set to listen on the bounded socket, and thereby ready to accept client connections. When the a client connection is established, the server creates a client TCP socket for the connected client. The server now expects 86
9.2. DESIGN OF GROUND STATION to receive a synchronization byte as defined in the previously shown client-server protocol. According to the value of syncbyte, the server takes action. If syncbyte equals GSRUN which it should according to the client-server protocol, the content of the shared memory is sent in a function, which returns the status of the client connection in clientstatus. If clientstatus equals GSRUN, the server loops back to receive a new syncbyte from the client, and the same procedure is repeated. If clientstatus however is GSSTOP, the client socket is closed and the server awaits a new client connection. If clientstatus equals GSERROR the client and server has lost synchronization, causing the server to close the client connection and await for a new client connection. When the server needs to be shutdown, it can be done by sending a kill signal to the process. When this is done the server takes the action shown in Figure 9.5 Kill Signal Received Close server socket
Close client socket
Figure 9.5: Flowchart for shutdown of server process when receiving kill signal
9.2.2 Ground-station Client The design of the client part of the ground station is shown in Figure 9.6. As shown in the figure, the design of the client is sequenCreate client TCP socket
Connect to client socket
Initialize screen
Send GSRUN to server
Receive data from server
Log received data to file
Print received data to screen
Loop
Figure 9.6: Flowchart for the client part of the ground station
tial. The client starts by creating its client TCP socket and connects to it. Afterwards the screen is initialized, by the use of a library called 87
CHAPTER 9. GROUND STATION Ncurses, which is a Linux library used to create simple text user interfaces, which is sufficient in the ground-station client application. Next the GSRUN message is send to the server, which replies according to Figure 9.3 by sending back the data. When the data is received it is logged to a file, and next it is displayed on the screen. These steps are looped back to sending GSRUN and repeated until aborted. To abort the process a kill signal can be send like to the server. When this is executed the client acts according to Figure 9.7, where Kill Signal Received Send GSSTOP to server
Close client socket
Close logfiles
Shutdown User Interface
Figure 9.7: Flowchart for shutdown of server process when receiving kill signal
the GSSTOP command first is send to the server to tell that the client is disconnecting and the client socket is closed. The log file is also closed and lastly the ncurses mode is ended. This concludes the design of the ground station- server and client, which has been implemented on the helicopter as shown in Appendix C.4. In the next section the test of the ground station will be presented.
9.3 Test of Ground Station The test of the ground station includes both a server side test and a client side test. At first the ground-station server will be presented, which is done by showing output information for the current state of the server. The same is done for the client, together with a display of the user interface.
9.3.1 Test of Ground-Station Server For test of the ground-station server, a test must be done that shows that the server is able to handle a connecting client and the disconnection of a client whereafter it must be ready to handle, the same or another connecting client, once again. To ensure this, the server is started, a client connects, disconnects and the another client connects. The resulting output from the ground-station server is shown in Figure 9.8. As shown in Figure 9.8 the output of the server first indicates that it is serving at the IP address 192.168.1.101. Then the 88
9.3. TEST OF GROUND STATION
01:
GS Server:
Serving @ 192.168.1.101
02:
GS server:
Succesfully attached to shared memory!
03:
GS Server:
Waiting for client connection
04:
GS Server:
Client 192.168.1.102 connected on socket 3
05:
GS Server: DATA (644 bytes)
07:
GS Server: (C(Θ)r1,unit ))2 + arccos(g2,unit > (C(Θ)r2,unit ))2 + arccos(g3,unit > (C(Θ)r3,unit ))2 .
(10.9)
To minimize f (Θ) from (10.9), f (Θ) is differentiated and set equal to 0. df it is not shown Due to the size and complexity of the expression of dΘ in the text neither is the full expression of (10.10) Θmin = argmin(f (Θ)).
(10.10) 99
CHAPTER 10. MAP GENERATION FOR UAV NAVIGATION df It is not possible to isolate Θ in the expression of dΘ = 0, in order to form an analytical expression for Θmin that minimizes f (Θ). Instead M ATLAB is used to calculate the value of Θmin numerically that satisfies (10.10).
10.4.3 Off-set The off-set of the relative reference points, in relation to the GPS reference points, must be determined in order to move the relative map coordinates to the position of the GPS reference coordinates. An illustration of the distance between the relative reference points and the GPS reference points are shown in Figure 10.6. Based on Figure 10.6, R2 G2
R1 G1
R3 G3
Figure 10.6: Illustration of reference points for calculation of distance offset.
(10.11) and (10.12) have been derived calculating the summed squared x- and y-distance between the relative reference points and the GPS reference points respectively, yielding f (dx ) =(G1,x − R1,x + dx )2 + (G2,x − R2,x + dx )2 + (G3,x − R3,x + dx )2 ,
(10.11)
and f (dy ) =(G1,y − R1,y + dy )2 + (G2,y − R2,y + dy )2 + (G3,y − R3,y + dy )2 . 100
(10.12)
10.4. COORDINATE TRANSFORMATION In order to minimize the summed squared distance, between the relative and the GPS reference points, (10.11) and (10.12) are differentiated and set equal to 0 and dx and dy are isolated, yielding df =2 · (3 · dx + G1,x + G2,x + G3,x − R1,x − R2,x − R3,x ) ⇒ ddx 0 =2 · (3 · dx + G1,x + G2,x + G3,x − R1,x − R2,x − R3,x ) ⇒ −(G1,x + G2,x + G3,x − R1,x − R2,x − R3,x ) dx = (10.13) 3 and df =2 · (3 · dy + G1,y + G2,y + G3,y − R1,y − R2,y − R3,y ) ⇒ ddy 0 =2 · (3 · dy + G1,y + G2,y + G3,y − R1,y − R2,y − R3,y ) ⇒ −(G1,y + G2,y + G3,y − R1,y − R2,y − R3,y ) dy = (10.14) 3 This concludes the derivation of the formulas to be used for the implementation of the map transform algorithm, which is described in the next section.
10.4.4 Implementation of Map Transform Algorithm In order to apply the derived equations calculating the scale factor, rotation, and off-set and using the results to convert the the relative house and waypoint coordinates into GPS coordinates, an algorithm has been implemented in M ATLAB. In Figure 10.7 a diagram is shown Load reference points
Calculate scale factor
Calculate rotation
Apply scaling and rotation to relative waypoints and house coordinates
Store temporary waypoints and house coordinates
Calculate off-set
Apply off-set to temporary waypoints and house coordinates
Store final GPS waypoints and house GPS coordinates
Figure 10.7: Illustration of implementation of the coordinate conversion algorithm.
describing the steps of the map transform algorithm. Firstly the coordinates of the relative and GPS reference points are loaded from a file, 101
CHAPTER 10. MAP GENERATION FOR UAV NAVIGATION and are used to calculate the scale factor and the rotation of the relative reference points. Since applying the scale factor and rotation changes the coordinates of the relative reference points, the off-set is not calculated until the scale factor and rotation has been applied to the relative reference points. Next, the scale factor and rotation are applied to the relative reference points and stored as temporary coordinates. Now the off-set is calculated and applied, and the final GPS waypoints and GPS house coordinates can be stored in a file, which can be used by the UAV. The implementation showing the code is available in Appendix E.
10.4.5 Test of Map Transform Algorithm To verify the correctness of the coordinate transformation, two tests will be conducted. One test to verify the functionality of the algorithm and ensure correctness of the earlier derived equations. A second test to verify the precision of the algorithm and test the quality of the calculated coordinates to determine if they are good enough for navigation of the UAV. The first test is to verify the functionality of the algorithm, and is based on conversion from one set of reference points to another set of reference points, where the relation between the sets of reference points are known in advance. In Figure 10.8 two sets of reference points are shown and from the figure it is shown that the scale factor of the sides between the two triangles are s = 2. It is also obvious from Figure 10.8, that the rotation angle between the two triangles are Θ = π2 rad. As an input to the algorithm the coordinates of r1 , r2 and r3 are given as the relative set of reference points and the coordinates of g1 , g2 and g3 as the the GPS reference points. The scale factor and the rotation calculated by the algorithm in M ATLAB based on the reference points are s = 2 and Θ = −1.5708 rad respectively, which complies with the prediction made before the test. In Figure 10.9, the calculated scale factor and rotation are applied to the relative reference points r1 , r2 and r3 from Figure 10.8, and as shown the two triangles now has the same orientation and scaling. In Figure 10.9 it shown that the coordinate off-set between the reference points are (x, y) = (25, −7) m, which also is the off-set calculated by the transformation algorithm. Figure 10.10 shows the final position of the reference points and only three points are visible. This is because the coordinates of r1 , r2 and r3 now have been transformed into the coordinates of g1 , g2 and g3 , as result of applying the scale factor, rotation, and off-set calculated by use 102
10.4. COORDINATE TRANSFORMATION
15 14 G3
13 12 11 Y position [m]
10 9
G2
G1
8 R1
7 6 5 4 3
R2
R3
2 1 0
5
6
7
8
9
10
11 12 13 14 X position [m]
15
16
17
18
19
20
Figure 10.8: Input points for functionality test.
of the earlier derived equations for minimization.This concludes that the functionality of the algorithm is correct and the scaling, rotation and off-set can be applied to different coordinate sets correctly. Since the waypoint coordinates and house coordinates transformed by the algorithm will be used for parts of the navigation of the UAV, accuracy of the converted coordinates are critical. To test the accuracy, the relative coordinates of the south-west corner of each house in the city, will be converted and compared to the actual the house corner GPS coordinate obtained from The McKenna MOUT Site. The actual GPS coordinates have been provided by Professor Eric Johnson from Georgia Institute of Technology, Atlanta, USA. The three chosen reference points are marked by the three red crosses in Figure 10.11, which are considered to be a realistic placement of reference points. In Table 10.1 the relative and GPS coordinates is shown respectively. Based on the two sets of reference points in Table 10.1 the scale factor, rotation and off-set was calculated to be s = 0.24485, Θ = −0.025262 rad and (x, y) = (704132.940m,13582371.946m)
(10.15) 103
CHAPTER 10. MAP GENERATION FOR UAV NAVIGATION
24 22 R3
20
Y position [m]
18 R2
16 R1 14
G3 12 10 G2 G1
8 6 4 2 0 −20
−16
−12
−8
−4
0 4 8 X position [m]
12
16
20
Figure 10.9: Scale factor and rotation calculated by transformation algorithm applied to the relative reference points.
# 1 2 3
Relative coordinates (448 , 408) (131 , 185) (607 , 134)
GPS coordinates (704240.13 , 13582474.74) (704163.92 , 13582417.91) (704280.57 , 13582408.47)
Table 10.1: Reference points used for test of accuracy
104
24
10.4. COORDINATE TRANSFORMATION
24 22 20
Y position [m]
18 16 14 G3 R3 12 10 G2 R2
R1 G1
8 6 4 2 0
0
2
4
6
8
10 12 14 16 X position [m]
18
20
22
24
Figure 10.10: Scale factor and rotation and off-set calculated by transformation algorithm applied to the relative reference points.
respectively. These results were applied to the relative coordinates of all the south-west corners of the houses, and plotted together with the actual measured GPS coordinates in Figure 10.12. In Figure 10.12, the three connected points represents the reference points, the red stars represents the actual GPS coordinate of the house corners and the blue diamonds represents the converted coordinates. The plot has an offset of (704150, 13582400) in order to improve readability of the values on the x- and y-axes as the GPS coordinates has too many digits. The mean error of all the converted coordinates, compared to all the actual GPS coordinates, is emean = 0.159 m, which is calculated as Euclidean distance. This error is considered to be acceptable for use in any navigation of the UAV since emean = 0.159 m is less than the accuracy of the on-board GPS, and the accuracy of the relative reference points cannot be guaranteed to more than approximately 25 m due to the resolution of the aerial photo. The accuracy of the GPS coordinates supplied by Professor Eric Johnson from Georgia Institute of Technology cannot be guaranteed, but an error of emean = 0.159 m indicates 105
CHAPTER 10. MAP GENERATION FOR UAV NAVIGATION
Figure 10.11: Aerial photo of the McKenna MOUT Site at Fort Benning with placement of reference points.
high accuracy of the coordinates.
106
10.4. COORDINATE TRANSFORMATION
80 Converted coordinates Actual GPS coordinates Connecting ref. points
North position − 13582400 [m]
70 60 50 40 30 20 10 0
0
20
40
60 80 100 East position − 704150 [m]
120
140
Figure 10.12: Transformed house coordinates compared to actual GPS coordinates. The connected points are the reference points for the coordinate transformation. The red start are the converted house coordinates and the blue diamonds are the actual GPS coordinates of the house corners.
107
Generation and Optimization of Level 2 Path
11
For the completion of level 2 the helicopter must be navigated to all of the waypoints in the city shown in Figure 11.1. In each waypoint, the
Figure 11.1: Picture of the waypoints in the city
helicopter must take a set of pictures and scan them for the presence of the marker. The order at which the waypoints are visited has influence on the amount of time spent, if all of the waypoints must be visited, which they must in the worst case scenario. Therefore the selection of the order, at which the waypoints must be visited, is selected by formulating the problem as a combinatorial optimization problem, minimizing the time spent on flying through all waypoints. This problem can be formulated as the traveling salesman problem, which is a classic combinatorial optimization problem. 109
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH In order to formulate the problem, the basics of graph theory is first presented. Afterwards the problem is elaborated, then the solution methods are considered and compared to each other, and lastly the solutions are applied to the waypoints for finding the path.
11.1 Basic Graph Theory As the group has not previously worked with graph theory, this section has been made both in order to introduce the reader to graph theory but also to give the group a basic understanding. A graph G is a finite nonempty set of objects called vertices and a possibly empty set of edges. The set V(G) is called the vertex set of G and E(G) the edge set of G. The order, p, of a graph is defined as the number of vertices while the number of edges, q, is defined as its size. A vertex is also assigned an order defining how many vertices that is adjacent to it, which is how many vertices that is connected to it. Figure 11.2 shows an example of a graph, where the set V(G) = {u, v, u
v
w
Figure 11.2: An example of graph G
w} are the vertices of G and the set E(G) = {{u, v}, {v, w}} are the edges of G. If e = {u, v} is an edge of G, u and v are adjacent to each other and e joins u and w. In Figure 11.2 the graph G has an order of 3 while its size is 2. Furthermore the vertices u, v and v, w are adjacent while w and u are not, which gives the vertex v an order of 2 while the vertices u and w has a order of 1. 110
11.2. DESCRIPTION OF PATH GENERATION PROBLEM The graph G can be described by its corresponding cost matrix
0
M = ev,u ew,u
eu,v eu,w 0 ev,w . ew,v 0
(11.1)
where en,m is the cost for moving from vertix n to m.
11.2 Description of Path Generation Problem As described earlier the problem of finding the shortest path through all the waypoints can be described as the classical traveling salesman problem (TSP), which can be formulated as: Given the cost of travel between each pair of a finite number of cities, the TSP is to find the cheapest path passing through all of the cities and returning to the point of departure That is to find a Hamiltonian path X such that cost(X) =
X
cost(e),
(11.2)
e ∈X
is minimized. In order to solve the problem it is assumed that the cost en,m = em,n ∀ n, m. Furthermore that the path does not need to be a Hamiltonian path, since the helicopter does not need to end in the point it begins in. As described in chapter 10, a set of 178 waypoints was found. This was done by applying a map-scaling algorithm using GPS reference points to the relative coordinates, which were found by analyzing the satellite photo shown in Figure 11.1. From the list of waypoint coordinates the cost matrix, Mdist , based on the distances between the waypoints, can be generated by calculating the cost between all the waypoints, resulting in a symmetric cost matrix of the dimension 178 × 178. After having defined the problem and generated the cost matrix Mdist , sufficient information is collected to find the path through all the waypoints. The path can be found through different methods which is elaborated in the next section. 111
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH
11.3 Methods The path through the waypoints in the city can be found through numerous approaches. An example of these could be an optimal solution giving the optimal path by taking the shortest path of all combinatorial possibilities, or an algorithm giving a suboptimal solution within a few percent of optimality. Algorithms producing a suboptimal solution are better known as heuristics in the study of combinatorial optimization [Graham 1995]. In the following subsections an optimal solution is first considered as a possible method for solving the problem, afterwards a heuristic known as the nearest neighbour is considered, then a optimized nearest neighbour heuristic and lastly a local improvement algorithm known as the 2-OPT improvement heuristic is considered. In each of the sections the algorithm will be demonstrated on the same set of waypoints, which are 10 randomly uniformly distributed waypoints between 0 to 10, defining the cost matrix 0 1.12 2.48 3.37 2.23 Mr = 3.76 3.26 1.89 4.21 3.74
1.12 0 1.36 4.31 3.07 3.10 4.38 2.40 3.91 2.97
2.48 1.36 0 5.54 4.26 2.72 5.74 3.43 3.98 2.44
3.37 4.31 5.54 0 1.30 5.31 2.19 2.26 7.33 7.10
2.23 3.07 4.26 1.30 0 4.13 2.57 1.00 6.39 5.95
3.76 3.10 2.72 5.31 4.13 0 6.48 3.14 6.67 5.13
3.26 4.38 5.74 2.19 2.57 6.48 0 3.41 6.22 6.56
1.89 2.40 3.43 2.26 1.00 3.14 3.41 0 6.08 5.37
4.21 3.91 3.98 7.33 6.39 6.67 6.22 6.08 0 1.92
3.74 2.97 2.44 7.10 5.95 5.13 6.56 5.37 1.92 0 (11.3)
of 10 × 10 elements.
11.3.1 Optimal Solution An optimal solution for the traveling salesman problem is found by choosing the path, of the minimum length, of all the combinatorial possibilities for a path in a given graph. Since all the permutions of the waypoints must be generated to take the shortest path, n! permutions must be generated, where n is the number of waypoints. This method is of course very computational demanding, when having a large number of waypoints, and is impractical for more than about 10 waypoints, since having 11 waypoints yields about 40 million permu112
11.3. METHODS tions of the waypoints. This type of problem is said to be NP-hard, meaning that a solution is not given within polynomial time. The algorithm to find the optimal path is implemented in M ATLAB , and is shown in Appendix D.1. A simulation of the algorithm using graph Mr generates a path as shown in Figure 11.3. Since the Optimal solution 8
#9
Waypoints Optimal route
7
#10 #8
6
#7
Units [⋅]
5 #6 4 #4 #5 3 #3 2 1 0
#1
#2 2
3
4
5 Units [⋅]
6
7
8
Figure 11.3: Shortest path for moving through all waypoints. The path length equals 17.67 units.
number of waypoints in the city totals 178 this method is disregarded as a possible method for finding the shortest path through the waypoints.
11.3.2 Nearest Neighbour Heuristic The nearest neighbour algorithm was one of the first heuristics used for solving the traveling salesman problem, and usually it produces results that are 75% optimal [Rosen 2000]. The principle of the nearest neighbour heuristic is to start at a given waypoint and then choosing the next waypoint as the one lying nearest to the current waypoint. This is done throughout all of the waypoints, producing a path visiting each waypoint exactly once. The nearest neighbour algorithm always makes the current best choice, which is why it is also known as a greedy algorithm. But in most optimizations problems greed does not pay, the greedy solution is not optimal, but can still be used in practice to generate a reasonable solution for a large amount of waypoints [Graham 1995]. The algorithm is 113
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH easy to implement and executes quickly, but the paths generated can easily be optimized when checked with human insight. In the nearest neighbour heuristic, used in this thesis, the algorithm has been modified to generate a path for all waypoints as starting waypoint, and return the shortest path of those. In Appendix D.2, the implementation of the Nearest Neighbour algorithm can be found. A simulation of the algorithm using graph Mr generates a path as shown in Figure 11.4. As can be seen the Nearest Neighbour Heuristic 8
#8 Waypoints NN Route
7
#9 #7
6
#6
Units [⋅]
5 #5 4 #10 #4 3 #3 2 1 0
#1
#2 2
3
4
5 Units [⋅]
6
7
8
Figure 11.4: Solution for moving through all waypoints found by the nearest neighbour heuristic. The path length equals 19.70 units.
path crosses itself, which proves that the solution can be improved to become closer to optimal, since a optimal path never crosses itself [Graham 1995].
11.3.3 Improved Nearest Neighbour Heuristic As mentioned in the previous section, the path generated by the nearest neighbour heuristic can be optimized using human insight. In this section an attempt to optimize the nearest neighbour heuristic is made by making the heuristic less greedier, by looking more than one neighbour forward. Instead, the optimal path between N nearest neighbours are found and the starting waypoint of that path is chosen as the next waypoint. To illustrate this improvement an example is shown in Figure 11.5, where the optimized nearest neighbour, where N = 3, 114
11.3. METHODS Example of NN vs improved NN 3.5
3
Waypoints Optimized NN NN
#3
#2
#5 #5
Units [ ⋅]
2.5
#3 #2
2
1.5 #4 1 0.5
#1
#1 #4 1
1.5
2
2.5 Units [ ⋅]
3
3.5
4
4.5
Figure 11.5: Optimized nearest neighbour (N = 3) versus nearest neighbour. The optimized nearest neighbour path length equals 6.75 units while the nearest neighbour path length equals 7.58.
is shown versus the nearest neighbour. The optimized nearest neighbour starts in waypoint #1, and from there it finds the three nearest waypoints as #2, #3 and #4. It then computes the shortest path between the waypoints #1, #2, #3 and #4, which is #1, #2, #3 and #4, and thus moves to waypoint #2. When moved to waypoint #2 the process is repeated with the next 3 nearest neighbours. If there does not exist N nearest neighbours the remaining neighbours are chosen. This process is repeated until all neighbours has been processed. In this case the result in Figure 11.5 is generated. As Figure 11.5 shows, the path produced by the optimized nearest neighbour is closer to being optimal, than the one produced by the nearest neighbour. In Appendix D.2, the implementation of the algorithm is shown. A simulation of the algorithm using graph Mr generates a path as shown in Figure 11.6. In this case the path has been shortened compared to the nearest neighbour, but still improvements can be made since the path still crosses itself.
11.3.4 2-opt Improvement Heuristic So far the heuristics shown has been based on the nearest neighbour heuristic. As it turns out the nearest neighbour heuristics makes locally the best choices, but potentially globally poor choices. This is 115
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH Improved nearest neighbour heuristic 8
#2
Waypoints INN
7
#1 #3
6
#4
Units [⋅]
5 #5 4 #10 #6 3 #9 2 1 0
#8
#7 2
3
4
5 Units [⋅]
6
7
8
Figure 11.6: Solution for moving through all waypoints found by the improved nearest neighbour (N = 3) heuristic. The path length equals 19.07 units
why these heuristics are classified as construction heuristics [Graham 1995], since they are used for construction of a path by applying a simple rule and not reconsidering the result of the rule. The advantage of this type of path construction is that, a reasonable path can be constructed relatively fast. An example of a poor solution created by the nearest neighbour heuristic was seen in Figure 11.4, where short connections initially were chosen but in the end lead to long steps. As mentioned earlier, the path in Figure 11.4 can easily come closer to being optimal with human insight, for example replacing the edges {#5, #6} and {#5, #9} by the edges {#9, #10} and {#6, #10} results in considerable savings. A heuristic known as the 2-OPT heuristic utilizes the idea of edge exchange. The 2-OPT heuristic does not generate a tour from a given set of waypoints, instead it improves a given tour, T , generated for instance randomly or by another heuristic for example the nearest neighbour. When given T it checks for all pairs of non-adjacent edges {{a, b}, {c, d}} of T whether the path S, is improved by exchanging the two edges with a new edge set {{a, c}, {b, d}} and reversing the edge path order between b and c. This is the case when the edges cross each other as they do in Figure 11.4. If T is improved by exchanging the edges, T is replaced by S, and the exchange test is repeated. 116
11.4. COMPARISON OF HEURISTICS In Appendix D.3, the implementation of the 2-OPT algorithm is shown. A simulation of the algorithm, with the path generated by the nearest neighbour heuristic shown in Figure 11.4 as input, has been carried out. The result, which is shown in Figure 11.7, is as expected an “unwrapping” of the edges crossing each other. In this case the Nearest neighbour route optimized by 2−OPT 8
#7
Waypoints 2−OPT
7
#6 #8
6
#9
Units [⋅]
5 #5 4
#10 #4
3 #3 2 1 0
#1
#2 2
3
4
5 Units [⋅]
6
7
8
Figure 11.7: Solution for moving through all waypoints found by the optimizing the nearest neighbour generated route with the 2-OPT heuristic. The path length equals 17.73 units.
path generated by the nearest neighbour is optimized, and the result does not show obvious further improvements. In the following section the heuristics described throughout this section will be compared to each other in order to select the heuristic to be used for finding the path through the waypoints in the city.
11.4 Comparison of Heuristics In this section the heuristics described in the previous section will be compared to each other. The algorithm providing the optimal solution will be left out, since it as mentioned becomes impractical for a number of waypoints exceeding about 10. Instead of comparing the efficiency of each heuristic relative to the optimal solution, the improvement of each of the heuristics relative to the nearest neighbour heuristic is shown. This improvement is measured as the percentage at which the path is shortened. In order to compare the heuristics, a statistical representation of the result is given. A simulation with 100 iterations is carried out and 117
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH 100 uniformly distributed waypoints are generated and used in each iteration. Three different variants of the modified nearest neighbour is used, ONN3-5, where the number indicates how many waypoints that are “looked forward” by the algorithm. Two different variants of the 2-OPT heuristics is used, where the first, NN2-OPT, is the heuristic applied to the output of the nearest neighbour and the other, RND2OPT, is the heuristic applied to a randomly generated path. The information is collected and the mean and variance of the results are taken, and are shown in Table 11.1. As shown in Table 11.1 the Mean Std. Dev.
ONN3 -1.4060 3.4816
ONN4 -0.7185 3.1935
ONN5 0.2139 3.6608
NN2-OPT 5.7681 2.1457
RND2-OPT 0.2193 0.3058
Table 11.1: Comparation of heuristics using 100 iterations on 100 uniformly distributed random generated waypoints. The results shown is the percentage at which the nearest neighbour generated path is shortened. The complete table can be found in Appendix B.
2-OPT heuristic optimizing the path generated by the nearest neighbour heuristic, shows the greatest improvement of the nearest neighbour generated path. The 2-OPT heuristic based on a path generated by the nearest neighbour heuristic is chosen as the heuristic for generating the path through the waypoints in Figure 11.1, which is done in the next section.
11.5 Path Generation for Level 2 Since the time is the variable that is supposed to be minimized Mdist is an insufficient description of the graph. Instead, a cost matrix, Mtime , based on the time that the helicopter spends on flying between the waypoints would be a better description of the graph since, this includes the dynamics of the helicopter system. In order to find the cost matrix Mtime , a simulation of the helicopter flying all the combinations between the waypoints in the graph is needed. However, since the order of the graph is 178, the number of simulations is 178 X (n − 1) = 15753, (11.4) n=1
118
11.5. PATH GENERATION FOR LEVEL 2 since it is a symmetric case. This simulation is out of scope due to the amount of time it would take to complete the simulation. Instead another approach is selected, where a series of simulations are carried out, where the distance traveled by the helicopter varies from 0m to 10m in steps of 0.1m. The time spent on traveling the distance is collected and can be plotted as a function of the distance resulting in the plot shown in Figure 11.8. The traveling time for distances greater Helicopter time spent to travel distance 7 6.5 6 5.5 5
Time [s]
4.5 4 3.5 3 2.5 2 1.5 1 0.5 0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10 Distance traveled [m]
Figure 11.8: Flying time as a function of the traveled distance. The distance varies from 0m to 10m to display the time progression as a function of the traveled distance. This simulation is performed on the helicopter system with the hover controller.
than 10 m are assumed to progress affine to the distance which is verified in Figure 11.9 by a simulation of the helicopter flying the distances 10 m to 100 m in intervals of 10 m. In order to find Mtime from Mdist , the data in Figure 11.8 needs to be fitted with a function f : d 7→ T,
(11.5)
where T is the time spent on traveling the distance d. To fit the data, the curve in Figure 11.8, is split into six functions where each piece are fitted to the data separately, as shown in Figure 11.10. Each of the function pieces are approximated with a first order polynomial and are found by calculating their slope and intersection with the time 119
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH
Helicopter time spent to travel distance 40
35
Time [s]
30
25
20
15
10
5 10
20
30
40 50 60 70 Distance traveled [m]
80
90
100
Figure 11.9: Flying time as a function of the traveled distance. The distance varies from 10m to 100m in steps of 10m to verify that the progression of time is proportional to the time.
axis. The result is six functions defined as f1 (d) = 0,
x = {0.0, . . . , 0.3}
f2 (d) = 0.275 · x − 0.825,
x = {0.3, . . . , 0.5}
f3 (d) = 0.1011 · x + 0.0446,
x = {0.5, . . . , 5.1}
f4 (d) = −0.065 · x + 8.515,
x = {5.1, . . . , 5.5}
f5 (d) = −0.004 · x + 5.16,
x = {5.5, . . . , 5.6}
f6 (d) = 0.0337 · x + 2.9,
x = {5.6, . . . , ∞},
(11.6)
(11.7)
where (11.7) is assumed to cover for all distances from 5.6 m to ∞ m. These functions are implemented as one function in M ATLAB , from which the cost matrix Mtime can be found by taking f (Mdist ). After having defined Mtime the path through the waypoints in the city can be found. Firstly, the nearest neighbour heuristic is applied to the waypoints which produces the path shown in Figure 11.11. The path found, shown in Figure 11.11, is then given as an input to the 2-OPT optimization heuristic which produces the result shown in Figure 11.12. As shown in Figure 11.12, the path is improved compared to the path found in Figure 11.11. The improvement of the path is 926.29 − 907.98 · 100 = 1.98%. 926.29 120
(11.8)
11.5. PATH GENERATION FOR LEVEL 2
Helicopter time spent to travel distance 7 6.5 6 5.5 5 4.5
Time [s]
4 3.5 3 2.5 2 1.5 1 0.5 0
0
0.5
1
1.5
2
2.5
3
3.5
4 4.5 5 5.5 6 6.5 Distance traveled [m]
7
7.5
8
8.5
9
9.5 10
Figure 11.10: Flying time as a function of the traveled distance with piecewise linearization
A simulation where the helicopter model flies the path found in Figure 11.12 by use of the hover controller, has been carried out. This is done to verify that the helicopter system is able to fly the path and to verify that the time spent is similar to the one found by the calculation of the path generated by the 2-OPT optimization heuristic. When calculating the time spend, the function shown in Figure 11.8 is used. However, when flying the route in the simulation the helicopter is not brought to hover at each waypoint. Instead, when entering a waypoint, the helicopter is told to yaw to a specified heading and then fly towards the next waypoint either flying by the nose or using the heading of the next waypoint, depending on the distance to the next waypoint. The result of the simulation is shown in Figure 11.13. As this figure shows, the time spent on flying the path is 907.94 seconds, which is the same as the calculated path time. This verifies that the flight time is calculated correctly. It is however a coincidence that the flight time simulated has such small deviation from the flight time calculated. It was expected that the two time values would be deviating from each other by maybe a couple of seconds due to the difference in the flight as explained earlier. In the simulation flight, the helicopter was not set to hover in each waypoint. This should make the simulation time 121
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH
Nearest Neighbour Heuristic
Northern position [m] (offset from 13582400)
140
Waypoints NN Route
120 100 80
start
60 40
stop
20 0 0
50 100 150 Eastern position [m] (offset from 704150)
200
Figure 11.11: Path generated by the nearest neighbour heuristic. The time spent on flying the path is 15 minutes and 43 seconds (926.29 seconds).
Nearest Neighbour optimized with 2−OPT Heuristic
Northern position [m] (offset from 13582400)
140
Waypoints NN2−OPT Route
120 100 80
start
60 40
stop
20 0 0
50 100 150 Eastern position [m] (offset from 704150)
200
Figure 11.12: Path generated by the nearest neighbour heuristic optimized with the 2-OPT optimization heuristic. The time spent on flying the path is 15 minutes and 13 seconds (907.98 seconds).
122
11.6. EVALUATION OF GENERATED PATH Simulation of helicopter flying generated path
Northern position [m] (offset from 13582400)
150 Waypoints Route Helic
100 start
50
stop
0 0
50 100 150 Eastern position [m] (offset from 704150)
200
Figure 11.13: Simulation of the helicopter system, with hover controller, flying the path found by the 2-OPT optimization heuristic. The time spent on flying the path is 15 minutes and 13 seconds (907.94 seconds).
smaller than the calculated. But in the simulation flight, the helicopter is set to yaw in each waypoint, and the time it takes to yaw apparently compensates for the time saved by not making the helicopter hover in each waypoint.
11.6 Evaluation of Generated Path After having found the level 2 path, it is interesting to see how close to the optimal path the result is. A program called Concorde[Cook 2006], has been used to find the optimal path through all the level 2 waypoints. The solution produced by Concorde is based on the Euclidean distance between the waypoints, since using a cost matrix based on time is not supported by Concorde. The result is shown in Figure 11.14. To compare the path in Figure 11.14 with the path found in Figure 11.12 the length of the path in Figure 11.12 must be calculated. Even though the route is calculated based on time, the length of it can be calculated by looking up the distances between waypoints in the order the route defines and take the sum of those, which results in a length of 1289.78 meters. If comparing the two distances the path in Figure 11.12 is found to be within 1289.78 − 1237.77 · 100 = 4.2% 1237.77
(11.9) 123
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH Optimal tour found by Concorde 140 North position [m] (offset from 13582400)
start
stop
Waypoints Route
120 100 80 60 40 20 0 0
50 100 150 East position [m] (offset from 704150)
200
Figure 11.14: Optimal path found by the program Concorde. The path length totals 1237.77 meters.
from being optimal. However, here the cost of the path was based on euclidian distances, and thus it might not be within 4.2% of being optimal when considering cost as the time spent on flying the path, as it was done in Figure 11.12. This is only the case if the function in (11.5) is linear, which it clearly is not, when considering Figure 11.10. However, if the distances between the level 2 waypoints is located within the part of the function, f3 in (11.7), it can be assumed linear. To examine if this is the case, a histogram of the distributions of the distances between the level 2 waypoints has been created and is shown in Figure 11.15. As the plot shows, the distances is not located within the linear part of the function, but is distributed over all parts of the function. Since the distribution is as shown in Figure 11.15, the length of the path in Figure 11.12 cannot be compared to the length of the path in Figure 11.14 directly. In order to estimate how far the time path, shown in Figure 11.12, is from optimal, an estimation of the nearest neighbour and 2-OPT distance path-length, has been calculated and are shown in Table 11.2, under the distance column. As the table shows, the results from the heuristics calculated in time are also shown, but the result, xtOPT , from the optimal path, based on time, is not known. In order to estimate xtOPT it is assumed that the factor between the optimization percentages between NN to 2-OPT and 2-OPT to optimal is equal for time and distance. Hence, the optimization percentage from 2-OPT to optimal 124
11.6. EVALUATION OF GENERATED PATH 90 80 70
Number
60 50 40 30 20
0
} } } } } }
0
f1
0.3
f2
0.5
f3
5.1
f4
5.5
f5
5.6
f6
8
10
Distance
Figure 11.15: Bar diagram showing the distribution of the distances between the level 2 waypoints. Note that the intervals of the distribution corresponds to the functions defined in (11.7).
NN NN2-OPT Optimal
Distance 1425.74 1320.98 1237.77
Time 926.29 907.98 xtOPT
Table 11.2: Path lengths for distance and time. NN indicates the Nearest neighbour heuristic and NN2-OPT indicates the nearest neighbour heuristic optimized with the 2-OPT optimization algorithm, and optimal is the optimal solution.
can be found through the expression Od2OPT−OPT Ot2OPT−OPT = OtNN−2OPT OdNN−2OPT
(11.10)
where Ot2OPT−OPT is the optimization percentage from 2-OPT to optimal in time, OtNN−2OPT is the optimization percentage from nearest neighbour to 2-OPT in time, and likewise for Od2OPT−OPT and OdNN−2OPT except that these are in distance. The elements in the above mentioned 125
CHAPTER 11. GENERATION AND OPTIMIZATION OF LEVEL 2 PATH expression can be calculated as 1320.98 − 1237.77 · 100 = 6.3% 1320.98 1425.74 − 1320.98 OdNN−2OPT = · 100 = 7.3% 1425.74 926.29 − 907.98 OtNN−2OPT = · 100 = 1.98%, 926.29
Od2OPT−OPT =
(11.11) (11.12) (11.13)
from which Od2OPT−OPT can be found to Ot2OPT−OPT 6.3% = ⇒ Ot2OPT−OPT = 1.71%. 1.98% 7.3%
(11.14)
From the result in (11.14) xtOPT can be estimated to xtOPT = 907.98 · (1 − 1.71%) = 892.45, which states that the path found in Figure 11.12 is 907.98 − 892.45 · 100 = 98.29% 1− 907.98
(11.15)
(11.16)
optimal under the assumption that the factor between the optimization percentages between NN to 2-OPT and 2-OPT to optimal is equal for time and distance. The achieved percentage in (11.16) comes close to being optimal. When having in mind that the result is found by a heuristic only guaranteed to produce a suboptimal solution, and the calculation cost it would require to compute the optimal solution, gaining only a few percentages, the result is satisfying.
126
Part IV
Conclusion and Perspectives This part contains two chapters, and concludes the project. The first chapter concerns the conclusion of each task in this project individually, and based on this, a final overall conclusion is presented. The second chapter contains considerations in relation to further improvements and perspectives of the project.
127
Conclusion
12
The scope of this project was to fully develop a functional UAV platform, capable of completing level 1 and level 2 of IARC06. In order to achieve this, a mission approach has been selected and based on this, a set of tasks which the group was to complete has been defined. These tasks were elaborated and carried out throughout this thesis as individual applications. To guide the UAV through level 1 and level 2, a navigation system has been developed. This navigation system consists of a navigator able to navigate the helicopter along a path generated from a given set of waypoints, and a mission control which takes the necessary in-flight decisions. The mission control has been fully developed and implemented in real-time. It is able to make decisions based on inputs from both the vision system as well as the navigator. The navigator has also been fully designed, implemented, simulated, and tested in real-time on the ground using the actual UAV and live sensor data. The simulations of the navigation system were used iteratively to make the software perform as was intended in the design. Since the implementation of the navigation system used for simulation is the same as the actual implementation, the grounded tests, which have been performed to prepare the UAV for in-flight tests, all succeeded and showed no abnormalities. Based on the grounded tests of the navigation system it can be concluded that the system is able to create a path from any given set of waypoints and navigate the UAV through this path, which is needed for level 1 flight. Furthermore, it is able to navigate the UAV to a waypoint with a specific yaw attitude and from this position perform house investigation, which is needed for level 2 flight. A tuning of the parameters in the navigator has been carried out to maximize the performance in relation to the specific controller in order to complete level 1 as fast as possible. The result from the tuning of the navigator is somewhat inconclusive, as the available controller was a hover controller and not designed specifically for the fast forward flight necessary for level 1. A vision system for marker and house-entrance recognition has 129
CHAPTER 12. CONCLUSION been developed. Both the framework for the marker and entrance recognition have been implemented, but as the recognition algorithms were to be developed by other people only the marker recognition has been tested as the algorithms for entrance recognition has not yet been developed. During tests the vision subsystem has been able to perform realtime picture acquiring and marker detection in the acquired images if the marker were present. The vision subsystem functions as intended. No performance test of the recognition algorithm were possible as the algorithm is still under development. As an actual map of the McKenna MOUT Site containing GPS coordinates are not available, a map of relative coordinates has been developed. In order to transform the relative coordinates into actual GPS coordinates, a transformation algorithm based on reference-point measurements, has been developed and tested. To minimize the total flight time of level 2, path optimization algorithms have been investigated and applied to generate a near optimal path connecting all the picture waypoints in the city. The transformation of relative coordinates into GPS coordinates has been tested, and the mean error of all the converted test coordinates compared to all the actual GPS test coordinates is emean = 0.159 m calculated as a euclidian distance. Based on the accuracy of the relative reference points, and the on-board GPS sensor, this result is considered acceptable in use for navigation of the UAV. From the generated GPS coordinates of the picture waypoints, a path has been generated using NN and optimized by using the 2-OPT algorithm. Under the assumption that the factor between the optimization percentages between NN and 2-OPT, and the optimization percentage between 2-OPT and optimal, are equal for a time based path and a distance based path, the path was estimated to be 98 % of optimal. Based on the calculation time it would require to calculate the optimal route compared to the calculation time of NN optimized by 2-OPT, the result of 98 % of optimal is considered satisfying. For visual presentation of the helicopter data in real-time during flight, a ground station has been developed. It has been implemented as a server/client communication through WLAN, the server being implemented on the OBC and the client on a ground-station PC. On the client side, a user interface has been implemented to give a visual representation of the flight data in real-time, these data is also logged 130
on the ground station PC in case of a malfunction or crash of the UAV. The ground station was fully designed, implemented and tested, and it has been working as intended and been very useful during test flights and under development of the hardware platform, and testing of all systems running on the UAV. A real-time software environment has been implemented on the UAV for scheduling of in-flight software subsystems. As the basis for the platform, Linux has been chosen, and the hard real-time software development capabilities has been achieved by patching the kernel with RTAI. The real-time software environment functions as intended and has been able to schedule all software subsystems on the UAV, both realtime and non real-time. The development of the UAV hardware platform has included implementation of an OBC, sensors and actuators for navigation and control, and interfacing of sensors and actuators to the OBC. In order to implement the flight electronics on the stock model helicopter, modifications have been necessary these including rearrangement of components and airframe modifications. The hardware platform has been performing satisfying during test flights and the system has gained flight approval according to Danish legislation. The flight electronics and the mechanics of the UAV are still in-progress of development and are being improved continuously. The purpose of this project is considered to be achieved successfully as all the tasks assigned to this project group have been resolved, as presented above, and all systems necessary to perform a level 1 and 2 flight has been developed. In order to actually perform a complete level 1 and 2 flight extensive in-flight testing and tuning are necessary. This has not yet been possible as the UAV at this point is uncontrolled. A controller, and sensor fusion still needs to be implemented.
131
Perspectives
13
The considerations presented in this chapter are based on the intention of completing level 1 and 2 of the IARC06, in a more effective way, than achieved during this project. The areas mentioned here are not the only areas that can be improved on the UAV platform but as they are the only ones that will influence the performance in level 1 and 2 they are the only ones mentioned. In order for the UAV to complete level 1 and 2 at this point, extensive aerial test needs to be conducted beforehand, in order to tune the hardware and software and ensure all systems are functioning as intended. This has not yet been possible due to the lack of an implemented and working controller and sensor fusion on the UAV. An alternative solution for this, could be to implement PID controllers to stabilize the helicopter, and possibly enable the UAV to perform stable hover-like flight. This type of controller will presumably be easier to implement than the intended model-based controller, and this solution would be sufficient in order to conduct aerial tests of the complete UAV platform developed in this project. tt would most likely not produce a very good performance in level 1 and 2 though, since PID controllers probably will provide a limited flight envelope and only be able to keep the UAV stable in hover and slow forward flight. Under the assumption of having a well performing controller implemented on the UAV, it would be possible to tune the navigator to a better performance in level 1 than achieved in the simulations at this point. This would result in the ability to complete level 1 in fast forward flight reducing the flight time. For completion of level 2, a more efficient placement of the picture waypoints could reduce the overall number of waypoints. This would yield less time spend in hover taking pictures, and less time overall spend investigating the houses in the city. An approach for a more efficient way to photograph the houses in the city could be not to take pictures with the camera pointed perpendicular to the house facades, and by this being able to photograph more than one side of a house at the time. Algorithms could be developed to automatic place the waypoints in an efficient way given the house coordinates in advance. 133
CHAPTER 13. PERSPECTIVES Furthermore a camera lens with a wider view angle could also reduce the number of picture waypoints necessary to cover all the house walls of the city. If another approach than waypoint flight was used for completion of level 2, overflying the city in a continuous trajectory could be a possibility. This would mean investigating the houses while the UAV is in motion, which could probably reduce the the level 2 flight time, but it would also introduce more real-time calculations, in order to keep track of which house the UAV is investigation all the time. Also motion blur in the pictures taken would require more real-time image processing capabilities.
134
Bibliography AUVSI [2006], ‘International Aerial Robotics Competition’, Available at http://avdil.gtri.gatech.edu/AUVS/CurrentIARC/ 200xCollegiateRules.html. Bak, T. [2002], ‘Modeling of Mechanical Systems’, Available at http:// www.control.auc.dk/~jan/undervisning/MechanicsI/mechbook. pdf. Lecture note.
Cook, W. [2006], ‘Concorde tsp solver’, Available at http://www.tsp. gatech.edu/concorde.html. Fossen, T. [2002], Marine Control Systems - Guidance, Navigation and Control of Ships, Rigs and Underwater Vehicles, Marine Cybernetics AS. Graham, R. [1995], Handbook of Combinatorics, The MIT Press. Hald, U. B., Hesselbæk, M. & Siegumfeldt, M. [2006], Nonlinear Modeling and Optimal Control of a Miniature Autonomous Helicopter. Rosen, K. H. [2000], Discrete and Combinatorial Mathematics, CRC Press LLC. Tanenbaum, A. S. [2003], Computer networks, Englewood Cliffs : Prentice-Hall.
135
RTAI Linux Platform
A
In this chapter the installation of RTAI Linux is ellaborated. RTAI version 3.2 is limited to only some of the Linux kernel versions. The newest stable version of the Linux kernel which RTAI 3.2 supports is Linux kernel version 2.6.10. The Linux kernel 2.6.10 is downloaded from kernel.org and RTAI 3.2 from rtai.org. The following will describe the compilation and installation of the RTAI kernel on a Debian 3.1 base system. The linux 2.6.10 source and RTAI 3.2 is unpacked: # tar xf linux-2.6.10.tar # tar xf rtai-3.2.tar
For the sake of convenience the two installation sources are placed under /usr/src, and a symbolic link to the two sources are created: # mv rtai-3.2 linux-2.6.10 /usr/src # cd /usr/src # ln -s linux-2.6.10 linux # ln -s rtai-3.2 rtai
Now the Linux source is prepared for the RTAI 3.2 patch, and is done by the following command: # cd linux # patch -p1 < ../rtai/base/arch/i386/patches/hal-linux-2.6.10-386 -r9.patch
After having patched the Linux kernel source with RTAI, the Linux kernel must be configured: # make xconfig
The kernel is configured according to the hardware configuration of the OBC, but furthermore it is important to make sure to • enable Adeos Support, • disable Module Versioning Support under Loadable module support, • disable Frame Pointers under Kernel Hacking and 137
APPENDIX A. RTAI LINUX PLATFORM • disable Use Register Arguments under Processor Type and Features. Afterwards the kernel and the kernel modules are installed: # make # make modules_install
Now the Linux kernel should be installed. Now the modules needed to start RTAI must be compiled. But alike the Linux kernel RTAI must also first be configured: # cd /usr/src/rtai # make xconfig
In this configuration it is important to enable Serial Line Driver under Add-ons since the OBC must be able to write actuator positions out via the serial port. Now the modules can be compiled: # make # make xconfig
This concludes the installation of RTAI Linux.
138
Comparison of Route Generation Heuristics Iteration 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25
ONN3 2.2686 -2.4783 2.2057 -4.8255 1.5689 4.6330 -1.3384 5.0182 -0.5145 0.5366 -2.3045 1.8964 -4.0757 -0.0203 0.6266 -3.5187 0.4878 -6.7569 -6.9693 -2.7500 3.6943 -4.5390 1.7556 3.7860 -1.7838
ONN4 0.7635 -0.6078 -0.5324 1.8308 -0.6748 6.9116 4.8871 3.7203 3.1309 -0.5627 -0.4184 -1.6313 -0.0927 0.0178 -4.1845 1.4881 -2.1848 -6.6896 -7.0670 -1.1595 2.9848 -2.1532 -0.7710 5.5651 -7.8276
ONN5 -0.4045 0.7589 5.0092 -5.3663 0.4063 3.8805 4.6802 -3.1771 4.1303 -1.8952 -4.5582 5.9317 -0.4350 2.0215 2.0078 3.6299 0.6931 -1.3373 -5.8198 -0.1485 1.2493 0.2513 4.4010 8.0006 -1.6096
NN2-OPT 7.1478 5.9860 7.5468 4.1165 5.9643 6.0319 8.4081 7.9880 8.9917 8.1781 3.8841 9.9862 7.1490 7.0036 5.1877 7.7936 4.8939 5.0184 3.6582 4.3719 9.7944 2.9916 5.1154 6.7915 4.5076
B RND2-OPT 0.1902 0.0788 0.9497 -0.1938 -0.0706 0.7321 0.4185 0.2114 0.4266 0.1234 -0.1294 0.8335 -0.0274 0.7814 0.0177 0.6852 0.1752 0.0088 -0.0417 0.2790 0.2897 0.2398 0.4039 0.7018 -0.0806
Table B.1: Comparation of heuristics using 100 iterations on 100 uniformly distributed random generated waypoints. (1 of 4)
139
APPENDIX B. COMPARISON OF ROUTE GENERATION HEURISTICS
Iteration 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50
ONN3 -1.4643 -5.0317 1.2683 -2.8977 2.2627 2.2740 -0.8077 -2.7930 -5.9619 -2.7664 -7.1540 -6.0945 0.7849 -3.0530 -1.4847 0.2755 -1.3045 -0.9277 -5.5451 -3.5152 0.8163 1.6124 1.9072 -2.7408 -6.4654
ONN4 -2.2325 0.2788 2.8922 -3.1440 -4.6292 -1.7784 -2.0803 -6.7780 0.3250 4.5859 -3.9624 -1.1475 1.9284 -1.1151 -5.0939 1.9064 -2.6817 -0.4673 -2.8460 -1.2395 -2.9997 -0.1768 0.9667 -1.0254 1.8575
ONN5 0.4669 -0.6025 4.3623 -2.3347 0.3174 -3.0474 2.3495 -3.8844 0.5812 7.4918 -0.7188 4.4904 -1.4634 -6.7529 -6.6876 3.8621 0.1011 -0.3683 6.3093 -2.9985 2.5245 1.6573 1.4510 -1.3196 -1.6101
NN2-OPT 7.7492 11.9514 7.7337 2.8980 5.7477 4.0642 8.1116 7.0913 5.2792 6.3237 4.4209 4.6466 3.7332 1.7849 4.5762 9.8443 4.9636 3.9977 6.3943 5.3703 8.1257 5.8151 7.2405 5.7851 3.5679
RND2-OPT 0.2093 0.2240 0.3424 -0.3213 0.0270 -0.4372 0.5111 0.6273 0.1078 0.6342 0.6945 0.6013 0.1235 -0.1560 0.0721 0.1907 0.0991 0.5262 0.1450 -0.3617 0.3604 -0.0064 -0.1860 0.2434 0.0899
Table B.2: Comparation of heuristics using 100 iterations on 100 uniformly distributed random generated waypoints. (2 of 4)
140
Iteration 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75
ONN3 -3.3971 -0.9001 0.9950 2.6920 -5.2260 -2.2539 -5.9468 0.6826 -5.5090 -3.1202 -2.3664 1.8163 1.7379 -0.3823 1.8549 -2.9826 -0.8582 -8.8422 -1.4250 -1.8680 -6.0107 -0.5835 -6.1355 1.1973 7.2214
ONN4 -3.3816 4.4139 -7.4932 3.6579 -2.9378 -0.0959 -1.4934 1.7836 0.4802 -3.9991 -0.4084 0.7646 0.5747 -2.6518 -1.4877 -4.1878 -3.3723 -3.1142 -1.6531 -2.4647 -0.5851 0.5072 -3.7597 -5.5528 7.1414
ONN5 1.1144 -0.0838 -2.8547 2.5614 -3.1214 2.3513 -1.8173 4.5687 0.4229 -4.4204 0.0002 0.0127 -0.5757 -1.4321 1.7784 -0.5419 -4.7919 -0.5585 6.2523 -4.3344 1.2972 -6.4801 -3.3029 -4.0355 6.9589
NN2-OPT 6.2735 8.2759 5.6019 5.9733 2.4462 5.1794 5.3150 8.5577 4.2355 1.7964 7.9381 6.4321 6.7532 4.3979 5.3997 4.8134 3.9775 4.3252 6.7910 7.1871 6.5995 6.2643 2.6823 3.4263 11.0421
RND2-OPT 0.1067 0.7495 0.6259 0.3741 -0.3507 0.5049 -0.2166 0.4954 0.2852 -0.3448 0.3641 0.4181 0.1783 0.2798 0.2108 0.0211 -0.2054 -0.1176 0.1227 0.4697 0.6584 -0.2304 0.1685 0.0943 0.6404
Table B.3: Comparation of heuristics using 100 iterations on 100 uniformly distributed random generated waypoints. (3 of 4)
141
APPENDIX B. COMPARISON OF ROUTE GENERATION HEURISTICS
Iteration 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100
ONN3 -6.2389 -1.7959 -1.5116 2.8420 3.4810 -3.1325 -5.6711 1.8672 -3.4701 -4.9446 2.5134 0.6585 -4.6403 -2.1465 -4.3163 0.9274 2.8081 -5.1861 4.0817 3.0022 0.7124 -5.5503 -7.8380 -9.3506 4.1155
ONN4 -5.9736 3.9841 0.0720 -0.5536 1.5119 3.0586 1.2751 -2.8828 -3.7285 -2.0406 0.2799 2.4041 -3.4986 -5.3166 -4.2615 4.5027 3.9372 -5.8424 0.7955 -0.1326 2.6341 -3.0510 -1.5781 -0.1330 4.4781
ONN5 -5.1739 3.7022 2.6360 -1.4661 5.2163 -1.8106 8.1743 -4.7940 2.3192 -5.0695 1.5845 4.0881 -9.0682 1.8183 -0.6770 4.3533 4.1648 -6.3370 2.6481 -0.0087 1.4776 3.3777 -4.5571 -3.6506 3.4257
NN2-OPT 3.7210 4.0854 5.0547 5.5789 6.1107 2.4658 9.6108 3.7482 3.8542 4.0320 7.1635 6.5418 4.3357 6.2674 3.4762 7.8601 7.7427 3.3378 7.7946 6.5463 7.8633 4.6572 2.7751 6.6080 9.7028
RND2-OPT -0.2553 0.4069 0.5837 0.2071 0.1518 0.2581 0.5713 -0.0333 0.1456 -0.2061 0.5294 0.5738 0.3691 0.3088 0.1773 0.5658 0.2463 0.0062 -0.1116 0.1712 0.6096 0.1549 -0.1557 0.3118 0.1126
Table B.4: Comparation of heuristics using 100 iterations on 100 uniformly distributed random generated waypoints. (4 of 4)
142
Helicopter code
C
In this chapter the implementation of all the subsystems running on the helicopter is shown. The different subsystems is divided into sections according to the mission applications.
C.1 Mission Control Code In this section the source code for the mission control subsystem is shown. 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36
# include # include # include # include # include # include # include # include # include
< l i n u x / module . h> < r t a i . h> < r t a i _ s h m . h> < r t a i _ s c h e d . h> < r t a i _ f i f o s . h> < r tai_ n am 2 n u m . h> " . . / include / pl a tf ormd e fi ni ti on s . h" " . . / include / f i fo d e fs . h"
# include " messages . h " # include " . . / vis ion / vis ion . h" # d e f i n e TICK_PERIOD 20000000 # d e f i n e TASK_PRIORITY 3 # d e f i n e STACK_SIZE 10000 s t a t i c RT_TASK m i s s i o n _ c o n t r o l _ t a s k ;
static static static static static
mission_control_navigator_t ∗to_navigator ; navigator_mission_control_t ∗from_navigator ; vi s i on _m i s s i on _c on t rol _ t ∗ from_vision ; mission_control_vision_t ∗ to_vision ; s h ar ed _ m em o r y _ t ∗ shared_memor y ;
s t a t i c in t mc_state ; # include " printmsg . c " v o i d s e n d _ m e s s a g e ( char msg , i n t e n t i t y , char v i s i o n _ m e s s a g e ) { i f ( e n t i t y == ENT_NAVIGATOR ) { / ∗ n a v i g a t o r ∗ / t o _ n a v i g a t o r −>command = msg ; t o _ n a v i g a t o r −> v i s i o n _ r e s u l t = v i s i o n _ m e s s a g e ; t o _ n a v i g a t o r −>new_message = 1 ; p r i n t m s g ( msg , ENT_MISSION_CONTROL, ENT_NAVIGATOR ) ;
143
APPENDIX C. HELICOPTER CODE 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92
144
} i f ( e n t i t y == 2 ) { / ∗ v i s i o n ∗ / t o _ v i s i o n −>command = msg ; t o _ v i s i o n −>new_message = 1 ; p r i n t m s g ( v i s i o n _ m e s s a g e , ENT_MISSION_CONTROL, ENT_VISION ) ; } } /∗ I n i t i a l i z e the v is i on system ∗/ void i n i t _ v i s i o n ( void ) { t o _ v i s i o n −> s t a t e = RUN; t o _ v i s i o n −>a l g o r i t h m _ i d = MARKER; ∗/ t o _ v i s i o n −>c a m _ r a t e = 5 ; 5 Hz ∗ / p r i n t k ( " M i s s i o n C o n t r o l : V i s i o n System }
/ ∗ S e t t o r u n mode ∗ / / ∗ S e l e c t m a r ker mode /∗ S et th e cam_rate to modes s e t \ n " ) ;
/ ∗ The m i s s i o n c o n t r o l t a s k ∗/ s t a t i c void m i s s i o n _ c o n t r o l ( i n t t ) { char n av _ m es s ag e = 0 ; char v i s _ m e s s a g e = 0 ; float vision_result = 0.0; f l o a t t h r e s h o l d = VISION_THRESHOLD ; i n t h o u s en u m b er = −1; i n t timestamp = 0; t o _ n a v i g a t o r −>new_message = 0 ; t o _ v i s i o n −>new_message = 0 ; init_vision () ; p r i n t k ( " M i s s i o n C o n t r o l : S t a r t OK\ n " ) ; while ( 1 ) { / ∗ R e c e i v e m es s a g e fr o m n a v i g a t o r ∗ / i f ( f r o m _ n a v i g a t o r −>new_message == 1 ) { n av _ m es s ag e = f r o m _ n a v i g a t o r −>m es s ag e ; h o u s en u m b er = f r o m _ n a v i g a t o r −>h o u s en u m b er ; f r o m _ n a v i g a t o r −>new_message = 0 ; } / ∗ R e c e i v e m es s a g e fr o m v i s i o n ∗ / i f ( f r o m _ v i s i o n −>new_message == 1 ) { v i s _ m e s s a g e = f r o m _ v i s i o n −>m es s ag e ; v i s i o n _ r e s u l t = f r o m _ v i s i o n −>mean ; f r o m _ v i s i o n −>new_message = 0 ; } switch ( mc_state ) { case 0: / ∗ W a it 20 s e c s b e f o r e h e l i c o p t e r i s r e a d y ∗ / i f ( shared_memory −>s am p len u m b er > 1 0 0 0 ) m c _ s t a t e ++;
C.1. MISSION CONTROL CODE 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145
break ; case 1: /∗ I n i t i a l i z e n a vig a to r f o r l e v e l 1 f l i g h t ∗/ s e n d _ m e s s a g e ( INIT_LEVEL_2 , ENT_NAVIGATOR , NULL_MSG) ; m c _ s t a t e ++; break ; c a s e 2 : / ∗ W a it u n t i l n a v i g a t o r h a s b een i n i t i a l i z e d ∗ / i f ( n av _ m es s ag e == INIT_LEVEL_2_OK) { mc_state =5; } break ; c a s e 3 : / ∗ Run l e v e l 1 ∗ / s e n d _ m e s s a g e ( LEVEL_1 , ENT_NAVIGATOR , NULL_MSG) ; m c _ s t a t e ++; break ; c a s e 4 : / ∗ W a it u n t i l n a v i g a t o r h a s c o m p l e t e d l e v e l 1 ∗ / i f ( n av _ m es s ag e == LEVEL_1_DONE) { s e n d _ m e s s a g e (HOVER, ENT_NAVIGATOR , NULL_MSG) ; / ∗ S e t h e l i c o p t e r to hover ∗/ m c _ s t a t e ++; } break ; c a s e 5 : / ∗ Run l e v e l 2 ∗ / s e n d _ m e s s a g e ( LEVEL_2 , ENT_NAVIGATOR , NULL_MSG) ; m c _ s t a t e ++; break ; c a s e 6 : / ∗ W a it u n t i l n a v i g a t o r h a s r e a c h e d a p i c t u r e w a y p o i n t and s t a r t t h e v i s i o n s y s t e m ∗ / i f ( n av _ m es s ag e == INSIDE_WAYPOINT) { / ∗ Ask v i s i o n s y s t e m t o t a k e p i c t u r e s ∗ / / ∗ s e n d _ m e s s a g e ( SCAN_FOR_MARKER, ENT_VISION , NULL_MSG ) ; ∗/ t i m e s t a m p = shared_memory −>s am p len u m b er ; m c _ s t a t e ++; } break ; c a s e 7 : / ∗ W a it u n t i l t h e v i s i o n s y s t e m r e p o r t s t h a t t h e scan i s completed ∗/ / ∗ HACK : u s e d f o r t e s t i n g l e v e l 2 f l i g h t ∗ / i f ( shared_memory −>s am p len u m b er > ( t i m e s t a m p + 1 0 0 0 ) ) { s e n d _ m e s s a g e ( LEVEL_2 , ENT_NAVIGATOR , MARKER_NOT_FOUND ) ; mc_state = 10; /∗ send to th e t e s t s t a t e ∗/ } / ∗ The r e a l d e a l ∗ / i f ( v i s _ m e s s a g e == SCAN_DONE ) { if ( vision_result > threshold ) { / ∗ T e l l t h a t t h e m a r ker was f o u n d t o t h e n a v i g a t o r ∗ / s e n d _ m e s s a g e ( LEVEL_2 , ENT_NAVIGATOR , MARKER_FOUND) ; m c _ s t a t e ++; } else {
145
APPENDIX C. HELICOPTER CODE 146
/ ∗ T e l l t h a t t h e m a r ker was n o t f o u n d t o t h e n a v i g a t o r ∗/ s e n d _ m e s s a g e ( LEVEL_2 , ENT_NAVIGATOR , MARKER_NOT_FOUND ) ; mc_state = 6;
147 148 149 150 151 152 153 154 155 156 157 158 159 160
} } break ; case 8: mc_state = 12; break ; c a s e 1 0 : / ∗ t h e same a s s i x ( u s e d f o r t e s t ) ∗ / i f ( n av _ m es s ag e == INSIDE_WAYPOINT) { / ∗ Ask v i s i o n s y s t e m t o t a k e p i c t u r e s ∗ / / ∗ s e n d _ m e s s a g e ( SCAN_FOR_MARKER, ENT_VISION , NULL_MSG ) ; ∗/ t i m e s t a m p = shared_memory −>s am p len u m b er ; m c _ s t a t e ++; } break ;
161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198
146
case 11: /∗ used f o r t e s t ∗/ i f ( shared_memory −>s am p len u m b er > ( t i m e s t a m p + 1 0 0 0 ) ) { s e n d _ m e s s a g e ( LEVEL_2 , ENT_NAVIGATOR , MARKER_FOUND) ; mc_state = 10; /∗ send to th e t e s t s t a t e ∗/ } break ; case 12: i f ( n av _ m es s ag e == LEVEL_2_DONE_FOUND) { p r i n t k ( " Found m ar k er on h o u s e : %d " , h o u s en u m b er ) ; } break ; default : p r i n t k ( " M i s s i o n C o n t r o l : ERROR ! S e t t i n g h e l i c o p t e r t o hover \ n " ) ; s e n d _ m e s s a g e (HOVER, ENT_NAVIGATOR , NULL_MSG) ; break ; } rt_task_wait_period () ; } } / ∗ Module i n i t i a l i z a t i o n s ∗/ i n t init_m odule ( void ) { /∗ Setup the t i c k period ∗/ RTIME t i c k _ p e r i o d ; / ∗ Make i t p e r i o d i c ∗ / rt_set_periodic_mode () ; /∗ I n i t i a l i z e the task ∗/ r t _ t a s k _ i n i t (& m i s s i o n _ c o n t r o l _ t a s k , m i s s i o n _ c o n t r o l , 1 , STACK_SIZE , TASK_PRIORITY , 1 , 0 ) ;
C.1. MISSION CONTROL CODE 199 200 201 202 203
p r i n t k ( " Mission Control : E n t i t y i n i t i a l i z e d \ n" ) ; / ∗ A t t a c h t o t h e s h a r e d memory ∗ / shared_memor y = r t a i _ k m a l l o c ( nam2num ( SharedMemId ) , s i z e o f ( s h ar ed _ m em o r y _ t ) ) ;
204 205 206
/ ∗ C r e a t e m es s a g e i n t e r f a c e s ∗ / f r o m _ v i s i o n = r t a i _ k m a l l o c ( nam2num ( "VIMCSM" ) , s i z e o f ( vision_mission_control_t ) ) ; t o _ v i s i o n = r t a i _ k m a l l o c ( nam2num ( "MCVISM" ) , s i z e o f ( mission_control_vision_t ) ) ; f r o m _ n a v i g a t o r = r t a i _ k m a l l o c ( nam2num ( "PCSVSM" ) , s i z e o f ( navigator_mission_control_t ) ) ; t o _ n a v i g a t o r = r t a i _ k m a l l o c ( nam2num ( "SVPCSM" ) , s i z e o f ( mission_control_navigator_t ) ) ;
207 208 209 210 211 212
i f ( ( f r o m _ v i s i o n ! = NULL) &( t o _ n a v i g a t o r ! = NULL) &( f r o m _ n a v i g a t o r ! = NULL) &( t o _ v i s i o n ! = NULL) ) p r i n t k ( " M i s s i o n C o n t r o l : IPC ’ s OK\ n " ) ;
213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243
/∗ Set i n i t i a l task ∗/ mc_state =0; /∗ S t a r t the task ∗/ t i c k _ p e r i o d = s t a r t _ r t _ t i m e r ( n a n o 2 c o u n t ( TICK_PERIOD ) ) ; r t _ t a s k _ m a k e _ p e r i o d i c (& m i s s i o n _ c o n t r o l _ t a s k , r t _ g e t _ t i m e ( ) + tick_period , tick_period ) ; return 0; } / ∗ Module c l e a n u p s when r e m o v i n g t h e module void cleanup_module ( void ) { /∗ Stop the tim er ∗/ stop_rt_timer () ;
∗/
/∗ D elete our r e a l tim e t a s k ∗/ r t _ t a s k _ d e l e t e (& m i s s i o n _ c o n t r o l _ t a s k ) ; / ∗ F r ee t h e a l l o c a t e d memory ∗ / r t a i _ k f r e e ( nam2num ( "VIMCSM" ) ) ; r t a i _ k f r e e ( nam2num ( "PCMHSV" ) ) ; r t a i _ k f r e e ( nam2num ( "SVPCSM" ) ) ; r t a i _ k f r e e ( nam2num ( "MCVISM" ) ) ; p r i n t k ( " M i s s i o n C o n t r o l : E n t i t y s h u t down \ n " ) ; return ; } MODULE_LICENSE( "GPL" ) ;
147
APPENDIX C. HELICOPTER CODE
C.2 Navigator Code In this section the implementation of the navigator can be found. It is divided into four subsections. The path planner and path follower subsections are the navigator itself while the real-time wrapper is to be able to use the navigator on the helicopter and the simulation wrapper is to be able to simulate the navigator in simulink.
C.2.1 Real-time Wrapper for Navigator 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44
148
# include # include # include # include # include # include # include # include
< l i n u x / k e r n e l . h> < l i n u x / module . h> < r t a i . h> < r t a i _ s h m . h> < r t a i _ s c h e d . h> < r tai_ n am 2 n u m . h>
# include " . . / include / p l a t f o r m d e f i n i t i o n s . h" # include " messages . h " / ∗ RTAI # define # define # define
s p e c i f i c t ask s ∗/ TICK_PERIOD 200000000 TASK_PRIORITY 2 STACK_SIZE 10000
/ ∗ T a s k name ( u s e d f o r RTAI ) ∗ / s t a t i c RT_TASK n a v i g a t o r _ t a s k ; / ∗ P o i n t e r t o s h a r e d memory s t r u c t ∗ / s t a t i c s h ar ed _ m em o r y _ t ∗ shared_memor y ; / ∗ Memories u s e d f o r IPC ∗ / s ta t i c mission_control_navigator_t ∗from_mission_control ; static navigator_mission_control_t ∗to_mission_control ; static navigator_controller_t ∗to_controller ; static controller_navigator_t ∗from_controller ; /∗ Implementation of # include " navigator / # include " navigator / # include " navigator / # include " navigator /
navigator f un c t i o ns ∗/ navigator . h" functions . c" path_follower . c" path_planner . c"
/ ∗ P r i n t m es s a g e t o k e r n e l l o g ∗ / # include " printmsg . c " / ∗ S t r u c t u s e d when s e n d i n g m e s s a g e s t o c o n t r o l l e r ∗ / typedef struct { short i n t used ; d o u b le v _ r e f [ 3 ] ; d o u b le y a w _ r e f ; } controller_interface_t ;
C.2. NAVIGATOR CODE 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82
83 84 85 86 87 88 89 90 91 92 93 94
v o i d s e n d _ m e s s a g e ( char msg , i n t e n t i t y , c o n t r o l l e r _ i n t e r f a c e _ t con , i n t h o u s en u m b er ) { i f ( e n t i t y == ENT_MISSION_CONTROL) { / ∗ m i s s i o n c o n t r o l ∗ / t o _ m i s s i o n _ c o n t r o l −>m es s ag e = msg ; i f ( ! ( h o u s en u m b er == −1) ) t o _ m i s s i o n _ c o n t r o l −>h o u s en u m b er = h o u s en u m b er ; t o _ m i s s i o n _ c o n t r o l −>new_message = 1 ; p r i n t m s g ( msg , ENT_NAVIGATOR , ENT_MISSION_CONTROL) ; / ∗ Send msg fr o m n a v i g a t o r t o m i s s i o n c o n t r o l ∗ / } i f ( ( e n t i t y == ENT_CONTROLLER) && ( con . u s e d == 1 ) ) { / ∗ Controller ∗/ t o _ c o n t r o l l e r −> r e f e r e n c e [ 0 ] = con . v _ r e f [ 0 ] ; t o _ c o n t r o l l e r −> r e f e r e n c e [ 1 ] = con . v _ r e f [ 1 ] ; t o _ c o n t r o l l e r −> r e f e r e n c e [ 2 ] = con . v _ r e f [ 2 ] ; t o _ c o n t r o l l e r −> r e f e r e n c e [ 3 ] = con . y a w _ r e f ; t o _ c o n t r o l l e r −>new_message = 1 ; p r i n t m s g (NULL_MSG , ENT_NAVIGATOR , ENT_CONTROLLER) ; } } / ∗ The N a v i g a t o r t a s k ∗/ s t a t i c void n a v i g a t o r ( i n t t ) { char v i s i o n _ r e s u l t =NULL_MSG; i n t new_message =0 , m e s s a g e _ s e n t = 0 ; i n t l e v e l _ 1 _ i n i t _ d o n e =0 , l e v e l _ 2 _ i n i t _ d o n e =0 , command=HOVER, p p _ s t a t e =PP_ONGOING , l e v e l = 0 ; c o n t r o l l e r _ i n t e r f a c e _ t con ; i n t h o u s en u m b er =−1; pp_output_t pp_outputs ; i n t hover = 0 , nextwpid =0; d o u b le p o s _ c u r r [ 5 ] = { 1 . 0 , 2 . 0 , 3 . 0 , 4 . 0 , 5 . 0 } ; i n t cnt =0; /∗ Fakies ∗/ d o u b le w a y p o i n t s [NUMBER_OF_WPS ] = { 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 6 5 0 7 7 5 . 8 8 , 6 3 2 1 5 2 2 . 9 4 , 1 1 , ( M_PI / 2 . 0 ) , 1 3 , 6 5 0 8 2 1 . 9 5 , 6 3 2 1 5 2 3 . 2 8 , 1 1 , M_PI , 1 4 , 6 5 0 8 2 1 . 9 6 , 6 3 2 1 5 6 4 . 8 7 , 1 1 , ( M_PI / 4 . 0 ) , 1 5 } ; i n t num_of_wps = 3 ; h o u s en u m b er = −1; con . u s e d = 0 ; w h i l e ( 1 ) / ∗ Do f o r e v e r ∗ / { / ∗ R e c e i v e m es s a g e fr o m m i s s i o n c o n t r o l ∗ / i f ( f r o m _ m i s s i o n _ c o n t r o l −>new_message == 1 ) { command = f r o m _ m i s s i o n _ c o n t r o l −>command ; v i s i o n _ r e s u l t = f r o m _ m i s s i o n _ c o n t r o l −> v i s i o n _ r e s u l t ; f r o m _ m i s s i o n _ c o n t r o l −>new_message = 0 ;
149
APPENDIX C. HELICOPTER CODE 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144
150
new_message = 0 ; } s w i t c h ( command ) { c a s e INIT_LEVEL_1 : i f ( l e v e l _ 1 _ i n i t _ d o n e == 0 ) { / ∗ R e p l a c e t h i s w i t h r e a d i n g s fr o m s e n s o r f u s i o n when i t s ready ∗/ w a y p o i n t s [ 0 ] = shared_memory −>GPS . UTMe ; w a y p o i n t s [ 1 ] = shared_memory −>GPS . UTMn; / ∗ w a y p o i n t s [ 2 ] = shared_memory −>GPS . A l t i t u d e ; ∗ / w a y p o i n t s [ 2 ] = 1 1 . 0 ; / ∗ HACK : Used f o r t e s t ∗ / w a y p o i n t s [ 3 ] = 0 . 0 ; / ∗ Must be i n c l u d e d b u t i s n o t u s e d ∗/ w a y p o i n t s [ 4 ] = 0 . 0 ; / ∗ Must be i n c l u d e d b u t i s n o t u s e d ∗/ l e ve l = 1; hover = 0; nextwpid = 1; / ∗ Send i n i t l e v e l 1 OK t o M i s s i o n C o n t r o l ∗ / s e n d _ m e s s a g e ( INIT_LEVEL_1_OK , ENT_MISSION_CONTROL, con , h o u s en u m b er ) ; level_1_init_done = 1; } break ; c a s e INIT_LEVEL_2 : i f ( l e v e l _ 1 _ i n i t _ d o n e == 0 ) { / ∗ R e p l a c e t h i s w i t h r e a d i n g s fr o m s e n s o r f u s i o n when i t s ready ∗/ w a y p o i n t s [ 0 ] = shared_memory −>GPS . UTMe ; w a y p o i n t s [ 1 ] = shared_memory −>GPS . UTMn; / ∗ w a y p o i n t s [ 2 ] = shared_memory −>GPS . A l t i t u d e ; ∗ / w a y p o i n t s [ 2 ] = 1 1 . 0 ; / ∗ HACK : Used f o r t e s t ∗ / w a y p o i n t s [ 3 ] = 0 . 0 ; / ∗ Must be i n c l u d e d b u t i s n o t u s e d ∗/ w a y p o i n t s [ 4 ] = 0 . 0 ; / ∗ Must be i n c l u d e d b u t i s n o t u s e d ∗/ l e ve l = 2; hover = 0; nextwpid = 1; inside_wp = 0; / ∗ Send i n i t l e v e l 2 OK t o M i s s i o n C o n t r o l ∗ / s e n d _ m e s s a g e ( INIT_LEVEL_2_OK , ENT_MISSION_CONTROL, con , h o u s en u m b er ) ; level_2_init_done = 1; } break ; c a s e HOVER:
C.2. NAVIGATOR CODE 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196
/ ∗ Send 0 . 0 r e f e r e n c e s t o c o n t r o l l e r ∗ / con . u s e d = 1 ; f o r ( c n t = 0 ; c n t GPS . UTMe ; p o s _ c u r r [ 1 ] = shared_memory −>GPS . UTMn; / ∗ p o s _ c u r r [ 2 ] = shared_memory −>GPS . A l t i t u d e ; ∗ / p o s _ c u r r [ 2 ] = 1 1 . 0 ; / ∗ HACK : u s e d f o r t e s t ∗ / /∗ Call the path planner ∗/ p a t h _ p l a n n e r (& p p _ o u t p u t s , w a y p o i n t s , num_of_wps , p o s _ c u r r , &n ex tw p id , &h o v er , l e v e l , NULL_MSG) ; / ∗ Send r e f e r e n c e s t o c o n t r o l l e r ∗ / con . u s e d = 1 ; / ∗ C o n t r o l l e r m e s s a g e s u s e d ∗ / f o r ( c n t = 0 ; c n t GPS . UTMe ; p o s _ c u r r [ 1 ] = shared_memory −>GPS . UTMn; / ∗ p o s _ c u r r [ 2 ] = shared_memory −>GPS . A l t i t u d e ; ∗ / p o s _ c u r r [ 2 ] = 1 1 . 0 ; / ∗ HACK : u s e d f o r t e s t ∗ / /∗ Call the path planner ∗/ p a t h _ p l a n n e r (& p p _ o u t p u t s , w a y p o i n t s , num_of_wps , p o s _ c u r r , &n ex tw p id , &h o v er , l e v e l , v i s i o n _ r e s u l t ) ; / ∗ Send r e f e r e n c e s t o c o n t r o l l e r ∗ / con . u s e d = 1 ; / ∗ C o n t r o l l e r m e s s a g e s u s e d ∗ / f o r ( c n t = 0 ; c n t h o u s e_ n u m b er = −1; / ∗ I n i t i a l i z e h o u s e number t o a nonvalid value ∗/ f o r ( i = 0 ; i v _ o u t , ∗ h o v er , p o s _ c u r r ) ; # endif / ∗ C a l c u l a t e d i s t a n c e fr o m h e l i t o n e x t wp ∗ / f o r ( i = 0 ; i v _ o u t ) −(M_PI / 2 ) ; } # i f d e f GRB_SIMULATION / ∗ Used f o r s i m u l a t i o n ∗ / i f ( length_v_next < i n n e r _ c i r c l e ) { /∗ True i f h e l i i s i n s i d e wp ∗ / # e l s e / ∗ Used on h e l i c o p t e r ∗ / i f ( length_v_next s t a t e =PP_LEVEL_1_DONE; } else { ∗ next_wp_ID =∗ next_wp_ID + 1 ; # i f d e f GRB_SIMULATION p r i n t f ( " Waypoint r e a c h e d : %d , h e a d i n g t o w a r d s : %d \ n " , ∗ next_wp_ID −1,∗ next_wp_ID ) ; # e n d i f / ∗ GRB_SIMULATION ∗ / } } } /∗ Level 2 ∗/ i f ( l e v e l ==2) {
f o r ( i = 0 ; i i n n e r _ c i r c l e ) { /∗ True i f h e l i i s o u t s i d e waypoint c i r c l e ∗/ # else i f ( length_v_next >min_wp_dist ) { /∗ True i f h e l i i s o u t s i d e waypoint c i r c l e ∗/ # endif ∗ hover =0; } e l s e { / ∗ T r u e i f h e l i i s i n s i d e wp c i r c l e ∗ / temp_yaw= w a y p o i n t s [ ( ∗ next_wp_ID ) ∗ w p _ l e n g t h + 3 ] ; ∗ hover =1; o u t p u t −> s t a t e =PP_LEVEL_2_INSIDE_WP ; s w i t c h ( msg ) { c a s e NULL_MSG : inside_wp =1; break ; c a s e MARKER_FOUND : inside_wp =0; h o u s e_ n u m b er = w a y p o i n t s [ ( ∗ next_wp_ID ) ∗ w p _ l e n g t h + 4 ] ; o u t p u t −> s t a t e =PP_LEVEL_2_DONE_FOUND; o u t p u t −>h o u s e_ n u m b er = h o u s e_ n u m b er ; break ; c a s e MARKER_NOT_FOUND : inside_wp =0; i f ( ∗ next_wp_ID ==( num_of_wps ) ) { ∗hover =1; o u t p u t −> s t a t e =PP_LEVEL_2_DONE_NOT_FOUND; } else { ∗hover =0; ∗ next_wp_ID =∗ next_wp_ID + 1 ; o u t p u t −> s t a t e =PP_ONGOING ; # i f d e f GRB_SIMULATION p r i n t f ( " Waypoint r e a c h e d : %d , h e a d i n g t o w a r d s : %d \ n " , ∗ next_wp_ID −1,∗ next_wp_ID ) ; # e n d i f / ∗ GRB_SIMULATION ∗ /
159
APPENDIX C. HELICOPTER CODE 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189
160
} break ; default : ∗hover =1; # i f d e f GRB_HELICOPTER p r i n t k ( "WARNING: p a t h _ p l a n n e r r e c e i v e d wrong msg ! S e t t i n g h e l i c o p t e r to hover \ n " ) ; # endif break ; } } } / ∗ Yaw m u s t be unwrapped ∗ / # i f d e f GRB_NAV_DEV p r i n t f ( "Yaw : %f \ n " , o u t p u t −>yaw ) ; p r i n t f ( " temp_Yaw : %f \ n " , temp_yaw ) ; # endif w h i l e ( temp_yaw (2∗ M_PI ) ) { temp_yaw=temp_yaw −(2∗M_PI ) ; } yaw_wrap ( temp_yaw , &( o u t p u t −>yaw ) ) ; # i f d e f GRB_NAV_DEV p r i n t f ( "Yaw1 : %f \ n " , o u t p u t −>yaw ) ; p r i n t f ( " temp_Yaw1 : %f \ n " , temp_yaw ) ; # endif
# i f d e f GRB_HELICOPTER / ∗ V a r i a b l e s w h ich m u s t be s e n d t o g r o u n d s t a t i o n when debugging ∗/ shared_memory −> n a v i g a t o r . y a w _ r e f = o u t p u t −>yaw ; shared_memory −> n a v i g a t o r . next_wp_ID = ∗ next_wp_ID ; shared_memory −> n a v i g a t o r . h o v e r = ∗ h o v e r ; shared_memory −> n a v i g a t o r . d i s t _ n e x t _ w p = l e n g t h _ v _ n e x t ; shared_memory −> n a v i g a t o r . p o s _ c u r r [ 0 ] = p o s _ c u r r [ 0 ] ; shared_memory −> n a v i g a t o r . p o s _ c u r r [ 1 ] = p o s _ c u r r [ 1 ] ; shared_memory −> n a v i g a t o r . p o s _ c u r r [ 2 ] = p o s _ c u r r [ 2 ] ; shared_memory −> n a v i g a t o r . v _ o u t [ 0 ] = o u t p u t −>v _ o u t [ 0 ] ; shared_memory −> n a v i g a t o r . v _ o u t [ 1 ] = o u t p u t −>v _ o u t [ 1 ] ; shared_memory −> n a v i g a t o r . v _ o u t [ 2 ] = o u t p u t −>v _ o u t [ 2 ] ; shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r
. prev_wp [ 0 ] . prev_wp [ 1 ] . prev_wp [ 2 ] . prev_wp [ 3 ]
= = = =
wp_last wp_last wp_last wp_last
[0]; [1]; [2]; [3];
C.2. NAVIGATOR CODE 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235
shared_memory −> n a v i g a t o r . prev_wp [ 4 ] = w p _ l a s t [ 4 ] ; shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r shared_memory −> n a v i g a t o r
. next_wp [ 0 ] . next_wp [ 1 ] . next_wp [ 2 ] . next_wp [ 3 ] . next_wp [ 4 ]
= = = = =
wp_next wp_next wp_next wp_next wp_next
[0]; [1]; [2]; [3]; [4];
shared_memory −> n a v i g a t o r . n a v _ s t a t e = o u t p u t −> s t a t e ; shared_memory −> n a v i g a t o r . l e v e l = l e v e l ; # endif
# i f d e f GRB_NAV_DEV p r i n t f ( " o u t p u t −>v _ o u t i s : [% f %f %f ] \ n " , o u t p u t −>v _ o u t [ 0 ] , o u t p u t −>v _ o u t [ 1 ] , o u t p u t −>v _ o u t [ 2 ] ) ; p r i n t f ( " w p _ l a s t i s : [% f %f %f %f %f ] \ n " , w p _ l a s t [ 0 ] , w p _ l a s t [1] , wp_last [2] , wp_last [3] , wp_last [ 4 ] ) ; p r i n t f ( " wp_next i s : [% f %f %f %f %f ] \ n " , wp_next [ 0 ] , wp_next [ 1 ] , wp_next [ 2 ] , wp_next [ 3 ] , wp_next [ 4 ] ) ; p r i n t f ( " y a w _ r e f i s : [% f ] \ n " , o u t p u t −>yaw ) ; # e n d i f / ∗ GRB_NAV_DEV ∗ / } # i f d e f GRB_NAV_DEV i n t main ( v o i d ) { d o u b le w a y p o i n t s [ 1 5 ] = { 0 , 0 , 0 , 1 , 4 , 1 , 1 , 0 , 8 , 9 , 1 0 , 1 1 , 1 2 , 1 3 , 1 4 } ; pp_output_t output ; i n t num_of_wps = 2 ; d o u b le p o s _ c u r r [ 3 ] = { 0 . 5 , 0 . 5 , 0 } ; i n t next_wp_ID = 1 ; i n t hover =0; i n t l e v e l =1; i n t msg= m s g _ n o t _ v a l i d ; o u t p u t . yaw = 2 5 . 2 2 ;
p a t h _ p l a n n e r (& o u t p u t , w a y p o i n t s , num_of_wps , p o s _ c u r r , & next_wp_ID , &h o v er , l e v e l , msg ) ; /∗ /∗ /∗
p r i n t f ( " Hover : %d \ n " , h o v e r ) ; ∗ / p r i n t f ( " W a yp o in t number : %d \ n " , n ext_ w p _ I D ) ; ∗ / p r i n t f ( " Yaw : %f \ n " , o u t p u t . yaw ) ; ∗ /
return 0; } # endif
C.2.4 Path Follower 1 2 3 4 5
# i f n d e f GRB_HELICOPTER # i n c l u d e < s t d i o . h> # i n c l u d e # i n c l u d e < s t d l i b . h>
161
APPENDIX C. HELICOPTER CODE 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55
162
# include " path_follower . h" # include " navigator . h" # include " f unc t io ns . h" # e n d i f / ∗ HELICOPTER ∗ / # i f d e f GRB_SIMULATION v o i d p a t h _ f o l l o w e r ( d o u b le ∗ w p _ l a s t , d o u b le ∗ wp_next , d o u b le ∗ v _ o u t , i n t h o v er , d o u b le ∗ p o s _ c u r r , f l o a t o u t e r _ c i r c l e ) # else v o i d p a t h _ f o l l o w e r ( d o u b le ∗ w p _ l a s t , d o u b le ∗ wp_next , d o u b le ∗ v _ o u t , i n t h o v er , d o u b le ∗ p o s _ c u r r ) # endif { i n t i =0; d o u b le v _ c o u r s e [ 3 ] , v _ p o s [ 3 ] , v _ n e x t [ 3 ] , v _ n e x t _ u n i t [ 3 ] , cross_p [3] , v_cross [3] , v_cross_unit [ 3 ]; d o u b le l e n g t h _ c r o s s _ p , l e n g t h _ v _ c o u r s e , l e n g t h _ v _ n e x t , length_v_cross , length_v_pos =0.0; d o u b le e _ c t = 0 . 0 ; / ∗ Cross −t r a c k e r r o r ∗ / d o u b le r o t a t i o n _ a m o u n t = 0 ; / ∗ amount v _ n e x t m u s t be r o t a t e d i f t h e h e l i d e v i a t e s [ rad ] ∗/ / ∗ I f h o ver , v _ o u t = [ 0 , 0 , 0 ] ∗ / i f ( ( h o v e r ==1) ) { v_out [0]=0; v_out [1]=0; v_out [2]=0; } else { / ∗ V e c t o r fr o m l a s t w a y p o i n t t o n e x t w a y p o i n t ( c o u r s e v e c t o r ) ∗/ f o r ( i = 0 ; i n a v i g a t o r . d i s t _ p r e v _ w p = l e n g t h _ v _ p o s ; shared_memory −> n a v i g a t o r . t h e t a _ h e l m = r o t a t i o n _ a m o u n t ; # endif }
C.3. CONTROLLER CODE
C.3 Controller Code This section contains the code in which the controller is to be implmented. 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51
# include # include # include # include # include # include # include
< l i n u x / module . h> < r t a i . h> < r t a i _ s h m . h> < r t a i _ s c h e d . h> < r t a i _ f i f o s . h> < r tai_ n am 2 n u m . h>
# i n c l u d e < r t a i _ m a t h . h> # include " . . / include / p l a t f o r m d e f i n i t i o n s . h" # include " . . / include / f i f o d e f s . h" # include " . . / include / l i n l i b / a l c _ l i b . h" /∗ Implementation of sensor fus ion ∗/ /∗ # include " s ens or fus ion / a t t _ d e t . c " ∗/ /∗ # include " s ens or fus ion / pos_fusion . c" ∗/
/∗ Real # define # define # define
Time r e l a t e d d e f i n e s ∗ / TICK_PERIOD 20000000 TASK_PRIORITY 1 STACK_SIZE 10000
s t a t i c RT_TASK c o n t r o l l e r _ t a s k ; / ∗ P o i n t e r t o s h a r e d memory s t r u c t ∗ / s t a t i c s h ar ed _ m em o r y _ t ∗ s e n s o r _ d a t a _ r a w ; s ta t i c n av i ga t or _c on t ro ll e r_ t ∗from_navigator ; static controller_navigator_t ∗to_navigator ; / ∗ S t a t i c i n t u s e d t o c o u n t number o f t i m e s s h a r e d memory s em a p h o r e i s l o c k e d ∗ / s t a t i c i n t s e n s o r _ d a t a _ r e a d _ f a i l u r e s =0;
/ ∗ The c o n t r o l l e r t a s k ∗/ s t a t i c void c o n t r o l l e r ( i n t t ) { /∗ # include " s ens or fus ion / m a t r i x _ i n i t . h" ∗/ / ∗ The numbers o f s a m p l e s r u n n ed ∗ / s t a t i c i n t n s am p le = 0 ; f l o a t x_ref =0.0 , y_ref =0.0 , z_ref =0.0; s e n s o r _ d a t a _ r a w −>S e n s o r s V a l i d = 0 ; / ∗ Do f o r e v e r ∗ /
165
APPENDIX C. HELICOPTER CODE 52 53 54 55 56 57 58 59 60 61 62 63 64 65
while ( 1 ) { / ∗ Count how many t i m e s s h a r e d memory i s n o t a v a i l a b l e ∗ / i f ( s e n s o r _ d a t a _ r a w −>l o c k e d == ’ 1 ’ ) s e n s o r _ d a t a _ r e a d _ f a i l u r e s ++; i f ( ! ( f r o m _ n a v i g a t o r −>new_message == 0 ) ) { f r o m _ n a v i g a t o r −>new_message = 0 ; x _ r e f = f r o m _ n a v i g a t o r −> r e f e r e n c e [ 0 ] ; y _ r e f = f r o m _ n a v i g a t o r −> r e f e r e n c e [ 1 ] ; z _ r e f = f r o m _ n a v i g a t o r −> r e f e r e n c e [ 2 ] ; /∗ p r i n t k (" Contr oller : Received fr o m n a v i g a t o r \ n " ) ; ∗ / s e n s o r _ d a t a _ r a w −> n a v i g a t o r . s r e f [ 0 ] = s e n s o r _ d a t a _ r a w −> n a v i g a t o r . s r e f [ 1 ] = s e n s o r _ d a t a _ r a w −> n a v i g a t o r . s r e f [ 2 ] =
66 67 68 69 70 71 72 73 74 75 76 77 78
/∗
79
/∗
80
/∗
81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
166
controller references x_ref ; y_ref ; z_ref ;
} / ∗ S t o r e t h e number o f s a m p l e s i n t h e s h a r e d memory s e n s o r _ d a t a _ r a w −>s am p len u m b er = n s am p le ;
∗/
/∗ ∗ S t a r t of c o n t r o l l e r algorithm ∗/ a t t _ d e t e r m i n a t i o n ( s e n s o r _ d a t a _ r a w , a _ i , m_i , a_b , m_b , B _ a t t , Z _ a t t , K _ a tt , K_eig , tm p _ s q r _ m a t , a t t ) ; ∗ / q u a t 2 e u l e r ( s e n s o r _ d a t a _ r a w , K_eig , a t t , qq_div_D , mult_array , mult_array1 ) ; / \ ∗ Conversion to Euler angles ∗ \ / ∗/ p o s _ f u s i o n ( s e n s o r _ d a t a _ r a w , Ad , Bd , Cd , X , U, Z , Q, R , P , P _ p r i o r , X _ p r i o r , K , eye12 , in v_ m a t , temp1 , temp2 , temp1_small , temp2_small ) ; ∗/
i f ( s e n s o r _ d a t a _ r a w −>s e r v o . mode== ’ 0 ’ ) { s e n s o r _ d a t a _ r a w −> c o n t r o l l e r _ o u t . u _ l a t = 1 ; s e n s o r _ d a t a _ r a w −> c o n t r o l l e r _ o u t . u _ l o n = 2 ; s e n s o r _ d a t a _ r a w −> c o n t r o l l e r _ o u t . u _ c o l = 3 ; s e n s o r _ d a t a _ r a w −> c o n t r o l l e r _ o u t . u_yaw = 4 ; } /∗ ∗ End o f c o n t r o l l e r a l g o r i t h m ∗/ / ∗ I n c r e m e n t t h e number o f s a m p l e s p r o c e s s e d n s am p le ++;
∗/
/ ∗ # # # # # # # # # # # # # # # # # # # # # # # # # ## # # ## # ## # # ## # # ## # ## # # ## # # ## # ∗ /
/ ∗ W r i t e t h e s h a r e d memory t o t h e l o g g e r FIFO i f d a t a v a l i d ∗/
C.3. CONTROLLER CODE 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118
r t f _ p u t ( LOGGER_FIFO_ID, s e n s o r _ d a t a _ r a w , s i z e o f ( ∗ sensor_data_raw ) ) ; / ∗ W a it u n t i l t i m e r f i r e s rt_task_wait_period () ; } } / ∗ Module i n i t i a l i z a t i o n s ∗/ i n t init_m odule ( void ) { /∗ Setup the t i c k period ∗/ RTIME t i c k _ p e r i o d ; / ∗ and make i t p e r i o d i c ∗ / rt_set_periodic_mode () ; /∗ I n i t i a l i z e the task ∗/ r t _ t a s k _ i n i t (& c o n t r o l l e r _ t a s k , c o n t r o l l e r , 1 , STACK_SIZE , TASK_PRIORITY , 1 , 0 ) ;
119 120 121 122 123
printk ( " Controller
: Entity i n i t i a l i z e d \ n" ) ;
/ ∗ A t t a c h t o t h e s h a r e d memory ∗ / s e n s o r _ d a t a _ r a w = r t a i _ k m a l l o c ( nam2num ( SharedMemId ) , s i z e o f ( s h ar ed _ m em o r y _ t ) ) ; f r o m _ n a v i g a t o r = r t a i _ k m a l l o c ( nam2num ( "PCLLSM" ) , s i z e o f ( navigator_controller_t ) ) ; t o _ n a v i g a t o r = r t a i _ k m a l l o c ( nam2num ( "LLPCSM" ) , s i z e o f ( controller_navigator_t ) ) ;
124 125 126 127
i f ( ( s e n s o r _ d a t a _ r a w ! = NULL) & ( f r o m _ n a v i g a t o r ! = NULL) & ( t o _ n a v i g a t o r ! = NULL) ) printk ( " Controller : S h a r e d Memory A l l o c a t e d \ n " ) ;
128 129 130 131 132 133
/ ∗ I n i t i a l i z e t h e h e l i c o p t e r i n manual mode ∗ / s e n s o r _ d a t a _ r a w −>s e r v o . mode = ’ 0 ’ ; printk ( " Controller : H e l i c o p t e r s e t t o manual mode ( 0 ) \ n " ) ;
134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151
∗/
/ ∗ C r e a t e t h e l o g g e r FIFO ∗ / r t f _ c r e a t e ( LOGGER_FIFO_ID, LOGGER_BUFFER_SIZE ) ; /∗ S t a r t the task ∗/ t i c k _ p e r i o d = s t a r t _ r t _ t i m e r ( n a n o 2 c o u n t ( TICK_PERIOD ) ) ; r t _ t a s k _ m a k e _ p e r i o d i c (& c o n t r o l l e r _ t a s k , r t _ g e t _ t i m e ( ) + tick_period , tick_period ) ; return 0; } / ∗ Module c l e a n u p s when r e m o v i n g t h e module void cleanup_module ( void ) { /∗ Stop the tim er ∗/ stop_rt_timer () ;
∗/
/∗ D elete our r e a l tim e t a s k ∗/ r t _ t a s k _ d e l e t e (& c o n t r o l l e r _ t a s k ) ;
167
APPENDIX C. HELICOPTER CODE 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
168
/ ∗ F r ee t h e s h a r e d memory ∗ / r t a i _ k f r e e ( nam2num ( SharedMemId ) ) ; r t a i _ k f r e e ( nam2num ( "PCLLSM" ) ) ; r t a i _ k f r e e ( nam2num ( "LLPCSM" ) ) ; / ∗ D e s t r o y t h e FIFOs ∗ / r t f _ d e s t r o y ( LOGGER_FIFO_ID) ; if ( sensor_data_read_failures ) { rt_printk ( " Controller : T h er e were %d r e a d f a i l u r e s ! \ n " , sensor_data_read_failures ) ; } printk ( " Controller return ; } MODULE_LICENSE( "GPL" ) ;
: E n t i t y s h u t down \ n \ n \ n " ) ;
C.4. GROUND STATION CODE
C.4 Ground Station Code This section contains the code for the ground station client, server, logger, and user interface.
C.4.1 Ground Station Client 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48
# include # include # include # include # include # include # include # include
< s t d i o . h> < s y s / s o c k e t . h> < a r p a / i n e t . h> < s t d l i b . h> < s t r i n g . h> < u n i s t d . h> < n e t i n e t / i n . h> < s i g n a l . h>
# include " . . / h e l i c o p t e r / include / tcp . h" # include " . . / h e l i c o p t e r / include / p l a t f o r m d e f i n i t i o n s . h" i n t sock ; struct sockaddr_in echoserver ; s h ar ed _ m em o r y _ t s h a r e d D a t a ; set_tasks_state_t set_tasks_state ; /∗ Defined in ui . c ∗/ void end_screen ( void ) ; void i n i t _ s c r e e n ( void ) ; v o i d p r i n t 2 s c r e e n ( s h ar ed _ m em o r y _ t ∗ s h a r e d D a t a ) ; /∗ Defined in logger . c ∗/ void i n i t _ l o g g e r ( void ) ; void s top_logge r ( void ) ; v o i d l o g d a t a ( s h ar ed _ m em o r y _ t ∗ l o g D a t a ) ; / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ P r i n t msg e r r o r and e x i t . ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / v o i d Die ( char ∗ mess ) { p e r r o r ( mess ) ; exit (1) ; } / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ S i g n a l h a n d l e r f o r c t r l −c e t c . ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / void i n t _ s i g _ h a n d l e r ( i n t s ) { i n t s y n c b y t e = GSSTOP ; p r i n t f ( "GS C l i e n t : −> GSSTOP \ n " ) ; / ∗ Send s y n c b y t e t o make s u r e c o m m u n i c a t i o n i s i n s y n c ∗ / i f ( s e n d ( sock , &s y n c b y t e , s i z e o f ( s y n c b y t e ) , 0 ) == −1) { Die ( " F a i l e d t o s e n d b y t e s t o c l i e n t " ) ;
169
APPENDIX C. HELICOPTER CODE 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
} / ∗ p r i n t f ( " Send s y n c b y t e (%d ) " , s y n c b y t e ) ; ∗ / / ∗ D eta ch fr o m s h a r e d memory h e r e ∗ / / / clo s e ( sock ) ; /∗ Close th e tcp s o cket ∗/ p r i n t f ( "GS C l i e n t : C l o s i n g UI \ n " ) ; e n d _ s c r e e n ( ) ; / ∗ End n c u r s e s mode ∗ / s top_logger ( ) ; /∗ Close the logger f i l e s ∗/ exit (0) ; } / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ Setup the tcp socket ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / int i n i t_ c l ie n t () { / / Handle c t r l −c s i g n a l ( SIGINT , i n t _ s i g _ h a n d l e r ) ; s i g n a l ( SIGTERM, i n t _ s i g _ h a n d l e r ) ; / ∗ C r e a t e t h e TCP s o c k e t ∗ / i f ( ( s o c k = s o c k e t ( PF_INET , SOCK_STREAM, IPPROTO_TCP) ) < 0 ) { Die ( " F a i l e d t o c r e a t e s o c k e t " ) ; } /∗ Construct the server sockaddr_in s t r u c t u r e ∗/ memset(& e c h o s e r v e r , 0 , s i z e o f ( e c h o s e r v e r ) ) ; s t r u c t ∗/ e c h o s e r v e r . s i n _ f a m i l y = AF_INET ; / IP ∗ / e c h o s e r v e r . s i n _ a d d r . s _ a d d r = i n e t _ a d d r ( SERVER_IP ) ; address ∗/ e c h o s e r v e r . s i n _ p o r t = h t o n s (DATA_PORT ) ; port ∗/
77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100
170
/∗ Clear /∗ I n t e r n e t / ∗ IP /∗ server
/∗ E s ta bl is h connection ∗/ i f ( c o n n e c t ( sock , ( s t r u c t s o c k a d d r ∗ ) &e c h o s e r v e r , s i z e o f ( echos erver ) ) < 0) { Die ( " F a i l e d t o c o n n e c t w i t h s e r v e r " ) ; } p r i n t f ( "GS C l i e n t : C o n n ecte d t o s e r v e r \ n " ) ; return 0; } in t receive_data ( in t syncbyte ) { p r i n t f ( "GS C l i e n t : −> GSRUN\ n " ) ; / ∗ Send GSRUN t o t e l l s e r v e r t h a t c l i e n t w a n ts d a t a ∗ / s y n c b y t e = GSRUN; i f ( s e n d ( sock , &s y n c b y t e , s i z e o f ( s y n c b y t e ) , 0 ) == −1) { Die ( " F a i l e d t o s e n d b y t e s t o c l i e n t " ) ; } / ∗ S en d s s e t _ t a s k s _ s t a t e s t r u c t ∗ /
C.4. GROUND STATION CODE 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149
i f ( ( s e n d ( sock , &s e t _ t a s k s _ s t a t e , s i z e o f ( s e t _ t a s k s _ s t a t e ) , 0 ) ) < 1) { p r i n t f ( " F a i l e d t o s e n d s e t _ t a s k s _ s t a t e from s e r v e r " ) ; } p r i n t f ( "GS C l i e n t : GSOK\ n " ) ; return syncbyte ; } / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ Main l o o p : R e c e i v e / s e n d d a t a fr o m / t o s e r v e r ∗ ∗ helicopter ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / i n t main ( i n t a r g c , char ∗ a r g v [ ] ) { i n t s y n c b y t e =GSRUN; s i g n a l ( SIGINT , i n t _ s i g _ h a n d l e r ) ; s i g n a l ( SIGTERM, i n t _ s i g _ h a n d l e r ) ; p r i n t f ( "GS C l i e n t : I n i t i a l i z i n g l o g g e r \ n " ) ; i n i t _ l o g g e r ( ) ; /∗ I n i t i a l i z e the logger ∗/ p r i n t f ( "GS C l i e n t : I n i t i a l i z e c o n n e c t i o n \ n " ) ; i n i t _ c l i e n t ( ) ; /∗ I n i t i a l i z e connection to server ∗/ p r i n t f ( "GS C l i e n t : I n i t i a l i z e UI \ n " ) ; i n i t _ s c r e e n ( ) ; / ∗ I n i t i a l i z e n c u r s e s mode ∗ / while ( 1 ) { / ∗ s l e e p f o r 20000 u s = 20 ms = 50 Hz ∗ / usleep (20000) ; syncbyte = receive_data ( syncbyte ) ; p r i n t f ( "GS C l i e n t : L o g g in g d a t a \ n " ) ; l o g d a t a (& s h a r e d D a t a ) ; p r i n t 2 s c r e e n (& s h a r e d D a t a ) ; } }
C.4.2 Ground Station Client Logger 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
172
# include # include # include # include # include # include # include # include # include
< t i m e . h> < s t d i o . h> < s y s / s o c k e t . h> < a r p a / i n e t . h> < s t d l i b . h> < s t r i n g . h> < u n i s t d . h> < n e t i n e t / i n . h> < s i g n a l . h>
# include " . . / h e l i c o p t e r / include / tcp . h" # include " . . / h e l i c o p t e r / include / p l a t f o r m d e f i n i t i o n s . h" s h ar ed _ m em o r y _ t s h a r e d D a t a ; FILE ∗ p F ileG P S ; FILE ∗ p F i l e G y r o ; FILE ∗ p F i l e C o m p a s s ; FILE ∗ p F i l e T a c h o ;
C.4. GROUND STATION CODE 19 20 21 22 23 24 25 26 27 28
29
30
31
FILE FILE char char char
v o i d l o g d a t a ( s h ar ed _ m em o r y _ t ∗ l o g D a t a ) { / ∗ GPS ∗ / f p r i n t f ( pFileGPS , "%d ,% l f ,% l f ,% l f ,% l f " , l o g D a t a −>samplenumber , l o g D a t a −>GPS . GPSTime , l o g D a t a −>GPS . UTMn, l o g D a t a −>GPS . UTMe, l o g D a t a −>GPS . A l t i t u d e ) ; f p r i n t f ( pFileGPS , " ,%d ,%d ,%d ,%d " , l o g D a t a −>GPS . S a t e l l i t e s , l o g D a t a −>GPS . GPSvalid , l o g D a t a −>GPS . GPSPosType , l o g D a t a −> GPS . GPSVelType ) ; f p r i n t f ( pFileGPS , " ,% l f ,% l f ,% l f ,% f ,% f " , l o g D a t a −>GPS . vSpeed , l o g D a t a −>GPS . hSpeed , l o g D a t a −>GPS . Heading , l o g D a t a −>GPS . S t d N o r t h , l o g D a t a −>GPS . S t d A l t ) ; f p r i n t f ( pFileGPS , " ,% ld ,%d \ n " , l o g D a t a −>GPS . ZoneNumber , l o g D a t a −>GPS . Z o n e L e t t e r ) ;
32 33 34
/ ∗ Gyro ∗ / f p r i n t f ( p F i l e G y r o , "%d ,% l f ,% l f ,% l f , " , l o g D a t a −>samplenumber , l o g D a t a −>g y r o . r ateX , l o g D a t a −>g y r o . r ateY , l o g D a t a −>g y r o . rateZ ) ; f p r i n t f ( p F i l e G y r o , "%l f ,% l f ,% l f ,% f , " , l o g D a t a −>g y r o . accX , l o g D a t a −>g y r o . accY , l o g D a t a −>g y r o . accZ , l o g D a t a −>g y r o . T ) ; f p r i n t f ( p F i l e G y r o , "%d ,%d ,%d , " , l o g D a t a −>g y r o . r a t e X i , l o g D a t a −> g y r o . r a t e Y i , l o g D a t a −>g y r o . r a t e Z i ) ; f p r i n t f ( p F i l e G y r o , "%d ,%d ,%d ,%d \ n " , l o g D a t a −>g y r o . accXi , l o g D a t a −>g y r o . accYi , l o g D a t a −>g y r o . accZ i , l o g D a t a −>g y r o . Ti ) ;
35 36 37
38 39 40
/ ∗ Compass ∗ / f p r i n t f ( p F ileC o m p as s , "%d ,% l f ,% l f ,% l f , " , l o g D a t a −>samplenumber , l o g D a t a −>compass . magX , l o g D a t a −>compass . magY , l o g D a t a −> compass . magZ ) ; f p r i n t f ( p F ileC o m p as s , "%d ,%d ,%d \ n " , l o g D a t a −>compass . magXi , l o g D a t a −>compass . magYi , l o g D a t a −>compass . magZi ) ;
41 42 43 44
/ ∗ Tacho ∗ / f p r i n t f ( p F i l e T a c h o , "%d ,%d ,% f \ n " , l o g D a t a −>samplenumber , l o g D a t a −>t a c h o . T ach o 1 i , l o g D a t a −>t a c h o . Tacho1 ) ;
45 46 47
/∗ Servos ∗/ f p r i n t f ( p F i l e S e r v o , "%d ,%d ,%d , " , l o g D a t a −>samplenumber , l o g D a t a −>s e r v o . mode , l o g D a t a −>s e r v o . T S e r v o 1 i ) ; f p r i n t f ( p F i l e S e r v o , "%d ,%d ,%d , " , l o g D a t a −>s e r v o . S e r v o 1 i , l o g D a t a −>s e r v o . S e r v o 2 i , l o g D a t a −>s e r v o . S e r v o 3 i ) ; f p r i n t f ( p F i l e S e r v o , "%d ,%d ,%d \ n " , l o g D a t a −>s e r v o . S e r v o 4 i , l o g D a t a −>s e r v o . S e r v o 5 i , l o g D a t a −>s e r v o . S e r v o 6 i ) ;
48 49 50 51 52 53 54 55
∗ pFileServo ; ∗ pFileSystem ; D ir ecto r y N a m e [ 9 9 ] ; Path [ 9 9 ] ; Time [ 2 0 ] ;
/∗ System ∗/ f p r i n t f ( p F i l e S y s t e m , "%d ,%d \ n " , l o g D a t a −>samplenumber , l o g D a t a −>s y s t e m . WLANQuality ) ; } void i n i t _ l o g g e r ( )
173
APPENDIX C. HELICOPTER CODE 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
100 101 102
103 104 105
174
{ char C r e a t e D i r [ 9 9 ] ; char F i l e n a m e [ 9 9 ] ; char Time [ 9 9 ] ; / ∗ Time s t r u c t s d e f i n e d i n t i m e . h tim e_ t rawtime ; s t r u c t tm ∗ t i n f o ;
∗/
/ ∗ Get t h e c u r r e n t t i m e and d a t e ∗ / t i m e (& r a w t i m e ) ; t i n f o = gmtime (& r a w t i m e ) ; /∗ Create log d i r ∗/ p r i n t f ( "GS C l i e n t : C r e a t i n g l o g d i r e c t o r y \ n " ) ; s p r i n t f ( D ir ecto r y N am e , " / l o g s / d a t a −%.4d %.2d %.2d−%.2d . % . 2 d . % . 2 d " , t i n f o −>t m _ y e a r +1 9 0 0 , t i n f o −>tm_mon +1 , t i n f o −>tm_mday , t i n f o −>tm _ h o u r +1 , t i n f o −>tm_min , t i n f o −>tm _ s ec ) ; s p r i n t f ( P ath , "%s " , D ir ecto r y N a m e ) ; s p r i n t f ( C r e a t e D i r , " m k d ir %s " , P a t h ) ; system ( Cr eateD ir ) ; p r i n t f ( "GS C l i e n t : C r e a t e d : %s \ n " , P a t h ) ; / ∗ C r e a t e t h e f i l e n a m e s and h a n d l e s f o r t h e l o g d a t a s p r i n t f ( F ilen am e , "%s / g p s . c s v " , P a t h ) ; p F ileG P S = f o p e n ( F ilen am e , " wt " ) ;
∗/
s p r i n t f ( F ilen am e , "%s / g y r o . c s v " , P a t h ) ; p F i l e G y r o = f o p e n ( F ilen am e , " wt " ) ; s p r i n t f ( F ilen am e , "%s / compass . c s v " , P a t h ) ; p F i l e C o m p a s s = f o p e n ( F ilen am e , " wt " ) ; s p r i n t f ( F ilen am e , "%s / t a c h o . c s v " , P a t h ) ; p F i l e T a c h o = f o p e n ( F ilen am e , " wt " ) ; s p r i n t f ( F ilen am e , "%s / s e r v o . c s v " , P a t h ) ; p F i l e S e r v o = f o p e n ( F ilen am e , " wt " ) ; s p r i n t f ( F ilen am e , "%s / s y s t e m . c s v " , P a t h ) ; p F i l e S y s t e m = f o p e n ( F ilen am e , " wt " ) ; /∗ F ile header ∗/ s p r i n t f ( Time , "%d %.2d %.2d %d:%d:%d " , t i n f o −>t m _ y e a r +1 9 0 0 , t i n f o −>tm_mon +1 , t i n f o −>tm_mday , t i n f o −>tm _ h o u r +1 , t i n f o −>tm_min , t i n f o −>tm _ s ec ) ; f p r i n t f ( pFileGPS , "GPS D ata from : %s \ n " , Time ) ; f p r i n t f ( pFileGPS , " Sample , GPSTime , UTMn, UTMe, A lt , S a t s , V alid , PosType , VelType , vSpeed , hSpeed , Heading , S t d N o r t h , S t d E a s t , S t d A l t , ZoneNumber , Z o n e L e t t e r \ n " ) ; f p r i n t f ( p F i l e G y r o , " Gyro D ata from : %s \ n " , Time ) ; f p r i n t f ( p F i l e G y r o , " Sample , r ateX , r ateY , r a t e Z , accX , accY , accX , T , r a t e X i , r a t e Y i , r a t e Z i , accXi , accYi , accXi , T i \ n ") ;
C.4. GROUND STATION CODE 106 107 108
f p r i n t f ( p F ileC o m p as s , " Compass D ata from : %s \ n " , Time ) ; f p r i n t f ( p F ileC o m p as s , " Sample , magX , magY , magZ , magXi , magYi , magZi \ n " ) ;
109 110 111 112 113 114
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149
f p r i n t f ( p F i l e T a c h o , " T ach o m eter D ata from : %s \ n " , Time ) ; f p r i n t f ( p F i l e T a c h o , " Sample , P e r i o d \ n " ) ; f p r i n t f ( p F i l e S e r v o , " S er v o D ata from : %s \ n " , Time ) ; f p r i n t f ( p F i l e S e r v o , " Sample , Mode , T a i l S er v o P o s i t i o n [ i n t ] , Collective [ i n t ] , Roll [ i n t ] , Pitch [ i n t ] , T a i l S er v o I n p u t [ i n t ] , NC, NC\ n " ) ; f p r i n t f ( p F i l e S y s t e m , " System D ata from : %s \ n " , Time ) ; f p r i n t f ( p F i l e S y s t e m , " Sample , Wlan Q u a l i t y \ n " ) ; p r i n t f ( "GS C l i e n t : Log f i l e s c r e a t e d \ n " ) ; } void s top_logge r ( ) { char m y s t r i n g [ 9 0 ] ; i n t tmp ; p r i n t f ( "GS C l i e n t : C l o s i n g l o g f i l e s \ n " ) ; i f ( ( tmp= f c l o s e ( p F ileG P S ) ) ! = 0 ) p r i n t f ( " L o g g er : E r r o r c l o s i n g GPS f i l e : %d \ n " , tmp ) ; i f ( ( tmp= f c l o s e ( p F i l e G y r o ) ) ! = 0 ) p r i n t f ( " L o g g er : E r r o r c l o s i n g Gyro f i l e : %d \ n " , tmp ) ; i f ( ( tmp= f c l o s e ( p F i l e C o m p a s s ) ) ! = 0 ) p r i n t f ( " L o g g er : E r r o r c l o s i n g Compass f i l e : %d \ n " , tmp ) ; i f ( ( tmp= f c l o s e ( p F i l e T a c h o ) ) ! = 0 ) p r i n t f ( " L o g g er : E r r o r c l o s i n g Tacho f i l e : %d \ n " , tmp ) ; i f ( ( tmp= f c l o s e ( p F i l e S e r v o ) ) ! = 0 ) p r i n t f ( " L o g g er : E r r o r c l o s i n g S er v o f i l e : %d \ n " , tmp ) ; i f ( ( tmp= f c l o s e ( p F i l e S y s t e m ) ) ! = 0 ) p r i n t f ( " L o g g er : E r r o r c l o s i n g System f i l e : %d \ n " , tmp ) ; / ∗ Copy d a t a t o " n e w e s t " ∗ / p r i n t f ( "GS C l i e n t : Copying l o g d a t a t o ’ n e w e s t ’− f o l d e r \ n " ) ; s p r i n t f ( m y s t r i n g , " cp −f %s / ∗ . ∗ / l o g s / n e w e s t / " , P a t h ) ; system ( mystring ) ;
p r i n t f ( "GS C l i e n t : L o g g in g s t o p p e d \ n " ) ; e x i t ( EXIT_SUCCESS ) ; }
C.4.3 Ground Station Client UI 1 2 3 4 5 6 7 8
# include # include # include # include # include
< t g m a t h . h> < n c u r s e s . h> " . . / he li c opt er / include / p la t fo rmde fi ni t io ns . h" " . . / helicopter / r t / navigator / navigator . h"
# d e f i n e DEBUG
175
APPENDIX C. HELICOPTER CODE 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61
176
v o i d p r i n t 2 s c r e e n ( s h ar ed _ m em o r y _ t ∗ s h a r e d D a t a ) { i n t w in d o w _ h eig h t , window_width ; / / Number o f l i n e s and co lo u m n s i n t t e x t _ w i d t h =5 0 ; / / W id th o f t h e t e x t # i f n d e f GRB_NAV_DEV / ∗ We a r e n o t p r i n t i n g t h e box s o t h i s v a r i a b l e i s n o t n eed ed ∗ / i n t v a l _ w i d t h =1 5 ; / / W id th o f t h e v a l u e # endif i n t f i r s t _ l i n e =0; i n t line_number= f i r s t _ l i n e ; getmaxyx ( s t d s c r , w in d o w _ h eig h t , window_width ) ; /∗ m v p r i n t w ( 3 0 , 1 0 , " H e i g h t : %d \ n " , w i n d o w _ h e i g h t ) ; ∗ / /∗ m v p r i n t w ( 5 0 , 1 0 , " W id th : %d \ n " , w in d o w _ w id th ) ; ∗ / clear () ; # i f n d e f GRB_NAV_DEV / ∗ GPS INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , "GPS" ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; m v p r in tw ( l i n e _ n u m b e r , 0 , " Uptime ( c o n t r o l l e r ) " ) ; / / s t a r t the actual printing m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −> s am p len u m b er / 5 0 ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " Number o f s a t e l l i t e s " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>GPS . Satellites ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " Measurement s t a t u s " ) ; s w i t c h ( s h a r e d D a t a −>GPS . G P S v alid ) { c a s e −2: m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "CRC e r r o r " ) ; break ; c a s e −1: m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "No s o l u t i o n " ) ; break ; case 0: m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " S t a r t i n g " ) ; break ; case 1: m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " V a l i d " ) ; break ; default : m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " Unknown " ) ; break ; } l i n e _ n u m b e r ++;
C.4. GROUND STATION CODE 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r H ead in g ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r hSpeed ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r vSpeed ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r Altitude ) ; l i n e _ n u m b e r ++; # i f d e f DEBUG m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r GPSTime ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r ZoneNumber ) ; / ∗ l i n e _ n u m b e r ++; # e n d i f / ∗ DEBUG ∗ / # i f n d e f GRB_NAV_DEV move ( f i r s t _ l i n e , v l i n e ( ACS_VLINE , move ( l i n e _ n u m b e r h l i n e ( ACS_HLINE , l i n e _ n u m b e r ++; # e n d i f / ∗ GRB_NAV_DEV
, 0 , " H ead in g [ d e g r e e s ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS .
, 0 , " H o r i z o n t a l s p e e d [m/ s ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS .
, 0 , " V e r t i c a l s p e e d [m/ s ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS .
, 0 , " P o s i t i o n ( x ( n o r t h ) ) [m] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS . UTMe)
, 0 , " P o s i t i o n ( y ( e a s t ) ) [m] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS . UTMn)
, 0 , " P o s i t i o n ( z ( a l t i t u d e ) ) [m] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS .
, 0 , "GPS t i m e [ s ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>GPS .
, 0 , " Timezone " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>GPS . FIXME : C o r r e c t t h e t y p e ∗ /
t e x t _ w i d t h + v a l _ w i d t h ) ; / / P r i n t t h e box l i n e _ n u m b e r −1) ; ,0) ; text_width +val_width ) ; ∗/
/ ∗ Gyrocube INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , "GYROCUBE: " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; # i f d e f DEBUG m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r rateXi ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r rateYi ) ;
, 0 , "Raw X−r a t e [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o .
, 0 , "Raw Y−r a t e [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o .
177
APPENDIX C. HELICOPTER CODE 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157
178
l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r rateZi ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r accX i ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r accY i ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r accZi ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r l i n e _ n u m b e r ++; # e n d i f / ∗ DEBUG ∗ / m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r rateX ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r rateY ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r rateZ ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r ); l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r ); l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r ); l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r l i n e _ n u m b e r ++; # i f n d e f GRB_NAV_DEV move ( f i r s t _ l i n e , v l i n e ( ACS_VLINE , move ( l i n e _ n u m b e r h l i n e ( ACS_HLINE , l i n e _ n u m b e r ++; # e n d i f / ∗ GRB_NAV_DEV
, 0 , "Raw Z−r a t e [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o .
, 0 , "Raw X−a c c e l a r a t i o n [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o .
, 0 , "Raw Y−a c c e l a r a t i o n [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o .
, 0 , "Raw Z−a c c e l a r a t i o n [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o .
, 0 , "Raw t e m p e r a t u r e [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>g y r o . T i ) ;
, 0 , "X−r a t e [ r a d / s e c ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o .
, 0 , "Y−r a t e [ r a d / s e c ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o .
, 0 , "Z−r a t e [ r a d / s e c ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o .
, 0 , "X−a c c e l a r a t i o n [m/ s e c ^ 2 ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o . accX
, 0 , "Y−a c c e l a r a t i o n [m/ s e c ^ 2 ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o . accY
, 0 , "Z−a c c e l a r a t i o n [m/ s e c ^ 2 ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o . accZ
, 0 , " Temperature [ ? ? ? ] " ) ; , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>g y r o . T ) ;
t e x t _ w i d t h + v a l _ w i d t h ) ; / / P r i n t t h e box line_number −f i r s t _ l i n e ) ; ,0) ; text_width +val_width ) ; ∗/
C.4. GROUND STATION CODE 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204
/ ∗ Compass INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , " Compass : " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; # i f d e f DEBUG m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r magXi ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r magYi ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r magZi ) ; l i n e _ n u m b e r ++; # e n d i f / ∗ DEBUG ∗ / m v p r in tw ( l i n e _ n u m b e r "); m v p r in tw ( l i n e _ n u m b e r magX ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r "); m v p r in tw ( l i n e _ n u m b e r magY ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r "); m v p r in tw ( l i n e _ n u m b e r magZ ) ; l i n e _ n u m b e r ++; # i f n d e f GRB_NAV_DEV move ( f i r s t _ l i n e , v l i n e ( ACS_VLINE , move ( l i n e _ n u m b e r h l i n e ( ACS_HLINE , l i n e _ n u m b e r ++; # e n d i f / ∗ GRB_NAV_DEV
, 0 , "Raw r o l l a n g l e (X) [ b i t s ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>compass .
, 0 , "Raw p i t c h a n g l e (Y) [ b i t s ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>compass .
, 0 , "Raw yaw a n g l e ( Z ) [ b i t s ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>compass .
, 0 , " M a g n e t i c f i e l d m e a s u r e m e n t s (X) ?? , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>compass .
, 0 , " M a g n e t i c f i e l d m e a s u r e m e n t s (Y) ?? , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>compass .
, 0 , " M a g n e t i c f i e l d m e a s u r e m e n t s ( Z ) ?? , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>compass .
t e x t _ w i d t h + v a l _ w i d t h ) ; / / P r i n t t h e box line_number −f i r s t _ l i n e ) ; ,0) ; text_width +val_width ) ; ∗/
/ ∗ S e r v o INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , " S e r v o s : " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; m v p r in tw ( l i n e _ n u m b e r , 0 , " C o l l e c t i v e P i t c h S er v o [ 1 0 b i t ] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s e r v o . Servo1i ) ; l i n e _ n u m b e r ++;
179
APPENDIX C. HELICOPTER CODE 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251
180
m v p r in tw ( l i n e _ n u m b e r ; m v p r in tw ( l i n e _ n u m b e r Servo2i ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r ); m v p r in tw ( l i n e _ n u m b e r Servo3i ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r Servo4i ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r TServo1i ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r m v p r in tw ( l i n e _ n u m b e r mode ) ; l i n e _ n u m b e r ++; # i f d e f GRB_NAV_DEV move ( f i r s t _ l i n e , v l i n e ( ACS_VLINE , move ( l i n e _ n u m b e r h l i n e ( ACS_HLINE , l i n e _ n u m b e r ++; # endif
, 0 , " L a t . C y c l i c P i t c h S er v o [ 1 0 b i t ] " ) , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s e r v o .
, 0 , " Long . C y c l i c P i t c h S er v o [ 1 0 b i t ] " , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s e r v o .
, 0 , " T a i l S er v o I n p u t [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s e r v o .
, 0 , " T a i l S er v o P o s i t i o n [ 1 0 b i t ] " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s e r v o .
, 0 , " Mode " ) ; , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s e r v o .
t e x t _ w i d t h + v a l _ w i d t h ) ; / / P r i n t t h e box line_number −f i r s t _ l i n e ) ; ,0) ; text_width +val_width ) ;
/ ∗ T a ch o m eter INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , " T ach o m eter : " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; # i f d e f DEBUG m v p r in tw ( l i n e _ n u m b e r , 0 , " R a t e o f r o t a t i o n ( r a q ) [ 1 6 b i t ] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>t a c h o . Tacho1i ) ; l i n e _ n u m b e r ++; # e n d i f / ∗ DEBUG ∗ / /∗ mvprintw ( line_number , 0 , " Rate of r o t a t i o n [ rad / s ec ] " ) ; ∗/ /∗ m v p r i n t w ( l i n e _ n u m b e r , t e x t _ w i d t h ,"% f " , s h a r ed D a ta −>t a c h o . Tacho1 ) ; ∗ / m v p r in tw ( l i n e _ n u m b e r , 0 , " Rounds P e r M in u te [RPM] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , ( s h a r e d D a t a −>t a c h o . Tacho1 ) ∗ 6 0 / ( 2 ∗ 3 . 1 4 ) ) ; l i n e _ n u m b e r ++; # i f n d e f GRB_NAV_DEV move ( f i r s t _ l i n e , v l i n e ( ACS_VLINE , move ( l i n e _ n u m b e r h l i n e ( ACS_HLINE , l i n e _ n u m b e r ++;
t e x t _ w i d t h + v a l _ w i d t h ) ; / / P r i n t t h e box line_number −f i r s t _ l i n e ) ; ,0) ; text_width +val_width ) ;
C.4. GROUND STATION CODE 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287
288 289 290 291 292 293 294 295 296 297 298 299 300 301 302
# endif / ∗ s y s t e m INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , " System : " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; m v p r in tw ( l i n e _ n u m b e r , 0 , "WLAN l i n k " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>s y s t e m . WLANQuality ) ; l i n e _ n u m b e r ++; # endif / ∗ Group B ’ s g r o u n d s t a t i o n
part ∗/
# i f d e f GRB_NAV_DEV / ∗ N a v i g a t o r INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , " N a v i g a t o r " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; m v p r in tw ( l i n e _ n u m b e r , 0 , " C r o s s t r a c k e r r o r [m] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>n a v i g a t o r . e_ct ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , "Yaw r e f e r e n c e [ deg ] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , ( s h a r e d D a t a −> n a v i g a t o r . y a w _ r e f / ( 2 ∗ M_PI ) ∗ 3 6 0 ) ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " H ead in g ( compass ) [ deg ] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , ( a t a n ( s h a r e d D a t a −> compass . magY / s h a r e d D a t a −>compass . magX ) +0 ) / ( 2 ∗ M_PI ) ∗360) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " n e x t w a y p o i n t ID " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>n a v i g a t o r . next_wp_ID ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " Hover " ) ; i f ( s h a r e d D a t a −>n a v i g a t o r . h o v e r ==1) { m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " H o v e r i n g " ) ; } else { m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " F l y i n g " ) ; } l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " C u r r e n t p o s i t i o n " ) ;
181
APPENDIX C. HELICOPTER CODE 303
304 305 306 307 308 309 310 311 312 313 314 315
316 317 318 319
320 321 322 323
324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340
182
m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f %f %f %d " , s h a r e d D a t a −> n a v i g a t o r . p o s _ c u r r [ 0 ] , s h a r e d D a t a −> n a v i g a t o r . p o s _ c u r r [ 1 ] , s h a r e d D a t a −>n a v i g a t o r . p o s _ c u r r [ 2 ] , s h a r e d D a t a −>GPS . Satellites ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " D i s t a n c e t o n e x t w a y p o i n t [m] " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>n a v i g a t o r . dist_next_wp ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " D i s t a n c e t o p r e v i o u s w a y p o i n t " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s h a r e d D a t a −>n a v i g a t o r . dist_prev_wp ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " Speed s q r t ( x_e ^2+ y_e ^2+ z_ e ^ 2 ) [m/ s ]") ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s q r t ( s h a r e d D a t a −> n a v i g a t o r . v _ o u t [ 0 ] ∗ s h a r e d D a t a −> n a v i g a t o r . v _ o u t [ 0 ] + s h a r e d D a t a −> n a v i g a t o r . v _ o u t [ 1 ] ∗ s h a r e d D a t a −>n a v i g a t o r . v _ o u t [ 1 ] + s h a r e d D a t a −> n a v i g a t o r . v _ o u t [ 2 ] ∗ s h a r e d D a t a −> n a v i g a t o r . v_out [ 2 ] ) ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " Next WP ( x , y , z , yaw , h o u s i d ) " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f %f %f %f %f " , s h a r e d D a t a −> n a v i g a t o r . next_wp [ 0 ] , s h a r e d D a t a −> n a v i g a t o r . next_wp [ 1 ] , s h a r e d D a t a −> n a v i g a t o r . next_wp [ 2 ] , s h a r e d D a t a −> n a v i g a t o r . next_wp [ 3 ] , s h a r e d D a t a −> n a v i g a t o r . next_wp [ 4 ] ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " P r e v i o u s WP ( x , y , z , yaw , h o u s i d ) "); m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f %f %f %f %f " , s h a r e d D a t a −> n a v i g a t o r . prev_wp [ 0 ] , s h a r e d D a t a −> n a v i g a t o r . prev_wp [ 1 ] , s h a r e d D a t a −> n a v i g a t o r . prev_wp [ 2 ] , s h a r e d D a t a −> n a v i g a t o r . prev_wp [ 3 ] , s h a r e d D a t a −> n a v i g a t o r . prev_wp [ 4 ] ) ; l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " N a v i g a t o r s t a t e " ) ; s w i t c h ( s h a r e d D a t a −> n a v i g a t o r . n a v _ s t a t e ) { c a s e PP_ONGOING : m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "PP_ONGOING " ) ; break ; c a s e PP_LEVEL_1_DONE: m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "PP_LEVEL_1_DONE" ) ; break ; c a s e PP_LEVEL_2_DONE_NOT_FOUND: m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " PP_LEVEL_2_DONE_NOT_FOUND" ) ; break ; c a s e PP_LEVEL_2_DONE_FOUND:
C.4. GROUND STATION CODE 341
m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "PP_LEVEL_2_DONE_FOUND" ) ; break ;
342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367
c a s e PP_LEVEL_2_INSIDE_WP : m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , " PP_LEVEL_2_INSIDE_WP " ) ; break ; } l i n e _ n u m b e r ++; m v p r in tw ( l i n e _ n u m b e r , 0 , " L e v e l " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%d " , s h a r e d D a t a −>n a v i g a t o r . level ) ; l i n e _ n u m b e r ++; # endif / ∗ Group A ’ s g r o u n d s t a t i o n # i f d e f GRA_DEBUG / ∗ N a v i g a t o r INFO ∗ / a t t r o n ( A_UNDERLINE ) ; m v p r in tw ( l i n e _ n u m b e r , 0 , " Group A DEBUG ( A t t i t u d e ) " ) ; a t t r o f f ( A_UNDERLINE ) ; l i n e _ n u m b e r ++; f i r s t _ l i n e =line_number ; m v p r in tw ( l i n e _ n u m b e r , 0 , " P h i " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s h a r e d D a t a −> sensor_fusion . phi ) ; l i n e _ n u m b e r ++;
368 369 370 371
m v p r in tw ( l i n e _ n u m b e r , 0 , " T h e t a " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s h a r e d D a t a −> sensor_fusion . theta ) ; l i n e _ n u m b e r ++;
372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392
part ∗/
m v p r in tw ( l i n e _ n u m b e r , 0 , " P s i " ) ; m v p r in tw ( l i n e _ n u m b e r , t e x t _ w i d t h , "%f " , s h a r e d D a t a −> sensor_fusion . psi ) ; l i n e _ n u m b e r ++; # e n d i f / ∗ GRA_DEBUG∗ / # i f n d e f GRB_NAV_DEV move ( f i r s t _ l i n e , v l i n e ( ACS_VLINE , move ( l i n e _ n u m b e r h l i n e ( ACS_HLINE , # endif
t e x t _ w i d t h + v a l _ w i d t h ) ; / / P r i n t t h e box line_number −f i r s t _ l i n e ) ; ,0) ; text_width +val_width ) ;
refresh () ; real screen ∗/
/∗ Print
i t on t o t h e
}
void i n i t _ s c r e e n ( void )
183
APPENDIX C. HELICOPTER CODE 393 394 395 396 397 398 399 400
{ initscr () ; } void end_screen ( void ) { endwin ( ) ; }
C.4.4 Ground Station Server 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
184
# include # include # include # include # include # include # include # include # include # include # include
< s t d i o . h> < s y s / s o c k e t . h> < a r p a / i n e t . h> < s t d l i b . h> < s t r i n g . h> < s i g n a l . h> < s y s / shm . h> < u n i s t d . h> < n e t i n e t / i n . h> < r t a i _ s h m . h>
# include " . . / include / tcp . h" # include " . . / include / p l a t f o r m d e f i n i t i o n s . h" # d e f i n e MAXPENDING 1
/ ∗ Max c o n n e c t i o n r e q u e s t s ∗ /
/ ∗ # d e f i n e GS_SERVER_DEBUG ∗/ int serversock , cl ientsock ; struct sockaddr_in echoserver , ec h o cl i e n t ; s t a t i c s h ar ed _ m em o r y _ t ∗ s h a r e d D a t a ; static set_tasks_state_t ∗ set_tasks_state ; v o i d p r i n t 2 s c r e e n ( s h ar ed _ m em o r y _ t ∗ s h a r e d D a t a ) ; / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ P r i n t msg e r r o r and e x i t . ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / v o i d Die ( char ∗ mess ) { p e r r o r ( mess ) ; exit (1) ; } / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ S i g n a l h a n d l e r f o r c t r l −c e t c . ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / void i n t _ s i g _ h a n d l e r ( i n t s ) { p r i n t f ( "GS S e r v e r : R e c i e v e d k i l l − C l o s i n g \ n " ) ; close ( c lientsock ) ; p r i n t f ( "GS S e r v e r : C l i e n t s o c k e t c l o s e d \ n " ) ; close ( serversock ) ; p r i n t f ( "GS S e r v e r : S e r v e r s o c k e t c l o s e d \ n \ n \ n " ) ; exit (0) ;
C.4. GROUND STATION CODE 48 49 50 51 52 53 54 55 56 57 58 59 60 61
} / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ S e t u p s e r v e r and a t t a c h t o s h a r e d mem ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / i n t i n i t _ s e r v e r ( void ) { / ∗ Handle c t r l −c ∗ / s i g n a l ( SIGINT , i n t _ s i g _ h a n d l e r ) ; s i g n a l ( SIGTERM, i n t _ s i g _ h a n d l e r ) ; / ∗ A t t a c h t o s h a r e d memory ∗ / s h a r e d D a t a = r t a i _ m a l l o c ( nam2num ( SharedMemId ) , s i z e o f ( s h ar ed _ m em o r y _ t ) ) ; i f ( s h a r e d D a t a == NULL) { p r i n t f ( "GS S e r v e r : E r r o r a t t a c h i n g t o s h a r e d memory \ n " ) ; return 0; } p r i n t f ( "GS s e r v e r : A t t a c h e d t o s h a r e d memory s u c c e s f u l l ! \ n " ) ;
62 63 64 65 66 67 68 69
/ ∗ C r e a t e t h e TCP s o c k e t ∗ / i f ( ( s e r v e r s o c k = s o c k e t ( PF_INET , SOCK_STREAM, IPPROTO_TCP) ) < 0) { Die ( "GS S e r v e r : F a i l e d t o c r e a t e s o c k e t " ) ; }
70 71 72 73 74
/∗ Construct the server sockaddr_in s t r u c t u r e ∗/ memset(& e c h o s e r v e r , 0 , s i z e o f ( e c h o s e r v e r ) ) ; /∗ Clear s t r u c t ∗/ e c h o s e r v e r . s i n _ f a m i l y = AF_INET ; /∗ I n t e r n e t / IP ∗ / e c h o s e r v e r . s i n _ a d d r . s _ a d d r = h t o n l (INADDR_ANY) ; /∗ Incoming addr ∗/ e c h o s e r v e r . s i n _ p o r t = h t o n s (DATA_PORT ) ; /∗ server port ∗/
75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98
/ ∗ B in d t h e s e r v e r s o c k e t ∗ / i f ( b i n d ( s e r v e r s o c k , ( s t r u c t s o c k a d d r ∗ ) &e c h o s e r v e r , s i z e o f ( echos erver ) ) < 0) { Die ( "GS S e r v e r : F a i l e d t o b i n d t h e s e r v e r s o c k e t " ) ; } / ∗ L i s t e n on t h e s e r v e r s o c k e t ∗ / i f ( l i s t e n ( s e r v e r s o c k , MAXPENDING) < 0 ) { Die ( "GS S e r v e r : F a i l e d t o l i s t e n on s e r v e r s o c k e t " ) ; } return 0; } / ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ F u n c t i o n t h a t h a n d l e s t h e c l i e n t conn ∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗ / i n t H an d leClien t ( i n t sock ) { i n t syncbyte =0;
185
APPENDIX C. HELICOPTER CODE 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146
186
/ ∗ R e c e i v e s y n c b y t e t o make s u r e c o m m u n i c a t i o n i s i n s y n c i f ( ( r e c v ( sock , &s y n c b y t e , s i z e o f ( s y n c b y t e ) , 0 ) ) < 1 ) { p r i n t f ( " F a i l e d t o r e c e i v e b y t e s from c l i e n t \ n " ) ; }
∗/
i f ( s y n c b y t e == GSRUN) { # i f d e f GS_SERVER_DEBUG p r i n t f ( " DATA (%d b y t e s ) \ n " , s i z e o f ( s h ar ed _ m em o r y _ t ) ) ; # endif / ∗ W a it f o r c l i e n t t o be i n s y n c ∗ / w h i l e ( ! ( s y n c b y t e == GSOK) ) { i f ( ( r e c v ( sock , &s y n c b y t e , s i z e o f ( s y n c b y t e ) , 0 ) ) < 1 ) r e t u r n GSERROR; i f ( s y n c b y t e == GSSTOP) { # i f d e f GS_SERVER_DEBUG p r i n t f ( "
