localisation in ad hoc networks using infrared...
TRANSCRIPT
Localisation in Ad Hoc Networks using Infrared ChannelsMechatronics Final Year Project: Final Report
Leo Felix Priestnall (19471254)Supervised by Dr. Y. Ahmet Şekercioğlu
Department of Electrical & Computer Systems EngineeringMonash University, Australia
Monday, February 23, 2009
DRAFT
i
Summary
The following is a project report for a final year thesis project entitled Localisation in Ad Hoc Networks using Infrared Channels.
Infrared is emerging as a leading candidate for providing secure gigabit indoor communications such as high speed internet to portable devices, and localisation would form an important part of such systems, increasing throughput through directional routing and preventing interference between devices.
The aims of this project were to implement an iterative localisation algorithm for infrared ad hoc networks, test its performance on a real world testbed of infrared wireless nodes and make recommendations as to improvements. The algorithm presented uses a novel approach to weighing position estimates in order to mitigate the effects of error propagation.
Key outcomes include the successful fabrication of a small testbed of nodes, and the implementation of the localisation algorithm in code. The localisation code was found to calculate bearings with reasonable accuracy. Position estimation was shown through simulation to work onsuccessful packet reception, however due to packet collisions there were problems in achieving it on the testbed.
This report provides a background on localisation in wireless ad hoc networks, details regarding the fabrication and operation of the infrared testbed, an examination of the localisation algorithm and a walkthrough of the code produced. Results of tests and recommendations as to future work are also provided.
ii
Acknowledgements
I gratefully acknowledge the support and guidance of my supervisor Dr. Y. Ahmet Şekercioğlu, whowas always enthusiastic and ready to provide me with assistance. His support even extended to providing me a laboratory to work from over the summer, for which I am very grateful.
I would also like to give special thanks to postgraduate student Joseph Violi, designer of the Sens-r infrared node and architect of the localisation algorithm featured in this project. His assistance was invaluable and his patience with my constant questions much appreciated.
I would further like to thank the ECSE electronics workshop staff for their assistance and advice regarding construction of the Sens-r testbed and various other electronics problems I came across.I wish also to thank my lab colleagues and fellow students, particularly Terry Truong for his advice on soldering & PCBs and David McKechnie for his useful summary on using the Atmel AVR port of µC/OS-II and for lending me his more compact RS232 to TTL circuit.
Lastly, I would like to thank my family and friends for their continued support.
iii
Glossary
AHDWL Anchor Hop Distance Weighted LocalisationCRC Cyclic Redundancy CheckDA Destination AddressEWMA Exponential Weighted Moving AverageGCC GNU C CompilerGNU GNU’s Not UNIX (OS)GPS Global Positioning SystemIC Integrated CircuitIDE Integrated Development EnvironmentIR InfraredIRQ Interrupt RequestISP In System ProgrammerISR Interrupt Service RoutineJTAG Joint Test Action GroupLED Light Emitting DiodeLSB Least Significant BitMAC Media Access ControlMANET Mobile Ad Hoc NetworkMSB Most Significant BitMUTEX Mutual Exclusion SemaphoreNAV Network Allocation VectorOS Operating SystemPC Personal ComputerPCB Printed Circuit BoardPWM Pulse Width ModulationRAM Random Access MemoryRF Radio FrequencyRSS Received Signal StrengthRSSI Received Signal Strength IndicatorRTOS Real Time Operating SystemRx ReceiverSA Source AddressSLIP Serial Line Internet ProtocolSRAM Static RAMTTL Transistor-Transistor Logic, Time to LiveTx TransmitterUART Universal Asynchronous Receiver/TransmitterUSART Universal Serial Asynchronous Receiver/TransmitterUSB Universal Serial BusWSN Wireless Sensor Network
iv
Table of Contents
Summary ....................................................................................................................................i
Acknowledgements.................................................................................................................... ii
Glossary .................................................................................................................................... iii
Table of Contents ...................................................................................................................... iv
1. Introduction ...........................................................................................................................1
1.1 Background & Motivation..........................................................................................................11.2 Objectives...................................................................................................................................11.3 Report Overview ........................................................................................................................1
2. Background ............................................................................................................................3
2.1 Wireless Ad Hoc Networks.........................................................................................................32.2 Infrared Communications ..........................................................................................................32.3 Localisation in Wireless Ad Hoc Networks.................................................................................4
3. Sens-r Testbed........................................................................................................................7
3.1 Overview ....................................................................................................................................73.2 Fabrication ...............................................................................................................................103.3 Issues & Recommendations.....................................................................................................11
4. Sens-r Based Localisation......................................................................................................12
4.1 Bearing Calculation ..................................................................................................................124.2 Position Estimation ..................................................................................................................154.3 Anchor Hop Distance Weighted Localisation...........................................................................194.4 AHDWL Implementation ..........................................................................................................20
5. Code Implementation...........................................................................................................22
5.1 Overview ..................................................................................................................................225.2 Communications Board............................................................................................................22
5.2.1 USART & SLIP Protocol ......................................................................................................225.2.2 Control of Transmission Power.........................................................................................245.2.3 Control of Transmitter & Receiver Arrays ........................................................................245.2.4 Commands from Mainboard.............................................................................................255.2.5 IR Packet Encoding & Transmission ..................................................................................255.2.6 IR Packet Reception & Decoding.......................................................................................27
5.3 Mainboard................................................................................................................................295.3.1 PC Communications ..........................................................................................................295.3.2 Control of IR Communications..........................................................................................305.3.3 Idle & Hello Sequence Tasks .............................................................................................315.3.4 Communications Task .......................................................................................................325.3.5 Neighbour Table Update Packet .......................................................................................33
v
5.3.6 Neighbour & Bearing Table Management ........................................................................355.4 Localisation ..............................................................................................................................36
5.4.1 Bearing Calculation ...........................................................................................................365.4.2 Position Estimation ...........................................................................................................375.4.3 AHDWL ..............................................................................................................................37
5.5 Brief Walkthrough....................................................................................................................38
6. Testing & Evaluation.............................................................................................................40
6.1 Bearing Testing.........................................................................................................................406.2 Position Calculation..................................................................................................................426.3 System Evaluation....................................................................................................................45
7. Conclusions & Recommendations .........................................................................................46
7.1 Conclusion................................................................................................................................467.2 Recommendations for Future Work ........................................................................................46
References ...............................................................................................................................48
Appendix A: Test Results ..........................................................................................................51
A.1 Bearing Test Results.................................................................................................................51
Appendix B: Mathematical Work..............................................................................................59
B.1 Equation 4.16 Proof .................................................................................................................59
Appendix C: Testbed Supplement .............................................................................................60
C.1 Sens-r Components List ...........................................................................................................60C.2 Sens-r Circuit Diagrams............................................................................................................61C.3 Additional Components List.....................................................................................................64
Appendix D: Project Code.........................................................................................................65
D.1 Configuration & Settings .........................................................................................................65D.1.1 Fuse Settings.....................................................................................................................65D.1.2 Library Settings .................................................................................................................66
D.2 Sens-r Communications Board ................................................................................................67D.2.1 atmega168.c .....................................................................................................................67
D.3 Sens-r Mainboard ....................................................................................................................80D.3.1 os_cfg.h.............................................................................................................................80D.3.2 includes.h..........................................................................................................................83D.3.3 atmega128.c .....................................................................................................................84
1
1. Introduction
1.1 Background & MotivationWhilst in the past relegated to low data rate applications, infrared and visible light systems are now being looked at as a way of providing high speed wireless indoor communications. In such a system the apparent shortcomings of infrared; being short range and requiring line of sight would provide protection from eavesdropping and interference whilst the high directionality of IR transmitters could be utilised to increase throughput. Infrared is also a viable means ofcommunication in aquatic environments, as clean water provides relatively low attenuation of infrared light.
Indoor mobile ad hoc networks (MANETs), like teams of cleaning robots or wireless sensor networks (WSNs) such as surveillance camera systems, could also employ infrared communications. In many such networks localisation is required to allow nodes to determine their positions (either relative to each other or absolutely). High level applications often require at least relative localisation information in order to perform tasks. The focus of this project is the implementation of a localisation scheme which reduces the error accumulation that occurs in other iterative localisation methods.
1.2 ObjectivesThe main aim of this project was to implement and test the performance of the localisation algorithm for infrared ad hoc networks outlined in the paper Incorporating Multiple Estimates for Accurate Localization in Infrared Ad Hoc Networks [1]. The algorithm takes an anchor hop count based approach to weighting position estimates in order to reduce the effects of error propagation found in iterative localisation schemes. Localisation code was written and tested on a testbed made up of non mobile Sens-r infrared nodes [2] which were built as part of the project.Based on these tests, an assessment of the algorithm and recommendations were made.
The localisation scheme outlined in this report is unique in two ways, firstly in the way position estimates are calculated without the need for signal strength indicators and secondly in the way position estimates are combined to reduce error caused by error propagation over multiple hops.
This project aims to further contribute to research in the areas of localisation and optical ad hoc networks. It builds upon the work done in the paper Incorporating Multiple Estimates for Accurate Localization in Infrared Ad Hoc Networks by moving from a simulated to a real world test platform, and provides an evaluation of the scheme with recommendations as to improvements.
1.3 Report Overview
2
The report first gives a background in section 2 on wireless ad hoc networks, infrared communications and localisation techniques. Details regarding the fabrication and operation of the Sens-r infrared testbed are given in section 3 (with schematics and component lists provided inAppendix C). The localisation scheme including bearing estimation, position estimation and the weighting algorithm is covered in section 4, whilst the actual code produced for the testbed is examined in detail in section 5 (and provided in Appendix D). Results of testing and an evaluation of system performance are provided in section 6, followed by conclusions and recommendationsin section 7. Any questions or clarifications about this report are welcomed by the author.
3
2. Background
2.1 Wireless Ad Hoc NetworksA wireless ad hoc network is a system of autonomous nodes which form a decentralised communications network. Wireless communication allows for a dynamic network topology where new nodes can be rapidly introduced and likewise rapidly removed. Nodes act as both host and router, performing tasks and forwarding messages to each other. For example, mobile nodes canform dynamic networks where they are linked with their nearest neighbours; nodes which movetoo far from their neighbours might lose connection but then come into contact with other nodes and begin talking, changing the network topology.
There is a need then for ad hoc networks to be able to adapt quickly to change. This requires efficient routing protocols so nodes can communicate new information over multi-hop paths consisting of possibly several links. The use of wireless communication also requires nodes to cope with noise and interference as well as sharing limited bandwidth.
There are two major types of wireless ad hoc networks; smart wireless sensor networks (WSNs) and mobile ad hoc networks (MANETs), both of which have great potential to perform tasks cooperatively in unpredictable environments. A wireless ad hoc sensor network consists of several (typically stationary) sensor nodes spread across a geographical area which can be used to take measurements, detect events of interest, and even classify and track objects. In a military sensor network it could be possible to detect an enemy tank, classify it, measure its speed and track it across the network. MANETs are ad hoc networks with mobile nodes. There are several diverse applications for MANETs, one being to establish efficient, robust lines of communication in disaster or military situations. The ability to configure and deploy nodes quickly make ad hoc networks suitable for such scenarios.
Robotic swarms are another area in which wireless ad hoc networking can be applied. In all of these applications ad hoc networks are required to be self organising, robust and energy efficient as well as of low cost and minimal complexity.
2.2 Infrared CommunicationsInfrared (IR) communications have traditionally been used in indoor, short range, low data rate applications such as in communications between computer peripherals and remote control of home devices. Messages are transmitted by encoding a modulated signal and emitting it via an IR LED. The LED focuses this into a narrow IR beam providing high directionality. Line of sight between the transmitter and receiver is required for messages to be received.
Whilst this line of sight requirement might first be seen as a disadvantage, it can be used to prevent eavesdropping as well as outside interference, providing increased security and reliability.
4
It also allows for independent networks to be placed in adjacent areas without interference (provided no line of sight between them). A similar set up using RF wireless networks would require different operational frequencies and would still be susceptible to eavesdropping from the outside. IR also has the advantage of being low cost by employing relatively cheap components.
Whilst previously limited to low data rate applications, infrared and visible spectrum communication systems are now being researched as a means to provide high speed indoor communications, such as high speed internet to portable devices. In addition to preventing eavesdropping, the high directionality provided by IR reduces interference between nodes and can increase network capacity by allowing multiple packets to be transmitted simultaneously. Energy consumption would also be reduced by directing transmission power only at the desired recipients.
Several optical wireless ad hoc networks have been developed at research institutions around the world (for example Pushpin nodes [3] and Moorebots [4]) especially in the area of indoor ad hoc networks. An application of such networks could be a team of indoor vacuums that can communicate to effectively coordinate the cleaning of large areas. Optical wireless ad hoc networks could also be used in aquatic environments as clean water provides a relatively low attenuation medium for near visible and visible light. In fact, Festo have developed an underwater robot called the AquaJelly [5], which swims like a jellyfish and communicates via infrared channels.
Figure 2.1: The Festo AquaJelly communicates underwater via an arrangement of 11 IR LEDs.
2.3 Localisation in Wireless Ad Hoc NetworksMany applications in ad hoc networks require localisation to determine node positions (either relative to each other or absolutely). Many higher layer applications require at least relative localisation information to perform tasks such as geographical routing, location based addressing and sensor mapping. In the case of MANETs localisation information is particularly critical as nodes must be able to determine their locations and that of their neighbours to effectively interact with each other and coordinate tasks.
5
Figure 2.2: A wide range of higher layer applications depend on localisation information.
An obvious way to achieve localisation would be to use existing infrastructure such as GPS, but in a network of multiple nodes such a solution would increase node complexity and costs as well asreducing lifespan. Also such a system would be limited to the infrastructure that supports it, for example GPS is often only accurate for outdoors with clear weather. A more autonomousapproach is for nodes to determine their positions from their immediate neighbours, forming a distributed localisation network which has the added advantage of energy efficiency.
There are two families of localisation techniques for wireless ad hoc networks. Namely, range based techniques in which nodes measure distances or angles to other reference nodes to determine their position and range free techniques where nodes are not required to do so, relying on connectivity information to determine node positions. Infrared communications are suitable for range based localisation as angles can be readily calculated due to the high directionality of IR transceivers.
The procedure for range based localisation can be broken into three stages. Firstly nodes must determine their distances and bearings to other reference nodes. Then using this information, by geometric principles (triangulation for example), a position estimate can be calculated. A localisation algorithm is used to determine how position estimates are shared and combined to produce accurate localisation across the network.
Often in an ad hoc network a small number of nodes will know their absolute position exactly, being either pre-programmed and stationary or having access to outside localisation means such as GPS. These nodes are commonly referred to as anchor nodes, acting as fixed known points in the network. In many ad hoc localisation schemes, nodes adjacent to the anchor nodes can determine their positions, and following this their neighbours can do likewise. In this way localisation information can propagate through the network over time (iterative localisation). The goal of such schemes is to allow nodes with unknown locations to make accurate, stable and quickly converging position estimates. A problem with iterative schemes is the potentialaccumulation of errors at each hop away from the anchor nodes, which can render position estimates unreliable in large networks.
The localisation method implemented in this project is unique in two ways. First, explicit RSSI information is not required to determine distances between nodes; nodes only need to take
6
bearing measurements. This is done via the circular transmitter and receiver arrays on the Sens-r infrared nodes which form the testbed for the project. Second, an anchor hop distance weighting localisation (AHDWL) algorithm is applied to mitigate the effects of error propagation that occur the further a node is from an anchor node.
7
3. Sens-r Testbed
This section provides details regarding the construction and operation of the testbed used in this project. Component lists and Sens-r circuit diagrams are provided in Appendix C.
3.1 OverviewThe Sens-r node was designed as a low cost node to do angle based infrared localisation. Each node consists of two circuit boards; the communications board which allows nodes to communicate via infrared links and the mainboard which is designed to perform higher level application tasks (in this case localisation).
Figure 3.1 Non mobile Sens-r node top view.
Both boards are driven by Atmel 8-bit AVR microcontrollers which provide a range of useful on-chip functions such as timer-counters, pulse width modulation (PWM) outputs, analog to digital conversion (ADC), serial (USART) communications and pin interrupts. There is a well maintained open source development toolkit [6] based on the GNU C Complier and a very active online support community [7] for AVR devices. The number of processors in the AVR family means that projects can be readily upgraded to use more capable chips without significant changes to the project code.
The communications board consists of a circular arrangement of sixteen directional IR LEDs and eight BRM-1030 infrared receivers [8], controlled by an ATmega168 microcontroller. The board works in the physical and data link layers, providing infrared links between nodes and passing packets via USART to the mainboard which performs higher level functions. Commands are likewise received from the mainboard via USART.
8
Figure 3.2: Components of the communications subsystem.
The infrared transmitter LEDs are arranged in a circular layout and wired up in a 4 × 4 matrix, which is fed a 38 kHz supply signal (PWM_IR) in order to modulate transmissions (the BRM-1030receivers require a 38 kHz carrier frequency to decode signals). The LEDs are wired such that the horizontal rails form quadrants (A, B, C & D), and the vertical rails ground paths for the LEDs in those quadrants (W, X, Y & Z). The horizontal rails & LED voltage supply are controlled by PNP transistors (requiring inverted logic signals) and the vertical rails by NPN transistors connected to ground.
a) Component layout. b) Switching logic.
Figure 3.3: Communications subsystem’s IR transmitter configuration.
In order to ensure transmitter power remains consistent it is a requirement that only one ground transmitter can be set at a time. This allows for up to four independent messages to be transmitted simultaneously, spaced 90° apart, at the same transmission power, and also ensures that adjacent transmitters do not interfere with each other. The localisation scheme in this project requires adjustable control of transmission power; this is done by using a variable voltage regulator where output voltage is controlled by a PWM signal (PWM_V) from the ATmega168. Increasing the duty cycle of this signal increases the voltage output (incidentally the transistors will not switch for an output voltage of much greater than 5V, which corresponded to a duty cycle of around 80% in the testbed setup). This voltage is run through a series resistor & LED combination, and as only one ground transistor can be set, the result is a linear relationship between current and supply voltage. Given a constant voltage drop across the LED this means transmission power is directly proportional to duty cycle of the signal PWM_V.
Messages can be received simultaneously on each of the eight infrared receivers. As mentioned
9
earlier, the BRM-1030 receiver modules only detect signals modulated at around 38 kHz. This helps to eliminate interference from other infrared sources such as the sun, although some outside sources might also be modulated (fluorescent lighting for example). The receiver module’s idle state is logic hi (5V in this case), meaning received messages will be inverted (which needs to be accounted for when decoding).
The mainboard is powered by an ATmega128 microcontroller which consists of more features and significantly more memory than the ATmega168. In this project the mainboard is used to maintain tables regarding immediate neighbours, do localisation calculations, send commands to the communications board as well as act on packets received and output data to a PC. The board employs the Micrum μC/OS-II RTOS [9] to manage processor usage across multiple tasks (whereas the communications board is run by interrupt driven C code).
In terms of chip IO, two serial UARTs are used for communications, four general IO pins are used as inputs from a DIL switch and a further eight used as outputs to some LEDs for debugging purposes. Also, whilst not utilised in this project the mainboard provides an ADC, an eight pin GPIO port, control circuitry for an external motor and IO for optical wheel encoders. This is a scalable board which allows for Sens-r nodes to be used in research requiring mobile nodes, and even allows for swapping infrared with other communications systems such as wireless [10].
Figure 3.4: Components of the main processing subsystem utilised in this project.
Serial communications are done relatively simply between the mainboard and communications board as both ATmega chips provide onboard USART functions based on transistor-transistor logic. To communicate with a PC however, requires RS232 compliant logic levels which transistor-transistor logic does not provide. So a RS232 to TTL circuit (see figure 3.5) was made as part of the testbed. Messages from the mainboard could then be received at the PC by using a serial port terminal (HyperTerminal was used in this project).
Figure 3.5: RS232 to TTL converter circuit used for sending messages to PC.
The AVR Studio 4 IDE [11] was used to simulate the chips and to program two boards via JTAG port using an AVR ISP MkII (sourced from Soanar). The programmer comes with a 6 pin plug, and so
10
required making up a 6 pin ISP to 10 pin JTAG crossover cable like that in figure 3.6 below.
Figure 3.6: 10 pin JTAG to 6 pin ISP cable used for programming.
3.2 FabricationOne of the key outcomes of this project was a testbed of five non mobile Sens-r nodes to provide a real world test platform for the localisation scheme. The Sens-r circuit boards are two layer milled PCBs, the mainboard rectangular (80×60mm) and the communications board circular (ф80mm). A two dimensional PCB milling machine was used to mill away copper foil from the top and bottom layers, leaving the desired copper tracks separate from the rest of the board. Vias and through holes were drilled to allow connections to be made between the top and bottom layers.
The mainboard consists of a large number of surface mount components including resistors, capacitors, LEDs and the ATmega128. Pin connectors, the reset switch and the motor drive IC were through hole soldered.
Figure 3.7: Sens-r mainboard.
All components on the communications board (with the exception of the voltage regulator) were through hole components. The sheer number of components and via holes made soldering the communications boards quite a challenge (see figure 3.8b).
11
a) Top view. b) Bottom view.
Figure 3.8: Sens-r communications board.
The following changes were made from the Sens-r original schematics (which are given in Appendix C.2). On the mainboard L293D ICs were used instead of L293 ICs, making the diodes specified for motor control in the original circuit diagrams redundant. Also all unused IO such as the ADC and GPIO ports were not given pin connectors. On the communications board an additional decoupling capacitor was soldered across the ATmega168’s vcc and ground input pins. Also, the original PCB layout had a track which was not able to be milled, which was fixed by soldering a new path using low gauge wire.
3.3 Issues & RecommendationsConstruction was made difficult by the use of milled copper PCBs, as a significant amount of effort was required to ensure that the milled tracks would not short circuit with the board at soldered junctures. The amount of through holes and vias required, particularly for the communications board, made this a significant problem. Another difficulty was the oxidation of the copper over time due to handling, this made soldering more difficult (later found to be improved by sanding with emery paper). The performance of the Sens-r nodes was adequate, although sometimes unreliable due to the quality of the circuit boards.
Debugging and testing of code on the nodes was quite a slow process, as WinAVR does not come with support for μC/OS-II. Code would first have to be compiled to satisfy AVR and C code requirements, then uploaded to the Sens-r mainboard and then during program execution error messages specifically added to the code would be output via USART to the PC terminal. This process would then be repeated until successful execution was achieved, making debugging a tortuous process. There were also many occasions when code would stop working and produce inexplicable results due to a lack of program memory, although later in the course of the project significant savings in data memory were achieved to prevent this.
In order to create a large network, it would be wise to alter the node design to a single communications board with a USB port. Higher level tasks could be done via PC by simulating the role of the mainboard, without worrying about lack of memory (which was a major concern in writing code for this project). Multiple nodes could be controlled by a USB hub and data from each recorded, providing a complete account of the state of nodes in the network over time. Most importantly the time taken to perform debugging would be significantly reduced.
12
4. Sens-r Based Localisation
The localisation algorithm in the paper Incorporating Multiple Estimates for Accurate Localization in Infrared Ad Hoc Networks [1] forms the basis of the scheme presented in this section, with some modifications. In this scheme nodes form a distributed ad hoc network, sharing localisation information (position and bearing data) with their immediate ‘1 hop’ neighbours. Relative bearing estimation forms the basis of this range based localisation scheme.
The scheme requires a small number of nodes in the network to know their positions absolutely (at least two anchor nodes are required to allow initial position estimates to be made). Once initial estimates have been made other nodes not adjacent to the anchors can make position estimates, followed then by their neighbours. The iterative localisation scheme presented here makes use of an anchor hop distance weighted localisation (AHDWL) algorithm to weigh position estimates based on node hop count from anchors in order to reduce the effects of error accumulation from the anchors.
Localisation can be broken down into three stages. First, nodes determine bearings to their immediate neighbours. Then, once they have obtained bearing and position data from other nodes they are able to calculate a position estimate via intersection of circles, new information received is used to form new position estimates. Finally, position estimates are combined using the AHDWL algorithm in order to accurately determine a node’s current position
The localisation scheme implemented in this project is unique in two ways. First, explicit RSSI information is not required to determine angles or distances between nodes. Bearing measurements are taken from sequences of messages sent over a range of power levels, via the circular transmitter/receiver arrays on the Sens-r nodes which form the testbed for the project. Second, anchor hop distance is used as a measure to weight and combine position estimates in order to reduce error propagation across the network.
4.1 Bearing CalculationThe BRM-1030 receiver module is a low cost IR receiver, used in typical low data rate applications such as remote controls. It is an integrated package which buffers the received signal through amplifier, band pass filter (centred at 38 kHz), integrator and comparator stages to obtain a digital signal. It is not possible to measure the properties of the original signal with such a device, particularly received signal strength (RSS), having passed the signal through this multistage system.
The characteristics of the BRM-1030 however allow for channel connectivity to be used as an indirect measure of RSS. Previous testing [12] found that for a series of transmitted pulses the device will either receive almost all pulses or none at all, a brick wall where the device is either connected or not. There was also shown to be a clear relationship between transmission power, transmitter and receiver angles. Connection quality reduces for greater angle between the
13
transmitter and receiver, thus requiring greater transmission power for a packet to be detected (or a reduced distance between them). This finding combined with the arrangement of the communications board can be used to do a sort of ‘indirect measurement’ of RSS, utilising transmission power to calculate bearings.
The method is similar to Hoyt, McKennoch and Bushnell’s approach [13], where the centroid of the received signals is calculated by taking their power levels into account. However, theirapproach involves taking explicit RSS measurements at the receivers of a signal transmitted at constant power. In this method, rather than take RSS measurements of a constant power signal, a sequence of ‘hello’ packets at increasing transmission power is sent in all directions and connectivity of the receivers used to determine the neighbour’s bearing (relative to an arbitrary 0° bearing) as follows:
N
nn
N
nnn
ij
PP
PP
1max
1max
(4.1)where, ij = bearing of node j relative to node i
N = total number of receivers which received a message from node jPmax = maximum power level at which a messages in the hello sequence are transmittedPn = minimum transmission power at which a message was received on Rxn
n = local bearing of receiver relative to 0° bearing
The centroid of the received sequence is found by recording the lowest transmission power of the received packets for each receiver (transmission power is encoded in the packet), and giving greater weighting to those receivers that received packets at lowest power. The distance between nodes is not critical as the circular arrangement of the transmitter and receiver arrays meanssymmetric pairs will form, effectively cancelling each other out in the above equation (4.1).
There are two ways in which bearings can be estimated using this technique. The first is passive reception of the packet sequence, where the lowest transmission power packet correctly decoded on each receiver is recorded and put into equation 4.1 above to obtain a bearing estimate. The second method is active reception, where nodes that receive hello packets send hello replypackets back to the transmitting nodes (where the estimate is made) indicating what packets have been received and from which transmitter diode. This would provide for more accurate estimates given that the Sens-r node has double the number of transmitters as receivers, but the transmission of reply messages from several nodes would require quite sophisticated media access control (MAC) in order to prevent collisions. On the other hand, passive reception at the receiver does not require replies to be sent, reducing the likelihood of collisions and can determine its bearing by simply listening for hello packets.
In this project passive bearing estimation was used. Note that while MAC is not required to prevent collisions between hello reply messages, it is still needed to prevent collisions between hello packet sequences. It was found during testing that sequences transmitting simultaneously can corrupt the hello packets such that they are not decoded correctly, which can cause bearing estimations to become very inaccurate.
To reduce variation in bearing estimates a simple moving average filter can also be applied to local
14
bearing calculations going back a number of estimates. Doing so, whilst reducing variation, would also reduce the responsiveness of the localisation scheme to moving nodes. The effects of implementing both moving average and median filters to bearing calculations are investigated in section 6.1.
It was discovered that there was a problem using the bearing calculation method outlined so far for hello packets received about the 0° bearing. Consider the following example; packets are received at the local 0°, 45° & 315° bearing receivers (note all angles in localisation are taken clockwise) with minimum transmission powers (duty cycle) of 8, 40 & 32 respectively. Given a maximum transmission power of 80 the bearing is found as follows:
80 8 0 (80 40) 45 80 32 315
80 8 (80 40) 80 32ij
(4.2)16920
105.75160ij
(4.3)
Clearly there is a problem, as the actual result should be around 0°. In this case a valid solution is obtained by using -45° instead of 315° in equation 4.2, the two symmetric bearings will then average to around 0° rather than 180° resulting in a bearing of -2.25° or 357.75°. However, if one tries to use a bearing range of (-180°, 180°] a similar problem will occur about 180°.
The following method was devised to obtain an accurate solution across the full bearing range. After a full hello packet sequence has been received, the receiver which successfully received a packet at the lowest (non zero) transmission power has its local bearing recorded and is assigned temporarily as the 0° bearing. The other receivers are then designated clockwise from the 0° receiver as 45°, 90°, 135°, 180°, -135°, -90° and -45°. Using these designated bearings a ‘delta’ bearing estimate can be made, which represents the bearing from the closest receiver (equation 4.4). Adding this delta bearing to the recorded local bearing yields the actual bearing estimate(equation 4.6).
Consider the following example where a transmitter node is at a bearing of 130° to the receiver. Maximum transmission power is again at 80% duty.
Rx ID Tx Power (D%) Local Bearing Assigned Bearing (φn*)0 - 0° -135°1 - 45° -90°2 26 90° -45°3 21 135° 0°4 38 180° 45°5 - 225° 90°6 - 270° 135°7 - 315° 180°
Table 4.1: Sample bearing calculation data.
The 135° local receiver is designated temporarily as the 0° bearing (and recorded as φ0°). The other receivers are assigned bearings in sequence (φn*). From this table the delta and final bearings are calculated as follows:
15
max
max
*
n nij
n
P P
P P
(4.4)540
5.62596
ij
(4.5)
0 ij ij
(4.6)5.625 135 129.375
ij
(4.7)
This results in an angle estimation error of around 0.6° in this case.
4.2 Position EstimationTrilateration is a method for calculating the intersection of three circles, given their centres and radii. For localisation purposes this means a node can determine its position if it knows the positions of three other nodes and their relative distances. The Sens-r node however in addition to obtaining position information can calculate the bearings of its neighbour nodes (see section 4.1), and given that information then determine the discrimination angles between nodes. This allows position estimates to be made with only two reference nodes of known position.
Figure 4.1: Triangle with two neighbour nodes A & B used by node C for localisation.
Nodes measure their relative bearings to each other with respect to a local 0° bearing. These bearings are denoted by Φij, the bearing of node j to node i. By sharing this bearing information nodes can calculate the discrimination angles of a neighbour triangle (figure 4.1) using the following equations:
( 360) mod360ACB CB CA (4.8)
( 360) mod360CBA BA BC
16
(4.9)( 360) mod360BAC AC AB
(4.10)
The positions of reference nodes A & B are also shared, from which the distance between the reference nodes ||A – B|| can be calculated. Using this and the calculated discrimination angles, node C can calculate its distance to the reference nodes A & B using the sine rule.
sin
sinCBA
ACB
A BC A
(4.11)sin
sinBAC
ACB
A BC B
(4.12)
The problem is then reduced to an intersection of two circles. Here a point P is introduced along the line AB which is perpendicular to node C.
Figure 4.2: P is a point on AB orthogonal to C.
By Pythagoras theorem:
2 2 2C A P A P C
(4.13)2 2 2
C B P B P C
(4.14)
17
Also:P A A B P B
(4.15)
From equations 4.13, 4.14 & 4.15 it is found (see Appendix B.1) that:
2 2 2
2
C B C A A BP B
A B
(4.16)
Using this result and equation 4.14:
2 2P C C B P B
(4.17)
As points A, B and P lie on the same line point P can be calculated using the ratios of PB and AB as follows:
B AP B
P B x xx x
A B
,
B AP B
P B y yy y
A B
(4.18) & (4.19)
This then allows the coordinates of node C to be calculated, by using the equation of a point perpendicular to AB for a distance of ||P – C|| away from point P. Note that as there are only two reference nodes it is an undetermined system with two symmetric solutions (as shown in figure 4.2).
1
A BC P
P C y yx x
A B
,
1
A BC P
P C x xy y
A B
(4.20) & (4.21)
2
A BC P
P C y yx x
A B
,
2
A BC P
P C x xy y
A B
(4.22) & (4.23)
To eliminate the geometrically incorrect ‘phantom’ solution, a slightly different approach was taken to that taken in the paper Incorporating Multiple Estimates for Accurate Localization in Infrared Ad Hoc Networks [1]. This disambiguation method is outlined below.
if (||xA - xB|| >= ||yA - yB||) if (xA < xB) if (θACB < 180) choose solution with smallest y else choose solution with largest y else if (θACB < 180) choose solution with largest y
18
else choose solution with smallest yelse if (yA < yB) if (θACB < 180) choose solution with largest x else choose solution with smallest x else if (θACB < 180) choose solution with smallest x else choose solution with largest x
A correct position estimate is made based on the size of the angle between the two reference nodes about the node C. The phantom elimination algorithm is run in the x or y axis, whichever provides greatest distance between the two reference nodes. This helps to increase reliability in the event of measurement errors, reducing the risk of an incorrect solution being selected. This method also works for cases where ||yA – yB|| = ||xA – xB||, as verified by testing (see section 6.2).
a) ||xA – xB|| ≥ ||yA – yB|| b) ||xA – xB|| < ||yA – yB||
Figure 4.3: Illustration of the phantom elimination method.
An additional point to note about position estimation is the requirement that only ‘well conditioned’ triangles as described by Chandra [14] should be used to make estimates. Inequations 4.11 & 4.12 the sine rule is applied to determine node C’s distance to the reference nodes, to get a rational solution we require sin θACB to be non zero. These equations and the resulting distances are very sensitive to errors in the measurement of θACB, especially when sin θACB is close to zero (at 0° and 180°).
Sensitivity to errors in angle measurement can be measured by taking the partial derivatives of equations 4.11 & 4.12 with respect to measured angle as shown below.
cos
sinCBA
CBA ACB
C A A B
(4.24)
2
sin cos
sinBAC ACB
ACB ACB
C B A B
(4.25)
‘Well conditioned’ triangles have discrimination angles that are less sensitive to errors. Error sensitivity could be used as a method to indicate the quality of position estimates made. However in this implementation, only a simple check is made before making a position estimate to ensure
19
that angles are not too close to 0° or 180°.
4.3 Anchor Hop Distance Weighted LocalisationIn a relatively dense ad hoc network nodes will have multiple neighbours from which they can make position estimates. All of these estimates must be combined into a single estimate which a node takes as its current estimated position and communicates to its neighbours. In iterative localisation schemes, as time goes on, more nodes become able to estimate their positions, these nodes then become references from which their neighbours can make position estimates. Over time, localisation information propagates from anchor nodes across the network and accuracy improves as more reference nodes become available to do position estimation.
The AHDWL algorithm utilised in this project is based on an iterative exponential weighted moving average (EWMA) algorithm. A EWMA applies weighting coefficients to data points which decrease exponentially; in localisation terms this means that the weighting for each older position estimate decreases exponentially giving greater importance to recent estimates whilst not totally discarding the older estimates. A general EWMA equation is given by:
1t t tS S Y (4.26)
1 (4.27)
where, St = current EWMA valueSt-1 = previous EWMA valueYt = latest estimate made
Note α & β are weighting coefficients that must sum to one, the larger β is the quicker the system can respond to change but at higher sensitivity to errors in estimates. For localisation, equation 4.26 can be used to continually update a node’s position each time a new position estimate is made. In that case St is its averaged new position, St-1 is its previous averaged position and Yt is the latest instantaneous position estimate made.
Iterative localisation schemes suffer from accumulation of errors at each hop away from the anchor nodes, as they rely only on immediate neighbours to do position estimation. Any position estimation errors will be passed onto neighbours and propagate across the network. This can render position estimates unreliable, particularly in large networks.
Figure 4.4: Position estimate errors can accumulate in iterative localisation schemes.
The AHDWL algorithm uses the anchor hop counts of reference nodes (how many hops each reference node is from an anchor node) to selectively weigh individual position estimates in the EWMA equation. It assumes that the further away nodes are from an anchor node, the more likely they are to have larger errors in their position estimates due to error accumulation. Likewise, the closer a node is to an anchor node the smaller the error is likely to be. Thus estimates made with
20
anchor nodes, or nodes close to the anchor nodes are considered better quality estimates and given higher weightings.
Additional weighting coefficients are added to the EWMA equation so that new position estimates will be weighted based on anchor hop count. There are many ways to go about this; in the following scheme weighting is inversely proportional to hop count.
1
2A B
Wd d
(4.28)
baseW (4.29)
where, W = weighting coefficientdA & dB = anchor hop count of nodes A & Bβbase = 2 × maximum value of ββ = weighting coefficient in EWMA (equation 4.27)
For simplicity β is given here as the average hop count of the two reference nodes. βbase represents the maximum weighting of a position estimate (in this implementation βbase is set at 0.7, so the maximum weighting is 0.35).
4.4 AHDWL ImplementationWhilst specific details as to the implementation of the algorithm are covered in section 5, a general overview of a node’s operation to achieve localisation is provided here. In order to be able to calculate position estimates a node must have both position and bearing information regarding its neighbours. As mentioned previously, hello sequence packets are used in order to determine local bearings of neighbours. In order to obtain a neighbour node’s position and bearing information a neighbour table update packet is used. Each node maintains a neighbour table of its own and its neighbour’s position, hop count and bearing information which is then used to calculate position estimates when new data is added.
Figure 4.5: Simplified state diagram of the AHDWL algorithm.
21
The operation of each node as specified in the AHDWL algorithm is as follows. Nodes are initialised and enter an idle state. After a period of no activity, the node will send a sequence of hello packets and return to being idle. If a hello sequence is received a bearing for the transmitting node is calculated and then added to the neighbour table. If a neighbour table update packet is received the information it contains is added to the neighbour table. If there is sufficient information in the table on reception of either of these packet type a position estimate will be calculated for all triangles affected by the new data and a neighbour table update packet will be sent. In this way node position estimates should quickly converge.
22
5. Code Implementation
The following section provides an overview of how the localisation scheme was implemented in code on the Sens-r testbed, the code itself is provided in Appendix D. Whilst there isn’t time to provided a detailed explanation of every line of code here, key functions are highlighted and explained in order to provide the reader with a detailed understanding of how each system block works and how these blocks interact to form the final system and do localisation. Note that all code snippets in this section are pseudocode representations of the actual code.
5.1 OverviewThe communications subsystem is required to do asynchronous detection and decoding of packets, packet transmission with clock information embedded as well as USART communications. This meant producing Atmel based C code, which was completely interrupt driven, in order to control the transmitter and receiver arrays. For the main processing subsystem a real time operating system was required to do multitasking and share processor usage and resourcesbetween tasks. The RTOS employed in this project was the Micrium μC/OS-II real time kernel.
In order to run μC/OS-II on the mainboard both the source code and an AVR port were required. Version 2.60 of the source code was obtained from the CD which came with the book MicroC/OS-II: The Real-Time Kernel [9], the AVR port by Julius Luukko [15] was utilised to customise the OS to run on the ATmega128 microcontroller. The WinAVR toolkit [6] was employed to provide a GNU C compiler and C runtime library [16]. AVR Studio 4 [11] was used both as a GUI for the compiler and for programming the boards via an AVR ISP MkII USB programmer.
5.2 Communications Board
5.2.1 USART & SLIP Protocol
USART is used to relay packets received via infrared to the mainboard, as well as to receive and act on commands sent by the mainboard. The USART hardware provided by the AVR chips makes serial communications relatively easy to implement. There are two read buffers and a shift register for receiving bytes as well as a write buffer and shift register for byte transmission. Data is read from the receive buffer and written to the transmit buffer by reading and writing to the USART data register UDR0. Functions are also provided for parity check bit generation, different frame formats and there are recovery units for asynchronous data reception. Transmit complete, receive complete and transmit buffer empty flags and interrupts are provided as well as frame, data overrun and parity error flags.
For all serial communications in the code a BAUD rate of 19200 bytes per second is used, with a frame format of eight bits, single start (St) and stop (Sp) bits (for synchronisation) and no parity.
23
Figure 5.1: Serial frame format.
The code for transmitting bytes is a simple polling routine where the data register empty flag must be set before writing to the buffer.
// send byte on USARTwhile (!(usart data reg empty))NOP
set uart data reg (UDR0) = byte to send
Serial receive and transmit buffers are provided in the code to allow for messages of multiple bytes to be sent and received. These packets are framed according to serial line internet protocol (SLIP) [17]. SLIP allows for variable length packets to be sent by use of an END character (0xC0) to end packet formation at the receiver.
Typically an END character is also sent at the start of a packet to flush out the receiver buffer. If there is a byte in the packet with the same value as the END character a two character code is sentinstead; an ESC character (0xD2) followed by an ESC_END character (0xD3). This will be decoded to the correct value at the receiver but packet formation will not terminate. Likewise in the case of a byte in the packet with the same value as the ESC character, a two character code is sent; an ESC character followed by an ESC_ESC character (0xD4).
END DATA… END
Figure 5.2: Serial packet framing using SLIP protocol.
At the receiver decoding SLIP protocol involves the use of an ESC character flag (in the code the most significant bit of the byte count variable is used) to indicate how the next character is to be decoded. In the event of an ESC character followed by non-control character protocol violation, the non-control character is added to the packet and the ESC character ignored.
// ISR on USART reception of a byteif (rxbyte == END)
// packet completeif(buffer pointer != start of buffer)
decode packet in buffer and perform command reset buffer pointerelse if (rxbyte == ESC)set ESC flag
else if (rxbyte == ESC_END) if(ESC flag) reset ESC flag add END char to bufferelse add ESC_END to buffer
else if (rxbyte == ESC_ESC) if(ESC flag) reset ESC flag add ESC char to bufferelse
24
add ESC_ESC to bufferelse if(ESC flag) reset ESC flagadd rxbyte to buffer
The code for transmitting bytes according to SLIP protocol is similar to the above; the byte to send is examined and then given control characters if necessary, and at the start and end of the packet an ESC byte is sent.
5.2.2 Control of Transmission Power
As stated earlier, control of transmission power is achieved by changing the duty cycle of a PWM signal (PWM_V) which controls the output power of a voltage regulator. This is achieved on the ATmega168 device by using a timer-counter in fast PWM mode. In this mode the counter counts up from 0 to a TOP value (in this case 262) specified by the ICR1 register and then resets to 0. An output is given when the count matches the value of an output compare register (OCR1B in this case), by varying this register control of duty cycle is achieved. Transmission power is directly proportional to the duty cycle.
Note that the duty cycle is restricted to a maximum of 80% as the transistors which control the transmitter array cannot switch at significantly above 5V; the regulator was found to deliver 6.2V at 100% duty.
5.2.3 Control of Transmitter & Receiver Arrays
The communications board has a transmitter array of sixteen IR LEDs wired up in a 4 × 4 matrix, which is supplied by a 38 kHz signal (PWM_IR). This signal is provided by using the same timer-counter that is used for PWM of the voltage regulator, but using a different output compare pin with a duty cycle of 50%. The main clock runs at 10 MHz and the timer-counter counts from 0 to 262, which gives an output of 10 MHz/262 = 38.2 kHz.
Control of the transmitter matrix is achieved by use of a transmit mask to indicate which quadrant and which ground transistor to set for transmission of each message. The transmit mask is of the form 0bZYWXDCBA, where the DCBA bits represent the quadrants and the ZYWX bits represent the ground transistors. In order to ensure consistent transmission power only one ground transistor can be set at a time, this is accounted for in the ‘transmit byte’ function where if more than one ground transistor bit is set in the mask, only one will be set.
// set only 1 GND transistorif (transmit mask & 0x10) set W pinelse if (transmit mask & 0x20) set X pinelse if (transmit mask & 0x40) set Y pinelse if (transmit mask & 0x80) set Z pin
The transistors for the A, B, C & D horizontal rails which control the quadrants and the PWM_IR transistor are PNP type. This inverts the logic required to control these rails, although for the PWM_IR signal this doesn’t really matter as the signal will still be at 38 kHz with 50% duty. So for
25
code concerning setting the quadrant transistors (used for encoding messages) this must be taken into account, often marked by ‘PNP logic’ in the code comments.
The receiver array of eight BRM-1030 receiver modules is set up such that whenever a receiver changes state a pin change interrupt is generated (note nesting of interrupts does not occur), allowing for asynchronous and simultaneous reception of packets (as the ISR will check all receivers as to whether they have changed state). The BRM-1030 only detects signals modulated at about 38 kHz and is idle logic hi, this means that received signals will be inverted.
Details regarding infrared packet transmission, encoding, reception and decoding are covered in sections 5.2.5 & 5.2.6.
5.2.4 Commands from Mainboard
The communications board essentially acts as a slave to the mainboard, executing commands and relaying completed packets. The command packet format is as follows:
END COMMAND\QuadID DATA… END
Figure 5.3: Serial command packet from mainboard.
The mainboard can give the following commands:- SET_TO (0x30)- SET_FROM (0x40)- SET_TYPE (0x50)- SET_DATA (0x60)- SET_LEN (0x70)- SET_TX_POW (0x20)- SEND_ALL (0x10)
The command byte is split into its four most significant bits which determine which command is to be executed and its remaining four bits which determine which quadrant and transmit packet buffer it is concerned with. The SET_TO, SET_FROM, SET_TYPE, SET_DATA and SET_LEN commands set the destination address, source address, IR packet type, payload length and payload data fields of the four transmit quadrant buffers respectively. The SET_FROM command also sets the global variable self_id which identifies the node. Also note that the SET_DATA command sets the payload length automatically, as it the payload length is measured when writing the data to the buffers. This makes the SET_LEN command largely redundant except in the case of an empty data payload.
The SET_TX_POW command sets the duty cycle of the signal which controls the output from the voltage regulator, calling the transmission power function and setting the global variable ir_tx_power which is appended to the IR packets. The SEND_ALL command tells the communications board to set the transmit mask (which determines which transmitters and quadrants are to send) and to then frame & send the packets in its buffers.
5.2.5 IR Packet Encoding & Transmission
Each quadrant has a transmit buffer for packets to be sent. On reception of a SEND_ALL command from the mainboard messages will be sent on each buffer according to the transmit mask specified. This sets one of the ground transistors and enables a timer interrupt which is used by an
26
interrupt routine to send the bits.
To provide synchronisation capability at the receiver, clock information needs to be included in the framing of each byte. Originally, RC5 [18] was examined as a method for framing bytes, where each byte consists of two start bits and Manchester encoding to embed clock information. While perfectly feasible, a simpler encoding and decoding method using only one start bit was found to be just as effective on the BRM-1030 receiver modules.
A timer-counter at 1/64th the main clock speed counts from 0 to 78, generates an interrupt, and resets to 0. This interrupt occurs at every half bit period, this is around 500µs which makes for a maximum pulse width about 1ms, which falls just outside the desired range of the receivers of 400 to 800µs but is seen to work well. At the first interrupt the start bit (logic 1, lo to hi in Manchester encoding) is sent which begins byte reception at the receiver. Then the data bits are sent from LSB to MSB with Manchester encoding by use of a next bit buffer, which determines which transmitters need to be toggled hi or lo for the next half period (indicated by a toggle flag).
Figure 5.4: IR frame format.
At the end of the byte the timer interrupt is disabled and an inter-symbol delay of at least 1ms is waited before sending the next byte (by re-enabling the interrupt) in order to ensure the receiver is idle for at least one bit period.
// ISR on timer-counter output compare matchreset counter
if ((bit count == 8) && !(toggle flag))ABCD output lodisable timer interrupt on compare matchreset counterset bit count to 0xFF // signals send bytes function that byte has been sent
else if (start flag) // start bit logic 1clear start flagset toggle flagABCD output lo // machester logic 1 is lo to hi
else if (toggle flag)clear toggle flagXOR ABCD output with transmit mask
elseset toggle flagfor each txbyte buffer n: 0 to 3 if (txbyte & (1<<bit count)) set (next bit buffer & (1<<n))AND transmit mask with next bit bufferABCD output lo for bits set in the above result // as machester logic 1 lo to hi, logic 0 hi to loincrement bit count
In each byte buffer there could be a packet of a different length, so the ‘send packets’ function will use the send mask to tell the ISR routine which packets still have bytes to be sent by setting only those quadrants which remain to be sent.
// send packets function
27
disable rx pin change interrupts
find max length of packets in transmit quadrant buffers
for all bytes waiting in buffers i: 0 to max set send mask to ZYWX0000
for quadrants ABCD j: 0 to 3 if ((buffer message length > i) && (transmit mask bit j)) set send mask bit j else reset send mask bit j
function send bytes(send mask, bytes to send)delay(intersymbol delay)
enable rx pin change interrupts
Given the small overhead and relatively simple implementation of SLIP for serial communications it is also used for framing of infrared packets. The IR frame format used is shown in figure 5.5below.
END END END DA SA TxID TxPOW TYPE LEN DATA… CRC END
Figure 5.5: IR packet framing using SLIP protocol.
Note that three END characters are appended to the start of the frame to clear out the receiver buffers rather than just one. This is because during testing it was found that the BRM-1030receiver would often have errors in the first two bits, taking time to ‘get set’. The error rate in decoding packets correctly was massively reduced by transmitting three END characters.
The MAC header contains the destination and source addresses of the packet, the packet type and the length of the data payload, all of which are set by the mainboard. It also contains the ID of the transmitter the packet is being sent on (the transmit mask for that LED) and the transmission power, which are added automatically by the communications board. On reflection, transmitter ID and power may not always be useful and should instead be sent in the payload only when required. Packets are framed on reception of a SEND_ALL command from the mainboard. A cyclic redundancy check byte is appended to the trailer of the packet by executing an update CRC function for each new byte added to the packet.
When packets are transmitted on the LEDs they will be detected by the adjacent receivers, interfering with any packets that are being received on them. It is desirable to delay packet transmission until packet reception on the receiver is complete. To do this one would need to ensure that the mainboard knows not to give the communications board commands to change the transmit buffers; a handshaking routine could achieve this. Unfortunately there was insufficient time to realise such a scheme, so a basic solution was implemented which disables the receivers when a byte is being transmitted. This would work well provided medium access control protocols were in place to ensure that only one node can transmit in an area at a time.
5.2.6 IR Packet Reception & Decoding
Receivers are idle hi when they are not receiving any bytes. On reception of data the output from the receivers will be that of the transmitted bit stream inverted. The first bit change from the start bit serves as the starting point for sampling of the other bits on that receiver, the receiver then
28
waits for 3/4 of a bit period and samples the bit signal after the middle synchronising edge. If there is no transition by 5/4 bit time then the byte is discarded. In the code an allowance for ±10% error in the bit width is made, and thus a range of 3/4 - 10% to 5/4 + 10% is allowed. Bit width is twice the half bit interrupt count given in the transmit code, so given 10% leeway this means a count of 156 ± 15.6. This corresponds to a minimum 3/4 count of 111 and a maximum 5/4 count of 226, thus these values appear in the code.
Figure 5.6: Synchronising and sampling of IR data.
Note that because the bit stream is inverted the state of the receiver when sampled will correspond to inverse logic (an inverted logic 1 is hi to lo, so it is sampled lo). The implementation of this decoding scheme in code is a bit complicated given it must to deal with eight receivers rather than just one. To do this another timer-counter running at 1/64th the main clock speed is used as a counter to feed eight ‘virtual’ timers. Basically, whenever a pin change interrupt occurs on one of the receivers the value of the timer-counter is added to the virtual timers, and the timer-counter reset. In the event of a virtual timer overflow the virtual timer is reset and any byte being received on that receiver discarded (this is the same as for the 5/4 bit width timeout for no sync transition). When a receiver has changed input its virtual timer is read to see if it falls within 3/4 and 5/4 bit width range. If it does the sampled bit is shifted into the receiver byte buffer, and on reception of eight bits the byte added to the receiver packet buffer after SLIP decoding.
// ISR for receiver pin changeset delta as timer-counter countreset counterenable timer-counter if disabled (on overflow)
store previous receiver pin statetake new receiver pin statepin change = new rx pin XOR prev rx pin
// update virtual timersfor each timer i: 0 to 7if (rx timer[i] < 253) rx timer[i] += deltaelse rx timer[i] = 253 // overflow
for each receiver i: 0 to 7if ((rx timer[i] > 226) && pin[i] has changed) // start bit from idle hi or timeout reset bit count[i] reset rx byte buffer[i] reset rx timer[i]else if ((rx timer[i] within 111 to 226) && bit count[i] < 8) if !(pin[i]) // inverse logic set bit[i] in rx byte buffer[i] increment bit count[i] if (bit count[i] == 8) // byte received do SLIP decoding of byte
29
function ir_recv_packet(i) reset ir_rx_bit_cnt[i]
When an END character is received the receiver packet buffer is then copied and each byte passed through a CRC check. If at the end of this check the CRC byte is found to be incorrect the packet is discarded, also if the packet if found to be addressed to another node it is discarded (the DA field is neither addressed to the node or a broadcast).
// receive packet functioncopy receiver packet buffer
check CRC byteif (CRC byte is incorrect)reset buffer byte counterdiscard packet & return
if (DA == self ID OR broadcast ID (OxFF)) discard CRC field append rx ID to packet forward packet on USART to mainboard using SLIP
reset buffer byte counter
Otherwise packets are forwarded to mainboard via USART with the CRC byte removed and the receiver ID appended to it like in figure 5.7.
END DA SA TxID TxPOW RxID TYPE LEN DATA… END
Figure 5.7: IR received packet forwarded to the mainboard.
5.3 Mainboard
5.3.1 PC Communications
Serial communications with the PC are done via serial UART in order to output data and provide a debugging platform for the code. An RS232 to TTL circuit is used in order to convert the output of the mainboard to compliant serial logic levels and HyperTerminal used to receive messages at the PC.
The code for transmitting bytes over USART (to both the PC and communications board) is different from that used on the communications board, as a polling routine would waste valuable clock cycles when other tasks could be doing work. Instead messages are written to a transmit buffer, and each byte written on the execution of a USART data register empty interrupt service routine. When the transmit buffer is written to, this interrupt is enabled and disabled on no bytes remaining to be sent. Note that SLIP protocol is not used to communicate with the PC (but it is for the communications board).
// send byte to PC on USARTpend semaphore(sem pc tx) // initialised to buffer size
add tx byte to bufferif reach end of buffer point to startincrement pc tx cnt
30
enable data reg empty interrupt
// ISR for USART data reg emptystore register state
if (pc tx cnt)decrement pc tx cntput buffer byte into uart data reg
post semaphore(sem pc tx)elsedisable data reg empty interrupt
restore register state
To provide suitable formatting for output to the PC the sprintf statement is used in order to produce formatted output strings to send via USART. Due to space constraints however, it was found unfeasible to simply use sprintf statements where format strings are stored in SRAM. Instead format strings are stored in program memory by using the macro PSTR, and the sprintf_P statement is used to produce output strings to send to the PC. Static strings are also stored in program memory and sent over USART via separate PC transmit functions denoted in the code by the “_P” suffix.
5.3.2 Control of IR Communications
By issuing commands over USART the mainboard can control the operation of the communications board by setting the transmission power, the quadrant transmit packet buffers and the transmit mask which determines on which transmitter LEDs messages are sent. The commands that can be issued are: SET_TO, SET_FROM, SET_TYPE, SET_DATA, SET_LEN, SET_TX_POW and SEND_ALL, the structure and operation of which was largely covered in section 5.2.4.
Access to the ‘send command’ function is protected by a mutex to ensure that access to the quadrant transmit buffers is exclusive to the task giving the command. Also when writing a whole message to the transmit buffers the full sequence of commands required is also mutex protected (to prevent other tasks from overwriting parts of the buffers). Furthermore a delay of the period required to send a packet is introduced after giving a SEND_ALL command to ensure that the buffers are not overwritten whilst infrared transmission occurs.
// command sequence for setting & transmitting a messagepend mutex(mutex comms msg tx)
function send cmd(SET_TO, DA)function send cmd(SET_FROM, self ID)function send cmd(SET_TYPE, message type)function send cmd(SET_DATA, data pointer, payload length)function send cmd(SET_TX_POW, tx power)function send cmd(SEND_ALL, transmit mask)
delay(time to send)post mutex(mutex comms msg tx)
Note that on initialisation the mainboard must send a SET_FROM command in order to set the value of self_id at the communications board, as it is used to determine which received packets to discard based on destination address.
31
5.3.3 Idle & Hello Sequence Tasks
In section 4.4 the implementation of the AHDWL algorithm was discussed with reference to a state machine representation (figure 4.5) of the system. In the code the state diagram is effectively divided into three tasks as follows:
Figure 5.8: Division of states amongst the RTOS tasks.
There are always at least two tasks running, the idle task and the communications task. The communications task is inactive until a message is received from the communications board, where it processes the packet received and performs actions accordingly. The idle task is the lowest priority task. It is first involved with initialisation of tables and setting the node ID according to the DIL switch (a node with the DIL MSB set is identified as an anchor node and its position also set). The task then sits in a while(1) loop where it counts a random delay time of at least one minute (idle timeout) and then creates a hello sequence transmit task of higher priority.
// idle taskinitialise tablesset self idfunction cmd(SET_FROM, self id)
while (1)for i: 0 to 60 + random (0 to 64) increment debug LEDs delay(1s) function pc send(“.”)
pend semaphore(sem hello seq) // initialised to 1 create task (hello seq task)
The hello sequence transmit task sends a sequence of hello packets at increasing transmission power on all transmitters to allow neighbour nodes to determine the transmitter node’s bearing. The hello sequence packet structure is like that in figure 5.7 with no data payload; the important values are the source address and the transmission power.
END END END DA = 0xFF SA TxID TxPOW = D% TYPE = ‘H’ LEN = 0 CRC END
Figure 5.9: Hello packet framing using SLIP protocol.
32
The hello sequence task first sends hello packets on all four quadrants with one ground transistor set. It increases the power of the packets linearly by a given duty cycle step size to a maximum duty (of no greater than 80%). The transmission power duty cycle is encoded in the hello packets. This is repeated for the remaining three ground transistors. To indicate the end of a hello sequence and to allow the receiver to calculate a bearing hello packets with a tx power field set to 0xFF are transmitted on each LED. The task then posts a semaphore signalling to the idle task that it is complete and deletes itself.
// hello seq taskwhile (1)
for each ground transistor WXYZ i: 0 to 3 transmit mask = one of ZYWX bits & DBCA set pow = POWER_MIN while (pow <= POWER_MAX)
pend mutex(mutex comms msg tx) transmit hello packet(DA:ALL, SA:self id, type:’H’, txpow:pow) pow += POWER_STEP post mutex(mutex comms msg tx)
// end hello seq for each ground transistor WXYZ i: 0 to 3 transmit mask = one of ZYWX bits & DBCA set pend mutex(mutex comms msg tx) transmit packet(DA:ALL, SA:self ID, type:’H’, txpow:0xFF) post mutex(mutex comms msg tx)
post semaphore(sem hello seq) delete task(hello seq task)
5.3.4 Communications Task
The communications task is highest priority task where the majority of the work is done, taking measurements of local bearings by reception of hello packets, updating neighbour tables, sending neighbour table update packets and performing localisation. The task is typically in an idle state pending on a signalling semaphore which is posted by the ‘USART byte received’ ISR when a full packet (an END character) is received from the communications board. Note that packets must be valid (pass a CRC check) and correctly addressed to even be forwarded to the mainboard.
// comms taskwhile (1) pend semaphore(sem comms rx)copy received packet to buffer
if SA is new add it to neighbour table
if (type == ‘H’) if (tx power != 0xFF) if (!hello flag) set hello flag
else if (SA != hello node id) discard packet else use packet in hello array else
33
reset hello flag calculate bearing recalculate position estimates affected by new bearing
if (self status != ‘U’) send neighbour update packet
else if (type == ‘N’) decode position and bearing data add position and bearing data (for other known nodes) to tables if (self status != ‘A’) check neighbour table and update self hops if required do position calculation for all triangles with SA as a ref node
if (self status != ‘U’) send neighbour update packet
When a packet is received it is copied to a buffer. If the packet is from a node never seen before the source address is added to the neighbour table as a new node.
In the code a hello sequence can only be received from one node at a time. Originally consideration was given to providing capability for receiving up to two or three sequences simultaneously but was dismissed due to memory constraints. Later significant savings in memory were achieved, but there was insufficient remaining time to implement this. Therefore on receipt of a hello (‘H’) packet, if the packet is the first received the transmitting node ID is recorded and a flag set. Any hello packets from a different node will be discarded while this flag is set. The flag is unset and the hello sequence ends on receipt of a hello packet of power 0xFF, then a bearing estimate can be made. If this new bearing information affects any neighbour triangles at the receiver, new position estimates for those triangles are made (given sufficient information is available) and the current position estimate updated.
When a neighbour table update packet (‘N’) is received hop count, position and bearing data aredecoded and added to the neighbour and bearing tables. Position estimates are made for all neighbour triangles which the transmitting node forms with the receiver, given there is sufficient information to do so. Details regarding neighbour table update packets are provided in the next section.
5.3.5 Neighbour Table Update Packet
Neighbour table update packets are sent on reception of either a complete hello sequence or a neighbour table update packet, provided that the receiving node has a current valid position estimate (is of known status). A node is initialised as being either an anchor node with position specified (anchor status ‘A’) or a node of unknown position (unknown status ‘U’). The minimum requirement for sending a neighbour table update packet is a current valid position estimate (known status ‘K’) and a hop count from the nearest anchor node (anchor nodes have a hop count of zero and are recognised as such).
x1 x2 y1 y2 hops n1 ID Φn1 1 Φn1 2 … nN ID ΦnN 1 ΦnN 2
Figure 5.10: Data payload for neighbour table update packet
An encoding scheme for position and bearing data was created in order to reduce the length of packets whilst achieving sufficient accuracy. In this scheme only two bytes are used for x and y position data as well as for each bearing.
34
Position information is encoded simply as a signed integer over two bytes. For encoding say an x value it is converted from a double value in metres to a signed integer (2’s complement in C code) value in centimetres. The upper eight bits are then put into the x1 byte and the lower eight bits into the x2 byte. It is decoded by sticking these two bytes together as an integer and converting back from centimetres into metres. This means that values of up to two decimal points accuracy from −327.68m to 327.67m can be encoded. If desirable millimetre accuracy could be achieved by conversion from metres to millimetres, resulting in a range of values −32.768m to 32.767m.
A more novel approach is taken for encoding bearings. Given a possible range of bearing values from 0° up to but not including 360°, maximum accuracy can be achieved by setting 0x0000 to 0° and 0xFFFF one step below 360°.
360Step Size 0.0055
65535
(5.1)
That is, a maximum resolution with a step size per bit of 0.0055°. To achieve this bearings areencoded as follows.
65535(int)
360 nNI
(5.2)
This value is rounded to the nearest unsigned integer and then the top eight bits and lower eight bits transmitted. Decoding is achieved by inverting equation 5.2 above and forcing the division to be done as a double type after sticking the two received bytes back together as an unsigned integer.
360(double)
65535nN I
(5.3)
Upon receiving a neighbour table update packet a node will add the information contained to its bearing and neighbour tables (which are covered in the next section). If in a neighbour table update packet, there is bearing information about a node the receiver has not previously received a packet from then that bearing is ignored; even though the node is a neighbour of the transmitter it may not be a neighbour of the receiver.
A neighbour table update packet is transmitted at maximum power to all nodes in the neighbour table for which there is local bearing data. A crude method of directional routing is achieved by taking those bearings and transmitting on the nearest transmitter LEDs. As only one packet is sent to ensure a neighbour will not receive multiple neighbour table update packets (which could result in multiple position estimates being made from the same data reducing the effect of the AHDWL).
// neighbour table update packet transmissionfor all neighbour nodes in neighbour table i: 1 to 7if (local bearing[i] != NAN) find nearest quadrant ABCD find nearest LED WXYZ set transmit mask accordingly
35
pend mutex(mutex comms msg tx) transmit packet(DA:i, SA:self id, type:’N’, txpow:MAX, data:position, hops and local bearing info) post mutex(mutex comms msg tx)
5.3.6 Neighbour & Bearing Table Management
Each node maintains a table of information about itself and its neighbour nodes, so that when new data is received, updated position estimates can be made using that data and then stored in the table. The table contains information about each node’s status, hop count, current position and bearings to other nodes.
ID status x est y est hops self n1 ... nN
self self.status xself yself dself - Φselfn1 ... ΦselfnN
n1 n1.status x1 y1 d1 Φn1self - ... Φn1nN
... ... ... ... ... ... ... ...
nN nN.status xN yN dN ΦnNself ΦnNn1 ... -
Figure 5.11: Neighbour and bearing data table.
Given that there was only five nodes in the testbed with a limited amount of flash memory available it was decided to limit the size of the table to accommodate for a maximum of eight nodes (including the node on which it is stored). For a large network of nodes it would be desirable to use dynamic memory allocation rather than pre-allocating it as done in the code, but for a network of up to eight nodes this is sufficient and makes management of the table somewhat easier.
In the code this table is split up into two data structures, the first is an array of structure variables with character type fields for ID, status and hop count as well as double type fields for x estimate & y estimate, and hop count which constitutes the neighbour table. On initialisation according to the DIL switch self ID and self status fields are filled, and for an anchor node the hop count is set to zero and the x, y position data is set as specified in the code. The remainder of the node IDs are set to 0, the status fields set to ‘U’ for unknown status, the x and y fields set to NAN (not a number) and hop count fields set to 0xFF.
The second data structure is an 8 × 8 double type array of bearing variables. The row and columns correspond to the node IDs given in the neighbour table. A variable Φij (row i, column j) in the array corresponds to the bearing of node j taken from node i. For example local bearings Φselfj are all given in the first row of the array. The entire bearing table is initialised to NAN on declaration.
On reception of a packet the neighbour table is checked to see if the transmitter node is already listed, this is done by checking the source address against the node IDs in the table. If a node ID is found to match the row index is recorded (effectively a neighbour table pointer), if no node ID is found to match then the first node ID which is zero (as initialised) has its row index recorded, and the source address will be written to the node ID field. In this way new nodes are added to the table.
// add node to neighbour table if newfor rows i: 1 to 7 //self is row 0 if (node ID == SA)
36
breakelse if (node ID == 0) set node ID = SA
break
set neighbour table row pointer = i
On reception of a hello packet sequence a new bearing will be written to the first row of the bearing table which corresponds to the local bearings, to the column of the transmitter node. The majority of information in the neighbour and bearing tables comes from reception of neighbour table update packets. These packets fill out all the remaining rows of the neighbour and bearing tables by providing position, hop count and bearing data. Note that if there is bearing information about a node the receiver doesn’t have in its neighbour table, then that information is discarded. The status of the transmitting node is determined by its hop count, if it is zero then the node is an anchor otherwise it is a node of known position (position must be known to transmit a neighbour table update packet).
Hop count of a non anchor node is determined directly by checking the neighbour table. It is simply the minimum of all the hop counts in the neighbour table plus one, as these are one hop neighbours. A non anchor node can only have its status change from unknown to known when a valid position estimate is made, which requires sufficient bearing and position information from atleast two reference nodes to make a neighbour triangle.
5.4 Localisation
5.4.1 Bearing Calculation
The bearing calculation method which uses a sequence of hello packets, as outlined in section 4.1,is implemented as follows. A character type array with eight cells representing each receiver is initialised to zero before receiving the first hello packet; this array represents the transmission power of hello packets received on the receivers. When a hello packet is received on a receiver its transmission power is checked against that stored in the array. If the array value is zero or if the packet power is less than the array value, then the transmission power is written to the array. In this way the minimum power of all packets received on each receiver is recorded.
From this array the receiver which received a hello packet at the lowest transmission power is identified and recorded, this receiver is assigned as temporarily as the 0° bearing. The other receivers are assigned local bearings in sequence as 45°, 90°, 135°, 180°, -135°, -90° and -45° taken clockwise from the minimum power receiver. Using the minimum received powers and these temporary bearings, a value for the delta angle from the minimum power receiver is calculated and thus the final bearing estimate is obtained. The bearing table is then updated for this new local bearing estimate.
// calculate bearing given power arrayfor receivers ID i: 1 to 7 if (array[i] != 0) if (array[i] < hello_array[rxmin] OR array[rxmin] == 0) rxmin = i;
for receivers ID i: 0 to 7 //rxmin assigned as bearing 0
37
j = (rxmin + i)mod 8;// sequence 0, 45, 90, 135, 180, -135, -90, -45if (i<=4) rxangle = 45*i;if (i>4) rxangle = -180 + 45*(i-4);
if (array[j] != 0) { num = sum((POWER_MAX - array[j]) × rxangle) den = sum(POWER_MAX - array[j])
bearing = num/den + rxmin*45if (bearing < 0) bearing += 360
reset arrayfunction update bearing table(self, SA, bearing)
5.4.2 Position Estimation
Position estimation in the code is a direct implementation of all the equations and the phantom elimination method outlined in section 4.2, and as such does not need to be covered further here. The issue of noise sensitivity due to errors in angle measurements was also touched upon this section, and highlighted as another possible means of weighting estimates. In this implementation however only a simple check is applied to see if estimates are going to be unduly affected by noise sensitivity.
// noise sensitivity checkif (θacb, θcba or θbac fall within ±20° of 0° or 180°) return without doing a position estimate
If a discrimination angle falls within 20° of the angles 0° or 180°, this would make an almost collinear triangle with angles very sensitive to noise. In such a case a position estimate is not made, judged as being too unreliable.
5.4.3 AHDWL
For non anchor nodes position estimates are combined to produce a single current position estimate using the equations of the AHDWL weighting algorithm outlined in section 4.3.
// AHDWLset d = (n1 hops + n2 hops) + 2set beta = BETA_BASE/dset alpha = 1 - beta
if x est AND y est are validif self x and self y are NAN // update neighbour table 1st estimate
set self x = x est set self y = y estelse // update neighbour table 1st estimate
set self x = alpha × self x + beta × x est set self y = alpha × self y + beta × y est
The first valid position estimate made is written directly to the neighbour table as the current
38
estimated position. Thereafter position estimates are weighted based on the anchor hop counts of their reference nodes.
5.5 Brief WalkthroughTo assist understanding an example scenario and a walkthrough of the code response is presented. Consider a single neighbour triangle within an ad hoc network which initially consists of an anchor node (A) and two nodes of unknown position (B & C), here the response of node C is examined.
Figure 5.12: Neighbour triangle ABC which forms part of an ad hoc network.
Node C first receives a hello sequence from node A. Node A is added to the neighbour table and the local bearing of node A is calculated and added to the bearing table. Node C then receives a hello sequence from node B and again adds this information to the neighbour and bearing tables. Upon reception of this hello sequence, node A sends a neighbour table update packet which contains its own position information as well as the bearing of node B taken at node A. This position and bearing information is added to node C’s tables, and node A is identified as an anchor with a hop count of zero. Therefore node C finds its own hop count to be one.
ID status x est y est hops C A BC U NAN NAN 1 - ΦCA ΦCB
A A xA yA 0 NAN - ΦAB
B U NAN NAN 0xFF NAN NAN -
Table 5.1: Node C’s tables upon receiving a neighbour table update packet from anchor node A.
Node C then sends its own hello packet sequence having reached timeout for its idle task. This triggers an immediate neighbour table update packet response from node A which now contains the bearing of node C at node A. This new information is added to the bearing table. Later node B transmits a neighbour table update packet, with position and hop count information as well as the bearing information of nodes A, C and another node. This means that node B has been able to perform position estimation with that other node and node A as references. This information is added to the neighbour and bearing tables of node C, with the exception of the bearing of the node unknown to it, and node A is updated as being of known status.
Given there are two nodes with position data and there is bearing data to and from each node in the triangle, node C can perform a position estimate. Given that this is the first position estimate node C makes, AHWDL is not applied, but the node now becomes of known status given a valid position estimate has been made. Node C then updates its own information in the neighbour table and sends a neighbour table update packet to nodes A and B. Future packets received will then be
39
used to make new position estimates which will be weighted using AHDWL to update node C’s position.
ID status x est y est hops C A BC K xC est yC est 1 - ΦCA ΦCB
A A xA yA 0 ΦAC - ΦAB
B K xB yB dB ΦBC ΦBA -
Table 5.2: Node C’s resultant neighbour and bearing tables.
40
6. Testing & Evaluation
6.1 Bearing TestingThe aim of the following tests was to check the range and standard deviation for calculated bearings by sending hello sequences at different step sizes of transmission power (duty cycle).
Figure 6.1: Bearing test set up at a range of 1m with receiver node left and transmitter node right.
The receiver node was set up at a range of 1m from the transmitter node, and placed at angles of approximately 0°, 30°, 45°, 60°, 90° and 135°. At each angle 50 hello sequences were sent for step sizes of 2%, 4%, 6%, 8% and 10% transmission duty. Each sequence started at the given step size and increased up to a maximum duty of 80%. The following table gives a summary of the results of the tests, with the actual observations given in Appendix A.1.
Angle 2% 4% 6% 8% 10%Mean 0° 1.30 1.75 3.00 3.79 1.67
30° 23.18 23.12 23.33 24.72 19.2745° 45.87 45.70 44.34 47.30 44.7860° 66.15 65.87 64.93 65.13 63.2490° 90.86 90.71 91.24 90.99 89.99
135° 133.98 134.38 135.17 134.95 141.46Variance 0° 4.23 6.80 13.03 18.30 18.72
30° 0.62 3.26 10.24 5.96 44.9645° 2.31 8.08 20.65 42.82 42.2360° 0.76 3.65 9.22 9.24 10.9190° 10.00 10.78 10.73 15.86 19.27
135° 7.62 12.09 17.49 28.91 51.70Std Dev 0° 2.06 2.61 3.61 4.28 4.33
30° 0.79 1.81 3.20 2.44 6.7045° 1.52 2.84 4.54 6.54 6.5060° 0.87 1.91 3.04 3.04 3.3090° 3.16 3.28 3.28 3.98 4.39
135° 2.76 3.48 4.18 5.38 7.19
Table 6.1: Descriptive statistics of bearing test results at 1m range.
As one would expect, for an increasing step size a typically increasing variance and standard
41
deviation is obtained. The resolution of the calculated bearing values is also seen to reduce. These results are as expected; with a greater resolution of step sizes available more precise measurements of the minimum transmission power for receiver connectivity can be made thus giving a better centroid calculation. The majority of the tests found that the average bearing calculation was within 10° of the actual bearings as measured by hand.
Standard deviation is the best measure of dispersion given here because (unlike variance) it is given in the same units as the calculations (degrees). It can be said with 95% confidence that a bearing calculation will lie within two standard deviations of the mean. In the above results (table 6.1) the largest standard deviation was 7.19, meaning there was a 95% probability of a bearing calculation lying within ±14.4° of the mean. For applications such as directional routing, given theSens-r transmitters are placed at about 22.5° apart from each other, even this is reasonably good result; packets could be forwarded to neighbours with reasonable confidence by using the nearest three transmitters to the calculated bearing.
There is a trade off here however between accuracy and time. The smaller the step size, the greater the bearing accuracy but so to the longer the hello sequence will take to transmit. At a step size of 2% a hello sequence will take 32.8s at 200ms per packet. Likewise 4% will take 16.8s, 6% will take 11.2s, 8% will take 8.8s and 10% will take 7.2s. Consideration needs to be given to the maximum time a node can be allowed to transmit, and there is a clear need for media access control to ensure that nodes will not transmit whilst a neighbour is sending a hello sequence as a few missed packets can result in a very different bearing calculation.
A method that could be used to achieve higher accuracy with lower step sizes would be to apply a moving average or median filter to bearing calculations made. For example consider the results from the 1m range 0° bearing test for a step size of 8%, then apply moving average and median filters with a window for up to five bearing calculations.
Bearing Calc
Moving Avg
Median Filter
Bearing Calc
Moving Avg
Median Filter
Bearing Calc
Moving Avg
Median Filter
0 0 0 8.18 4.772 3.75 8.18 4.022 3.753.75 1.875 1.875 -3.75 4.022 3.75 3.75 4.772 3.753.75 2.5 3.75 -3.75 2.522 3.75 8.18 4.772 3.753.46 2.74 3.605 -3.75 1.022 -3.75 8.18 6.408 8.183.75 2.942 3.75 8.18 1.022 -3.75 3.75 6.408 8.18
0 2.942 3.75 8.18 1.022 -3.75 0 4.772 3.750 2.192 3.46 8.18 3.408 8.18 8.18 5.658 8.180 1.442 0 3.75 4.908 8.18 8.18 5.658 8.180 0.75 0 8.18 7.294 8.18 3.75 4.772 3.75
8.18 1.636 0 0 5.658 8.18 8.18 5.658 8.188.18 3.272 0 3.75 4.772 3.75 8.18 7.294 8.18-3.75 2.522 0 -8.18 1.5 3.75 8.18 7.294 8.183.75 3.272 3.75 8.18 2.386 3.75 8.18 7.294 8.18
0 3.272 3.75 0 0.75 0 0 6.544 8.183.75 2.386 3.75 8.18 2.386 3.75 3.75 5.658 8.183.75 1.5 3.75 0 1.636 0 5 5.022 58.18 3.886 3.75 3.75 4.022 3.75 - - -
Table 6.2: Moving average and median filters applied to 1m range, 0° bearing, 8% step size results.
42
Bearing Calc
Moving Avg
Median Filter
Mean 3.79 3.68 3.92Variance 18.30 4.03 11.42Std Dev 4.28 2.01 3.38Range 16.36 7.29 11.93
Table 6.3: Descriptive statistics for the moving average & median filters applied.
Both the moving average and median filters result in less variance in the bearing calculations. However applying such a filter would also reduce the responsiveness of the system to changes in node positions, as at multiple hello sequences would need to be received (depending on the size of the filter window) before reaching a new stable bearing. This would be particularly true of MANETs and a relative non issue for stationary networks.
To apply a moving average filter to bearing data in the code as given would arrays (of the filterswindow size) for each local bearing in the bearing table to store past calculations. This would be relatively simple to implement as outlined below.
// bearing calculation moving average filter (five data points)// bearing data[] initialised to NAN
for i: 0 to 4if((bearing data[i] == NAN) OR (i == 4)) breakbearing data[i] = bearing data[i+1]
bearing data[i] = new bearing
bearing = function avg(bearing data[0 to i])// for median filter// bearing = function median(bearing data[0 to i])
6.2 Position CalculationBefore implementing the code on the testbed, testing was done In order to ensure that position calculation and phantom elimination worked correctly. This was done by simulating packet reception at the mainboard, that is to say explicitly specifying packets and their contents to be decoded at the mainboard. In the code, this meant using the idle task to signal the communications task semaphore (rather than the USART ISR), and using a counter to determine what packet was to be decoded by the task.
// idle task test code1 min + random delay
post semaphore(sem comms rx)
// comms task test codepend semaphore(sem comms rx)increment packet counter
switch(packet counter) put simulated data into packet for given packet count
respond to simulated packet
43
The first set of tests for position calculation were for triangles where ||xA – xB|| = ||yA – yB||. This was because it was unclear how the original algorithm would cope with such scenarios and also because for such triangles it is easy to calculate expected results (right angled triangles with 45° discrimination angles).
Figure 6.2: Simulation test for ||xA – xB|| = ||yA – yB||.
With some modifications to the original scheme resulting in that outlined in section 4.2, a successful simulation test with neighbour nodes at equal distances in the positive and negative x and y directions was performed (as shown in figure 6.2). For each triangle combination the correct result of (0,0) was obtained.
Successful simulations were also conducted for triangles where ||xA – xB|| ≠ ||yA – yB||. One of these tests is detailed below.
Figure 6.3: Simulation test for ||xA – xB|| < ||yA – yB||.
This triangle was simulated by using the follow packet sequence.
Packet No. SA Packet Type Info1 1 H Pow: 8, Rx ID: 1 (45°)2 1 H Pow: 0xFF (end seq)3 2 H Pow: 8, Rx ID: 3 (135°)4 2 H Pow: 0xFF (end seq)5 1 N x: 0, y: 0, hops: 0, Φ1self: 225°, Φ12: 180°
44
6 2 N x: 0, y: -10, hops: 0, Φ2self: 0°, Φ21: 315°
Table 6.4: Received packet sequence for simulation test.
This resulted in the correct position estimate of (-5,-5) being made and output to the PC as shown below in figure 6.4.
Figure 6.4: Results of the simulation test as seen on PC terminal.
By doing these tests the code operation, position calculation and phantom elimination methodswere demonstrated to work given successful packet reception at the receiver. Testing then moved from simulated reception at single Sens-r node to a triangle of three nodes.
Figure 6.5: Experimental set up for position estimation with the lower left node unknown, the other two nodes as anchors.
This set up exposed a number of problems with the code as it stood. Originally after reception of a hello sequence anchor or known nodes would then immediately transmit a neighbour table update packet, however given the implementation of sending hello packets of power 0xFF on each LED to end the sequence a delay had to be introduced. This delay was of a random period to ensure multiple nodes wouldn’t transmit their neighbour table update packets simultaneously.
45
Another problem was in preventing the simultaneous transmission or partial transmission of hello sequences by nodes. Simultaneous transmissions render the packets sent useless, as they are corrupted and therefore discarded for failing CRC checks. An attempted solution to the problem was to implement a wait and random resend routine, where if a message is being received then transmission is delayed and an attempt to resend made after a random period of time.
A further difficulty involved neighbour table update packets being missed at their intended recipient nodes. This may have been caused by inaccurate bearing calculations affecting which LED the packet was to be forwarded on or simultaneous transmissions as mentioned previously.
It became quite clear, that without implementing proper media access control and directional routing, position estimation would be practically unachievable. Estimates were able to be made but the length of time it took to successfully receive the necessary packets (requiring multiple resends before a packet was received) made the process so slow as to be made redundant. Only by strictly controlling when each node was to transmit and on which transmitter LEDs wassuccessful position estimation was achieved. Given the problems in achieving position estimation for a single neighbour triangle, position testing was halted at that point and unfortunately there wasn’t sufficient time to attempt to implement a MAC scheme.
6.3 System EvaluationBy using the Sens-r testbed and code it was found that reasonable accuracy can be achieved by using the bearing calculation method outlined in this paper. Higher accuracy is achieved for sending hello sequences with smaller step sizes in transmission power, however the smaller the step size the longer it takes to transmit a hello sequence. Another method to improve accuracy is achieved by applying a moving average or median filter to bearing calculations, although this would lower the system’s response time to topology changes.
Position calculation and phantom elimination was found to work correctly through simulated reception of packets at the receiver node but problems with simultaneous transmission and directional routing made it difficult to achieve on the actual testbed. Unfortunately due to these problems, significant testing beyond making single position estimates was not achieved. This means that a real life assessment of many aspects of system performance were unable to be made. This includes for example, the effectiveness of the AHDWL as compared to just EWMA, effectiveness of different methods of weighting by hop count, examining system robustness in dealing with node failures and efficiency in obtaining new estimates given a changed node structure.
46
7. Conclusions & Recommendations
7.1 ConclusionThe primary aim of this project was to implement an angle based localisation scheme for infrared ad hoc networks which reduces error accumulation that other iterative localisation methods suffer from and doesn’t rely upon explicit signal strength indicators. The two main outcomes of the project were one, a testbed of five non mobile Sens-r nodes to provide a test platform for the scheme and two, the successful implementation of the scheme in code.
Testing of the code found that using the method outlined in this paper, bearings could be calculated to reasonable accuracy without the need for RSSI, although it takes a significant amount of time to make a measurement due to the length of the transmitted sequences. Longer sequences of greater transmission power resolution were found to produce more accurate results. Applying a moving average or median filter was also shown to improve accuracy.
Position estimation was found to work correctly by simulating reception of packets at the receiver node. This demonstrated that the localisation scheme works given successful packet reception. However there were significant problems with regard to packet reception when trying to perform position estimation on the actual testbed. This is mainly attributed to packet collisions due the lack of a media access control scheme.
The work done in this project contributes to research in the areas of localisation and optical ad hoc networks. It is hoped that this project provides a platform upon which future work in these fields can be based.
7.2 Recommendations for Future WorkClearly the immediate focus of future work should be implementing a directional media access control scheme to prevent simultaneous transmissions from neighbours interfering with each other. Without media access control simultaneous transmissions can corrupt packets which in terms of the localisation scheme, means a receiver node may miss neighbour table update packets or miss a number of packets in a hello sequence, resulting in inaccurate bearing estimations.
A possible media access control scheme would be to implement a network allocation vector as a means to do virtual carrier sensing. For example, when a node wants to transmit it could first send a packet which specifies the time it will take to send its following packets, or in the MAC header there could be an NAV duration field which tells other nodes not to transmit for that specified period. Neighbour nodes then take this value and store it in a NAV counter which decrements to zero over time, at which point they are free to try and gain the right to transmit. Random wait times might also need to be implemented to prevent collisions between initial packets.Implementing proper MAC would also likely help solve the problem where a node’s own
47
transmissions interfere with packets on its receivers by preventing transmission and receptionfrom occurring simultaneously in the same direction.
Another area of work to be done is in directional routing, in particular forwarding of packets to specific nodes utilising the directionality of the transmitter LEDs. A crude implementation was given in the code for transmitting of neighbour table update packets, by sending on the closest transmitter, however it is likely a better method could be found.
Future work would undoubtedly involve moving to a larger testbed of say 20 nodes and upwards. For such a network it would be best to utilise dynamic memory allocation rather than pre-allocating it for the neighbour and bearing tables. Fortunately the AVR C library provides functions for dynamic memory allocation to on board and external RAM. Alternatively it may be beneficial to use PC to simulate the role of the mainboard, removing the concern for memory. By using a simpler node design with a single communications board using USB, multiple nodes could be controlled through a single USB hub. This would provide data on the state of many nodes in the network as well as greatly reducing the debug process time.
Also, with regard to the tables, it would be advisable to implement a sort of ‘time to live’ scheme for node data in order to account for node failures. In the current scheme, if a node fails its data remains in its neighbours’ tables; if it were an anchor node then its neighbours would think they are one hop nodes, affecting the weightings applied to position data across that area of the network. One could apply TTL to data in the table, which on expiring would be prohibited from use in anchor hop and position estimate calculation until a new packet from that node is received.
Another essential and quite simple change would be to alter the code such that two or three hello sequences could be received at a time by using multiple receive buffers. This would be useful in the case where a node can see two neighbours directly opposite each other, but where they cannot see each other and so both transmit hello sequences.
48
References
1. Joseph Violi, Y. Ahmet Şekercioğlu, and Andrew Price, Incorporating Multiple Estimates for Accurate Localization in Infrared Ad Hoc Networks (draft rev 1335), Department of Electrical & Computer Systems Engineering, Monash University, Australia, December 23, 2008.
2. Sens-r: A Low-cost Wireless Sensor Node with Mobility, TWiki.WSensornets [Online]. Available: http://ctieware.eng.monash.edu.au/twiki/bin/view/WSensornets/Sens-r
3. Michael Broxton, Joshua Lifton, and Joseph A. Parasido, Localization on the pushpin computing sensor network using spectral graph drawing and mesh relaxation, ACM SIGMOBILE Mobile Computing and Communications Review, Volume 10, 2006.
4. Jim Pugh and Alcherio Martinoli, Relative Localization and Communication Module for Small-Scale Multi-Robot Systems, IEEE International Conference on Robotics and Automation, 2006.
5. AquaJelly: An Artificial Jellyfish with Electric Drive Unit, Festo, 2008. Available: http://www.festo.com/cms/en-us_us/5889.htm
6. WinAVR, Open Source Toolkit, June 10, 2006 [Online]. Available: http://winavr.sourceforge.net/index.html
7. AVR Freaks, AVR Community [Online]. Available: http://www.avrfreaks.net
8. BRM-1030 Infrared Receiver Module, Bright LED Electronics Corp. Available: http://www.datasheetarchive.com/pdf-datasheets/Datasheets-4/DSA-64063.pdf
9. Jean J. Labrosse, MicroC/OS-II: The Real-Time Kernel, 2nd Ed., CMP Books, 2002.
10. David McKechnie, Real-Time Control over Wireless Links, Department of Electrical & Computer Systems Engineering, Monash University, Australia, 2008.
11. AVR Studio 4, AVR Integrated Development Environment [Online]. Available: http://www.atmel.com/dyn/Products/tools_card.asp?tool_id=2725
12. Joseph Violi, Accurate Node Localisation in Ad Hoc Networks using Directional Pulsed Infrared Light Communications, Department of Electrical & Computer Systems Engineering, Monash University, Australia, December 2008.
13. Sean Hoyt, Sam McKennoch, and Linda G. Bushnell, An Autonomous Multi-Agent Testbed using Infrared Wireless Communication and Localization, Technical Report, Department of
49
Electrical Engineering, University of Washington, 2005.
14. A. M. Chandra, Higher Surveying, New Age International, 2005.
15. µC/OS-II AVR GNU port, Julius Luukko, July 21, 2003 [Online]. Available: http://www.micrium.com/downloads/ports/ucos-ii/at-avr-jlu-210703
16. AVR Libc 1.6.4, C Library for Atmel AVR 8-bit RISC Microcontrollers [Online]. Available: http://www.nongnu.org/avr-libc/user-manual/index.html
17. J. Romkey, A Nonstandard for Transmission of IP Datagrams over Serial Lines: SLIP(RFC1055), Network Working Group, June 1988.
18. AVR410: RC5 IR Remote Control Receiver Application Note (rev 1473B), Atmel, May 2002. Available: http://www.atmel.com/dyn/resources/prod_documents/doc1473.pdf
19. Srdjan Capkun, Maher Hamdi, and Jean-Pierre Hubaux, GPS-free positioning in mobile ad-hoc networks, Proceedings of the 34th Hawaii International Conference on System Sciences, 2001.
20. Koen Langendoen and Niels Reijers, Distributed Localization Algorithms, Faculty of Electrical Engineering, Mathematics & Computer Science, Delft University of Technology, Netherlands, 2004.
21. Carlos de Morais Cordeiro and Dharma Prakash Agrawal, Ad Hoc & Sensor Networks: Theory and Applications, World Scientific, 2006.
22. Georgios Sarigiannidis, Localization for Ad Hoc Wireless Sensor Networks, Department of Applied Mathematics, Delft University of Technology, Netherlands, August 2006.
23. Paul Bourke, Intersection of Two Circles, University of Western Australia, April 1997 [Online]. Available: http://local.wasp.uwa.edu.au/~pbourke/geometry/2circle/
24. Wireless Ad Hoc Networks, NIST: Advanced Network Technologies Division [Online]. Available: http://www.antd.nist.gov/wahn_home.shtml
25. <util/crc16.h>: CRC Computations, AVR Libc Reference [Online]. Available: http://www.nongnu.org/avr-libc/user-manual/group__util__crc.html
26. Joseph Violi, Y. Ahmet Şekercioğlu, and Andrew Price, A Consensus Algorithm for Improving Localization in Ad Hoc Networks with Infrared Channels (draft revs 916, 1076), Department of Electrical & Computer Systems Engineering, Monash University, Australia, 2008.
27. Joseph Violi, Y. Ahmet Şekercioğlu, and Andrew Price, Channel Characteristics of Inexpensive Infrared Transmitter-Receiver Pairs for Localisation (draft rev 838), Department of Electrical & Computer Systems Engineering, Monash University, Australia, 2008.
28. MAX232, MAX232I DUAL EIA-232 Drivers/Receivers, Texas Instruments, March 2004. Available: http://focus.ti.com/lit/ds/symlink/max232.pdf
50
29. 8-bit AVR Microcontroller with 128K Bytes In-System Programmable Flash (rev 2467R), ATmega128, ATmega128L, Atmel, June 2008. Available: http://www.atmel.com/dyn/resources/prod_documents/doc2467.pdf
30. 8-bit AVR Microcontroller with 128K Bytes In-System Programmable Flash (rev 2545M), ATmega48/V, ATmega88/V, ATmega168/V, Atmel, September 2007. Available: http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf
51
Appendix A: Test Results
A.1 Bearing Test ResultsRange 1m, Bearing 0°
Step Size: 2% 4% 6% 8% 10%0.00 3.60 -2.81 0.00 5.63-1.70 3.60 12.86 3.75 -5.620.83 0.00 0.00 3.75 0.003.53 0.00 6.00 3.46 0.000.00 1.73 -2.81 3.75 -5.620.00 0.00 2.81 0.00 0.001.73 8.18 7.16 0.00 5.631.73 1.73 6.00 0.00 5.633.60 0.00 7.16 0.00 5.631.70 0.00 0.00 8.18 -5.620.87 1.73 6.00 8.18 5.630.00 1.73 2.81 -3.75 5.634.50 0.00 6.00 3.75 5.630.85 3.60 7.16 0.00 5.631.76 7.83 7.16 3.75 5.630.00 1.73 2.81 3.75 5.633.53 1.73 -2.81 8.18 0.002.60 1.73 2.81 8.18 0.000.00 0.00 -2.81 -3.75 5.632.60 0.00 2.81 -3.75 -5.620.00 0.00 0.00 -3.75 5.63-0.85 3.60 0.00 8.18 0.000.85 1.73 -2.81 8.18 -5.620.00 7.83 7.16 8.18 0.002.70 3.60 -2.81 3.75 0.001.70 7.83 6.00 8.18 -5.62-2.60 0.00 6.00 0.00 0.003.60 1.73 6.00 3.75 5.63-0.83 7.83 2.81 -8.18 0.000.00 0.00 7.16 8.18 0.000.85 3.60 2.81 0.00 5.63-1.70 3.75 6.00 8.18 0.003.53 1.73 2.81 0.00 5.63
52
0.83 3.60 6.00 3.75 5.635.63 3.60 2.81 8.18 5.630.83 0.00 0.00 3.75 5.63-0.83 0.00 0.00 8.18 5.630.87 -1.73 0.00 8.18 5.630.00 1.73 2.81 3.75 5.631.70 -1.73 0.00 0.00 5.630.83 0.00 7.16 8.18 -5.620.00 1.73 0.00 8.18 0.001.73 -1.73 2.81 3.75 0.00-0.83 1.73 2.81 8.18 5.630.00 -1.73 0.00 8.18 0.002.65 -1.73 2.81 8.18 0.006.56 1.73 0.00 8.18 0.000.85 0.00 0.00 0.00 5.630.83 0.00 7.16 3.75 -5.627.83 0.00 6.28 5.00 -6.59
Table A.1: Bearing test results for 1m range, 0° bearing.
Range 1m, Bearing 30°Step Size: 2% 4% 6% 8% 10%
23.54 23.23 21.36 24.00 0.0021.75 23.91 23.64 24.00 20.4523.20 24.00 23.64 24.00 18.0023.57 23.91 23.54 18.75 20.4522.86 20.89 21.36 24.00 18.0023.54 23.18 23.54 25.71 20.4522.86 21.72 21.36 25.71 20.4521.75 23.91 26.32 24.00 0.0023.91 18.00 23.54 25.71 25.0023.54 23.23 23.64 20.77 15.0022.86 23.91 23.54 24.00 0.0022.86 20.89 25.93 25.71 20.4523.20 24.68 24.68 25.71 20.4523.61 23.23 34.77 24.00 20.4522.86 26.38 20.09 24.00 20.4523.54 18.00 21.36 27.69 20.4523.20 23.91 23.54 25.71 18.0022.13 20.89 25.93 25.71 20.4523.91 23.91 21.23 32.73 20.4522.85 23.23 21.36 24.00 20.4523.23 24.00 20.09 26.25 27.0022.86 23.23 31.70 24.23 20.4523.54 20.89 23.64 25.71 24.5523.20 23.91 21.36 25.71 24.5523.95 25.00 26.32 24.00 27.00
53
23.20 23.18 24.91 25.71 20.4523.20 23.23 21.36 25.71 25.0025.00 23.18 23.64 18.00 27.0024.29 23.23 22.50 25.71 20.4521.75 23.23 21.36 26.25 20.4523.61 23.23 26.32 17.31 20.4522.86 21.72 21.36 24.00 20.4522.85 25.71 23.64 25.71 27.0022.86 24.00 26.32 24.23 24.5523.57 23.91 23.64 26.25 20.4523.20 25.00 15.32 24.00 24.5522.86 26.38 21.36 24.00 20.4521.72 25.71 20.09 25.71 20.4523.20 23.23 23.54 24.00 24.5522.86 21.72 23.54 25.71 20.4521.75 23.23 24.68 24.00 6.4323.86 20.89 24.68 25.71 20.4523.54 21.77 23.54 25.71 18.0022.12 20.89 23.54 25.71 18.0023.20 23.23 27.32 25.71 24.5523.20 23.91 25.93 27.69 15.0026.05 24.00 20.09 24.00 20.4523.54 25.71 15.32 26.25 20.4523.23 20.00 23.91 26.67 0.0023.23 23.64 21.00 20.45 20.45
Table A.2: Bearing test results for 1m range, 30° bearing.
Range 1m, Bearing 45°Step Size: 2% 4% 6% 8% 10%
47.25 45.88 47.81 49.50 59.0646.80 41.40 49.66 47.14 47.3743.69 42.24 49.50 42.95 39.7143.14 57.69 53.04 46.88 42.1946.34 43.04 40.78 42.95 37.0645.46 46.80 42.27 42.86 47.3744.56 44.12 43.55 47.14 34.4146.35 46.80 57.60 47.05 42.1944.13 44.04 40.91 48.91 40.0045.43 42.00 42.00 46.88 65.7747.32 45.92 46.45 43.13 47.5048.75 44.04 46.41 41.25 37.0643.16 50.00 46.36 41.25 54.6448.83 45.92 46.32 41.09 39.7144.56 48.00 39.19 43.27 47.5044.57 45.92 41.03 46.88 47.3746.38 44.12 46.41 55.00 37.06
54
46.78 44.10 43.55 39.13 42.1946.32 46.80 43.64 46.88 42.5045.89 47.93 43.55 43.13 39.7147.25 46.88 49.50 51.14 54.6444.57 45.92 46.41 40.71 42.5045.89 45.92 43.59 56.25 56.2545.90 45.92 34.14 43.20 42.5047.32 44.12 43.68 43.20 51.9245.90 47.76 42.00 66.00 47.2544.13 46.88 46.41 54.47 42.5045.44 39.13 57.12 47.14 39.7144.10 46.88 39.00 66.00 42.7548.75 41.25 39.00 43.13 40.2646.78 46.00 41.90 50.87 40.0045.90 46.80 41.79 66.18 39.7146.88 46.80 42.00 55.00 57.0046.34 47.76 38.57 41.40 47.3746.78 45.92 40.91 47.05 42.7546.82 43.04 40.34 43.13 39.7146.36 47.87 46.32 43.04 39.7143.13 45.92 46.50 40.71 54.6445.46 49.00 40.50 40.50 39.7147.32 47.87 39.19 43.20 47.3746.34 45.92 45.00 56.25 42.1945.90 45.92 46.41 48.91 42.5046.34 46.80 49.50 47.05 47.2545.88 42.24 46.50 45.00 51.9245.44 47.76 39.00 46.96 39.7146.78 46.91 46.41 47.05 47.5045.91 46.80 40.91 43.13 42.1946.82 43.09 40.91 49.29 42.5046.32 43.20 46.41 41.25 39.7140.91 42.00 46.91 54.51 45.00
Table A.3: Bearing test results for 1m range, 45° bearing.
Range 1m, Bearing 60°Step Size: 2% 4% 6% 8% 10%
65.82 67.50 67.50 57.27 63.7566.52 69.00 63.68 65.77 63.7566.84 66.82 64.07 61.36 63.7566.18 67.50 65.32 69.00 63.0066.49 64.50 58.30 60.00 63.7566.46 66.18 66.46 67.50 65.7766.09 66.09 66.46 69.00 65.7764.50 64.50 64.07 67.50 65.7764.18 66.82 66.46 64.29 65.77
55
65.77 66.82 61.13 66.00 65.7766.49 66.82 64.07 58.50 56.2566.18 65.32 65.09 60.00 61.3666.84 66.82 55.23 61.36 65.7766.49 66.09 63.68 62.31 63.0066.84 66.18 66.46 69.00 56.2566.84 60.58 66.46 66.18 65.7764.18 66.82 66.46 64.29 63.7566.46 66.82 66.46 67.50 61.3665.71 63.62 68.54 62.31 63.7566.36 65.32 62.68 64.29 55.0066.46 59.40 66.46 60.00 65.7766.46 66.09 66.46 65.77 63.7566.14 66.82 66.36 67.50 60.0064.07 67.50 67.50 64.29 67.5066.49 66.82 64.07 64.29 63.7566.49 64.29 65.32 66.18 65.7766.84 66.82 66.46 67.50 65.7766.84 66.09 66.46 67.50 58.5067.17 66.82 64.07 67.50 63.7566.52 66.82 66.46 69.00 58.5066.84 66.82 52.68 64.29 55.0066.49 66.82 64.07 62.31 65.7766.49 64.50 67.50 60.00 65.7765.00 64.50 66.46 65.77 63.7566.49 63.62 66.46 69.00 67.5066.14 66.82 66.46 69.00 56.2565.77 66.09 66.46 66.18 65.7763.75 66.09 66.46 63.00 63.0066.14 66.82 59.40 64.29 63.0065.32 66.09 63.68 66.00 63.7567.87 66.82 65.09 65.77 63.7565.71 59.40 65.32 62.31 58.5066.82 65.45 66.46 67.50 65.4566.49 65.32 66.46 64.29 65.7766.14 67.50 64.07 67.50 63.7567.16 66.82 64.07 67.50 65.7766.49 66.09 66.46 67.50 65.4566.84 66.82 67.50 67.50 65.7766.09 65.17 68.64 67.50 60.2864.50 68.57 64.50 66.49 65.77
Table A.4: Bearing test results for 1m range, 60° bearing.
Range 1m, Bearing 90°Step Size: 2% 4% 6% 8% 10%
90.83 90.00 90.00 90.00 90.00
56
90.00 91.80 87.00 90.00 95.6391.88 91.80 92.81 90.00 90.0090.00 100.59 96.43 93.75 90.0091.10 91.80 90.00 93.75 95.6390.00 91.73 93.00 93.75 90.0090.00 90.00 91.67 90.00 84.3890.00 91.88 90.00 98.18 90.0091.41 90.00 84.00 90.00 90.0092.76 93.46 105.00 93.75 84.3890.00 88.20 90.00 93.75 95.6391.80 88.27 93.00 93.75 90.0090.00 79.41 96.43 93.75 90.0090.00 91.80 90.00 90.00 90.0090.00 90.00 90.00 86.25 95.6390.00 90.00 90.00 90.00 90.0091.71 90.00 90.00 90.00 90.0090.00 88.27 87.00 79.41 84.3890.00 90.00 87.19 90.00 90.0089.17 88.20 92.81 90.00 84.3889.17 91.80 87.00 93.75 84.3890.00 90.00 90.00 98.18 90.0090.00 90.00 93.00 90.00 95.6390.63 90.00 92.81 98.18 90.0089.17 90.00 93.00 90.00 95.6390.83 88.20 90.00 93.75 84.3889.15 90.00 90.00 81.82 95.6380.36 90.00 92.81 90.00 90.0091.55 90.00 87.19 81.82 84.3890.00 90.00 90.00 90.00 90.0090.00 91.80 90.00 93.75 90.0090.83 88.20 90.00 90.00 95.6390.00 88.20 96.00 90.00 90.0090.00 88.20 90.00 90.00 90.0090.00 91.80 96.43 86.25 90.0091.10 91.73 90.00 90.00 90.0088.24 90.00 90.00 90.00 90.0091.70 90.00 90.00 86.25 90.0090.85 90.00 92.81 93.75 90.0090.00 95.63 90.00 90.00 84.38
101.09 90.00 90.00 90.00 95.6390.83 100.59 93.00 93.75 95.6390.00 100.29 96.00 90.00 73.6491.70 90.00 92.81 86.25 90.00
101.74 90.00 90.00 93.75 95.6389.15 90.00 90.00 90.00 90.0090.00 91.80 90.00 100.59 95.63
57
90.00 88.20 92.81 93.75 90.0093.13 91.80 90.00 93.75 90.00
101.10 90.00 90.00 90.00 83.41
Table A.5: Bearing test results for 1m range, 90° bearing.
Range 1m, Bearing 135°Step Size: 2% 4% 6% 8% 10%
135.54 135.00 135.00 128.86 138.21131.04 133.16 138.00 128.86 140.29135.00 133.85 133.59 133.13 137.81135.00 139.09 132.27 137.50 140.29131.09 129.77 136.73 135.00 140.29131.21 137.93 136.73 137.05 151.36132.26 134.10 130.50 132.50 143.44130.11 130.91 139.50 131.09 140.29136.07 130.00 138.60 133.13 140.29131.82 130.91 130.50 139.29 140.29136.11 130.11 125.69 135.00 140.29134.02 136.96 136.73 132.50 140.29129.62 131.93 136.73 128.86 137.50138.18 135.00 136.73 132.95 153.75133.31 130.00 141.21 132.95 155.77139.24 135.94 138.00 141.14 135.00130.60 134.08 135.00 135.00 140.29137.07 139.00 129.19 135.00 155.77129.07 139.09 132.27 131.09 135.00132.80 129.00 133.27 137.25 129.71136.06 134.08 135.00 131.09 155.77132.38 130.31 139.50 151.07 135.00131.90 139.09 141.21 130.71 143.44134.55 132.30 128.79 133.13 140.29130.16 134.10 129.19 123.75 140.29133.94 141.43 132.27 139.29 148.50135.00 139.19 131.40 142.50 151.36130.74 139.09 138.10 139.29 140.29132.83 131.86 139.50 137.05 135.00135.55 133.13 133.59 128.86 140.29135.95 133.24 133.55 135.00 140.29137.02 132.07 132.27 133.13 140.29136.59 133.90 130.50 142.50 135.00135.48 132.00 135.00 131.09 140.29134.42 138.00 135.00 135.00 155.77138.50 139.19 139.50 128.86 129.71133.94 134.08 138.10 135.00 155.77136.08 130.31 141.21 141.75 155.77132.98 136.10 130.50 137.81 140.29
58
138.58 134.08 127.50 153.00 137.50131.13 131.09 132.27 128.86 140.29129.50 139.09 139.50 133.13 135.00133.14 135.00 130.50 137.50 135.00138.58 129.77 141.21 135.00 129.71135.54 135.00 143.04 133.13 143.44132.47 135.00 136.73 135.00 143.44132.47 133.98 138.60 128.86 137.81139.00 136.96 132.27 132.50 140.29134.44 128.15 135.00 135.00 132.19131.09 141.43 141.35 140.63 139.15
Table A.6: Bearing test results for 1m range, 135° bearing.
59
Appendix B: Mathematical Work
B.1 Equation 4.16 ProofProof of equation 4.16:
2 2 2
2
C B C A A BP B
A B
2 2 2
2
C B C A A BRHS
A B
Substitute equations 4.13 & 4.14:2 2 2
2
P B P A A BRHS
A B
Substitute equation 4.15:
22 2
2
P B P A P A P BRHS
A B
2
2 2
2
P B P A P BRHS
A B
P B P A P BRHS
A B
Substitute equation 4.15:P B A B
RHSA B
RHS P B
LHS
Q.E.D
60
Appendix C: Testbed Supplement
The following appendix provides the circuit diagrams and parts required to produce the non mobile Sens-r nodes used in this project. Component lists for additional electronics used are also supplied.
C.1 Sens-r Components ListQty/Node
Value Supplier Part No. Description
Mainboard1 Monash Uni Mainboard PCB1 Futurlec ATMEGA128-16AC ATmega 128 128kB MCU (64 TQFP)1 Futurlec L293D L293D Motor Drive IC1 16MHz Altronics V2289 (1x10 pack) 16 MHz HC49SM Surface Mount Crystal8 Altronics Y1041 (1x10 pack) LED 1206 Red8 430 Altronics R1049 (1x10 pack) Surface Mount Resistor1 10k Altronics R8188 (1x10 pack) Surface Mount Resistor3 2k Altronics R8137 (1x10 pack) Surface Mount Resistor1 4k7 Through Hole Resistor1 Jaycar SM1020 4 Way Dil Switch1 Altronics S1120 PCB Mount SPST Momentary Pushbutton
Switch2 22pF Altronics R9836 (1x10 pack) Surface Mount Capacitor5 0.1uF Altronics R8835 (1x10 pack) Surface Mount Capacitor1 10uF Altronics R4768 Polarised Capacitor1 Altronics Y0908 MC78M05CD2T (D2PAK) 3-Terminal 0.5A
Positive Voltage Regulator1 Altronics P5513 3 Way 90 degree PCB Mount Pin Header2 Altronics P5514 4 Way 90 degree PCB Mount Pin Header1 Altronics P5515 5 Way 90 degree PCB Mount Pin Header1 Altronics P5420 40 Way 90 degree Dual Row Pin (5 way
required)1 Altronics P5410 40 Way Dual Row Pin (11 way required)1 Pin Jumper1 Altronics P5430 40 Way PCB Pin (9 pins required)Communications Board1 Monash Uni Communications Board PCB1 Futurlec ATMEGA168V-10PI ATmega168 (PDIP) + Chip Socket1 10MHz Altronics V1259 10 MHz Low Profile HC49 Crystal3 0.1uF Altronics R2930A Capacitor2 22pF Altronics R2814 Capacitor4 Altronics BC547 NPN Transistor5 Altronics BC557 PNP Transistor11 2k2 Through Hole Resistor4 560 Through Hole Resistor1 3k3 Through Hole Resistor1 4k7 Through Hole Resistor1 Altronics S1120 PCB Mount SPST Momentary Pushbutton
Switch
61
8 Altronics Z1611 BRM-1030 IR Rx Module16 Jaycar ZD1945 5mm IR LED1 RS LM317MDT/NOPB 500 mA Adjustable Output Positive Voltage
Regulator1 Altronics P5410 40 Way Dual Row Pin (5 way required)1 Altronics P5400 40 Pin PCB Pin Socket (need 10 pins)Other1 Monash Uni 5 Pin PCB Cable with Female Plug1 Monash Uni 9V Power Supply or 9V Battery Connector & 3
Pin Female PCB Plug3 Altronics H1320 (1x10 pack) 5 x 12mm Round Nylon Spacer3 Altronics H3150A (1x10 pack) M3 x 25mm Pan Pozi Nickel Bolt3 Altronics H1345 (1x8 pack) 8mm Nylon Tapped Spacer*Note: Motor, wheels, encoders not included as non mobile.
Table C.1: Non mobile Sens-r node complete components list.
C.2 Sens-r Circuit DiagramsThe following circuit diagrams are updated versions of those available at TWiki.WSensornets [2].
Figure C.1: Mainboard circuit diagram: Microcontroller.
62
Figure C.2: Mainboard circuit diagram: Power and connectors.
Figure C.3: Mainboard circuit diagram: Motor control.
Figure C.4: Communications board circuit diagram: IR reception.
63
Figure C.5: Communications board circuit diagram: IR transmission.
Figure C.6: Communications board circuit diagram: Microcontroller.
64
C.3 Additional Components ListQty Value Supplier Part No. DescriptionSerial to TTL Converter Circuit5 1uF Monash Uni Polarised Capacitor1 Monash Uni MAX232 TTL to RS232 Converter IC1 Monash Uni 9 Pin Serial Cable2 Altronics 10 Pin JTAG Header Plug1m Altronics 10 Wire JTAG CableOther1 Soanar ZBP018 AVR ISP MkII Programmer2 Altronics 10 Pin JTAG Header Plug
Table C.2: Additional components list.
65
Appendix D: Project Code
The following section provides the localisation code produced for the Sens-r circuit boards and information with regard to configuration and settings used. Note that in the code there may be functions which the reader is not familiar with, particularly if they have had no previous experience with µC/OS-II or Atmel based C code. Information with regards to µC/OS-II can be found in the book MicroC/OS-II: The Real-Time Kernel [9] and information about functions from the AVR C library can be found at the AVR Libc online users manual [16].
D.1 Configuration & Settings
D.1.1 Fuse Settings
The following gives the fuse settings of the Sens-r microcontrollers.
Mainboard ATmega128 Fuse SettingsM103C NoWDTON NoOCDEN NoJTAGEN NoSPIEN YesEESAVE NoBOOTSZ 4096 words, start address $F000BOOTRST NoCKOPT NoBODLEVEL 2.7V VccBODEN NoSUT_CKSEL Ext. Crystal/Resonator, High Freq. Start-up time 16CK + 64msMemory AddressesExtended 0xFFHigh 0xC9Low 0xFFSignature 0x1E 0x97 0x02, no lockbits set.
Communications Board ATmega168 Fuse SettingsBOOTSZ 1024 words, start address $1C00BOOTRST NoRSTDISBL NoDWEN NoSPIEN YesWDTON No
66
EESAVE NoBODLEVEL DisabledCKDIV8 NoCKOUT NoSUT_CKSEL Ext. Crystal/Resonator, 3 – 8 MHz, Start-up time PWRDWN/RESET
1KCK/14CK + 0msMemory AddressesExtended 0xF9High 0xDFLow 0xECSignature 0x1E 0x94 0x06, no lockbits set.
D.1.2 Library Settings
The following gives the library and configuration settings required to compile the code in AVR Studio 4 using the WinAVR GNU C Compiler.
Mainboard ATmega128 Library SettingsSource Files atmega128.c
os_cpu_a.sos_cpu_c.cucos_ii.c
Header Files includes.hos_cfg.hucos_ii.h
External Dependencies
avr_isr.hcommon.hdelay.hdelay_basic.hfuse.hinterrupt.hinttypes.hio.hiom128.hlibm.alibprintf_flt.alock.hmath.hos_cpu.hpgmspace.hportpins.hsfr_defs.hstdarg.hstddef.hstdint.hstdio.hstdlib.hstring.hversion.h
67
Other Files atmega128.lssatmega128.map
Include Directories .\uCOS-II\ATmega128\uCOS-II\SOURCE\\WinAVR\avr\include\
Linked Libraries libm.alibprintf_flt.a
Linker Compilation Options
-Wl,-u,vfprintf-lprintf_flt-lm
Communications Board ATmega168 Library SettingsSource Files atmega168.cHeader Files -External Dependencies
common.hdelay.hdelay_basic.hfuse.hinterrupt.hinttypes.hio.hiom168.hiomx8.hlock.hportpins.hsfr_defs.hstddef.hstdint.hstring.hversion.h
Other Files atmega168.lssatmega168.map
Include Directories -Linked Libraries -Linker Compilation Options
-
D.2 Sens-r Communications BoardThe following gives the communications subsystem code ‘atmega168.c’. Credit is given to Joesph Violi as some functions are based on code that he provided.
D.2.1 atmega168.c
// SENS-R IR COMMUNICATIONS BOARD// 2008/09 Leo Felix Priestnall// Some functions based on code produced by Joseph Violi
68
#include <avr/io.h>#define F_CPU (10000000UL)#include <util/delay.h>#include <avr/interrupt.h>#include <string.h>
#define INTERSYMBOL_DELAY 3.5
/* SLIP RFC1055 special character codes */#define END 192 // end of packet#define ESC 219 // byte stuffing#define ESC_END 220 // ESC ESC_END means END data byte#define ESC_ESC 221 // ESC ESC_ESC means ESC data byte
// serial packet commands, packet format <COMMAND><PAYLOAD>...<END>#define SEND_ALL 0x10#define SET_TX_POW 0x20#define SET_TO 0x30#define SET_FROM 0x40#define SET_TYPE 0x50#define SET_DATA 0x60#define SET_LEN 0x70
void initialise();void pwm_v(unsigned char D);void pwm_ir(unsigned char s);
void uart_send_byte(unsigned char c);void uart_send_esc(unsigned char byte);void uart_send_packet(unsigned char *data, unsigned char len);void uart_cmd();
void ir_send_bytes(unsigned char id, unsigned char byteA, unsigned char byteB, unsigned char byteC, unsigned char byteD);void ir_send_packets(unsigned char mask);unsigned char ir_frame_esc(unsigned char *packet, unsigned char pos, unsigned char byte);void ir_frame_packet(unsigned char id, unsigned char txid);
void ir_recv_pinchange();void ir_recv_packet(unsigned char id);
unsigned char crc_update(unsigned char crc, unsigned char byte);
/* Initialise ports, timers, UART etc */void initialise() { /* Initialising ports DDxm PORTxn PUD State 0 0 X Z 0 1 0 1,0 depending on input 0 1 1 Z 1 0 X 0 out 1 1 X 1 out
On reset all regs initialised to 0.*/
// Port B (6 bits) - IR Rx on bits 5 & 4, IR Tx lines Z & Y on bits 3 & 0, PWM_V bit 2, PWM_IR bit 1 DDRB = 0b001111; //IR Rx only inputs PORTB = 0b110100; //PWM_V intialise to 1, Tx to 0, PWM_IR to 0 (negative logic PNP)
69
// Port C (6 bits) - IR Rx DDRC = 0b000000; PORTC = 0b000000; // Port D (8 bits) - IR Tx on bits 7 to 2, Serial Tx on 1, Serial Rx on 0 DDRD = 0b11111100; // Tx/Rx overidden by Tx/Rx Enable PORTD = 0b00111100;
// Timer/Counter 1: PWM_V & PWM_IR// Initialise Counter
TCNT1 = 0;// Fast PWM with TOP ICR1 (WGM13:0 = 14);
TCCR1A |= (1<<WGM11); TCCR1B |= (1<<WGM13)|(1<<WGM12); // Non-inverting mode TCCR1A |= (1<<COM1A1)|(1<<COM1B1); // Set TOP f = 10MHz/38kHz - 1 ICR1 = 262;
// fosc/1TCCR1B |= (1<<CS10);
// Set PWM_V & PWM_IR duty cyclespwm_v(80);pwm_ir(1);
// Initialising UART// Baud Rate 20834, fosc 10MHz UBRR 29, fosc 16MHz UBRR 47, error 0.0%// CHANGED TO 19200, fosc 10MHz UBBR 32, fosc 16MHz UBRR 51, error ~1%?UBRR0L = 32;UBRR0H = 0;// Enable Rx, Tx, Rx Complete InterruptUCSR0B = (1<<RXEN0)|(1<<TXEN0)|(1<<RXCIE0);// Set frame format: 8 data bits, 1 stop bit (0<<USBS1), 0 parity (0<<UPM10), asynch
mode (0<<UMSEL1)UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
// Pin change interrupts on Rx pins// PCINT 14..0 can be enabledPCICR = (1<<PCIE1)|(1<<PCIE0);// Enable PCINT 14..8 (PINC all), 5..4 (PINB5, PINB4)PCMSK1 = 0b111111;PCMSK0 = 0b110000;
// Timer/Counter 0: 1/2 period interrupt for IR Tx// Send byte fn enables interrupt, disabled on send complete// Initialise Counter
TCNT0 = 0;// CTC (Clear on Timer Compare) Mode with TOP OCR0A (WGM02:0 = 2);// OCR0A/OCR0B pins disconnected
TCCR0A |= (1<<WGM01);// fosc/64TCCR0B |= (1<<CS01)|(1<<CS00);
// Set TOP ~500us (20 cycles of 38kHz): N = 500e-6 * (10MHz/64) OCR0A = 78;
// Timer/Counter 2: Use for sampling of incoming IR Rx data// Used in combination with Rx pin change interrupt (sync edges)// Sample at 3/4 bit time from middle transistion to get bit state// If no sync edge within 5/4 bit time FAULT// Initialise Counter
TCNT2 = 0;// Normal counter operation TOP 0xFF (WGM02:0 = 0)// Output compares disabled (normal port operation)
70
TCCR2A = 0x00;// fosc/64TCCR2B |= (1<<CS22);// Overflow interrupt enableTIMSK2 |= (1<<TOIE2);
// Enable interruptssei();
}
volatile struct ir_packet{volatile unsigned char to, from, type, len, data[30], txid, txpow, crc;
} ir_rx_packet[8], ir_tx_packet[4], ir_tmp_packet;
volatile unsigned char ir_tx_msk;volatile unsigned char ir_tx_bit_cnt;volatile unsigned char ir_tx_bit;volatile unsigned char ir_tx_byte[4];volatile unsigned char ir_tx_power;unsigned char ir_tx_framed_packet[4][60];volatile unsigned char ir_tx_len[4];
volatile unsigned char ir_rx_delta, ir_rx_in, ir_rx_prev, ir_rx_change;volatile unsigned char ir_rx_timer[8], ir_rx_bit_cnt[8], ir_rx_byte[8];volatile unsigned char ir_rx_st1, ir_rx_esc;volatile unsigned char ir_rx_pktcnt[4];
volatile unsigned char ir_global_tx_msk;volatile unsigned char self_id;
// IR Transmit using send mask & RC5 IR protocol with Manchester encoding// tx mask 0bTog St1 St2 0 D C B A// 1/2 Period ISR for transmittersISR(SIG_OUTPUT_COMPARE0A){
TCNT0 = 0;
// Bit count 8 and toggle flag off (finished sending byte)if ((ir_tx_bit_cnt == 8) && !(ir_tx_msk & 0x80)) {
PORTD |= 0b00111100; // turn off ABCD (PNP logic)
// Disable interrupt on OCR0A matchTIMSK0 &= ~(1<<OCIE0A);TCNT0 = 0;ir_tx_bit_cnt = 0xFF; // ends ir_send_bytes while(1) polling
}// Start bit 1 flag (manchester 1 code)else if (ir_tx_msk & 0x40) {
ir_tx_msk &= ~0x40; // clear st1 flagir_tx_msk |= 0x80; // set toggle flagPORTD |= 0b00111100; // PNP lo
}// Toggleelse if (ir_tx_msk & 0x80) {
ir_tx_msk &= ~0x80; // clear toggle flagPORTD ^= (ir_tx_msk & 0x0F)<<2; // toggle ABCD to be sent on
}else {
ir_tx_msk |= 0x80; // set toggle flagir_tx_bit = 0x00; // next bit buffer 0b0000DCBAif (ir_tx_byte[0] & (1<<ir_tx_bit_cnt)) // A
ir_tx_bit |= 0x01;
71
if (ir_tx_byte[1] & (1<<ir_tx_bit_cnt)) // Bir_tx_bit |= 0x02;
if (ir_tx_byte[2] & (1<<ir_tx_bit_cnt)) // Cir_tx_bit |= 0x04;
if (ir_tx_byte[3] & (1<<ir_tx_bit_cnt)) // Dir_tx_bit |= 0x08;
// logic 1 needs a logic lo to hi, 1 is PNP loPORTD = (PORTD & 0xC3) | (ir_tx_bit<<2) | ((~ir_tx_msk & 0x0F)<<2); // 0bxx0000xx |
0b00ABCD00;
ir_tx_bit_cnt++;}
}
// IR Transmit Byte// send_msk 0bZYXWDBCA, ir_tx_msk tog st1 st2 0 D B C A, bytes to send on ABCD, based on Joseph's codevoid ir_send_bytes(unsigned char send_msk, unsigned char byteA, unsigned char byteB, unsigned char byteC, unsigned char byteD){
ir_tx_bit_cnt = 0;ir_tx_byte[0] = byteA;ir_tx_byte[1] = byteB;ir_tx_byte[2] = byteC;ir_tx_byte[3] = byteD;
ir_tx_msk = send_msk & 0xF0;
// clear current ZYXW, DBCAPORTD = 0x3C;PORTB &= 0b110110;
// Set only 1 GND transistor (prevent mutliple LEDs on same quadrant R)if (ir_tx_msk & 0x10)
PORTD |= (1<<PD6); // Welse if (ir_tx_msk & 0x20)
PORTD |= (1<<PD7); // Xelse if (ir_tx_msk & 0x40)
PORTB |= (1<<PB0); // Yelse if (ir_tx_msk & 0x80)
PORTB |= (1<<PB3); // Z
ir_tx_msk = send_msk & 0x0F;ir_tx_msk |= 0x40; // set start bit 1 flag
// Disable Rx PC INTPCICR = 0x00;
// reset counter to ensure correct st1 widthTCNT0 = 0;
// Enable interrupt on OCR0A matchTIMSK0 |= (1<<OCIE0A);
// send with ISRwhile (ir_tx_bit_cnt != 0xFF) {
;}
// Enable Rx PC INT
72
PCICR = (1<<PCIE1)|(1<<PCIE0);}
// CRC calculation, returns updated CRC value// http://www.nongnu.org/avr-libc/user-manual/group__util__crc.html // http://www.maxim-ic.com/appnotes.cfm/appnote_number/27unsigned char crc_update(unsigned char crc, unsigned char byte){
unsigned char i;
crc = crc ^ byte; for (i=0; i<8; i++) { if (crc & 0x01) crc = (crc >> 1) ^ 0x8C; else crc >>= 1; }
return crc;}
// SLIP append esc char for IR packet// returns position of next byte in packet// joseph's codeunsigned char ir_frame_esc(unsigned char *packet, unsigned char pos, unsigned char byte) {
switch (byte) {case END:
packet[pos++] = ESC; // pospacket[pos++] = ESC_END; // pos+1break;
case ESC:packet[pos++] = ESC;packet[pos++] = ESC_ESC;break;
default:packet[pos++] = byte;break;
}return pos; // pos+1 or pos+2
}
// Framing of IR packets// does CRC calculation and SLIP RFC1055, fills in txid & txpower// based on joseph's code// IR packet <END><DA><SA><TXID><TXPOW><TYPE><LEN><DATA...><CRC><END>void ir_frame_packet(unsigned char id, unsigned char txid){
unsigned char pos = 0, crc = 0;unsigned char i, len;
// initial END chars to clear out Rx buffer and allow Rx to get setir_tx_framed_packet[id][pos++] = END;ir_tx_framed_packet[id][pos++] = END;ir_tx_framed_packet[id][pos++] = END;
// add each byte to framed packet, append SLIP esc chars as neededpos = ir_frame_esc(ir_tx_framed_packet[id], pos, ir_tx_packet[id].to);crc = crc_update(crc, ir_tx_packet[id].to);pos = ir_frame_esc(ir_tx_framed_packet[id], pos, ir_tx_packet[id].from);crc = crc_update(crc, ir_tx_packet[id].from);
73
pos = ir_frame_esc(ir_tx_framed_packet[id], pos, txid);crc = crc_update(crc, txid);pos = ir_frame_esc(ir_tx_framed_packet[id], pos, ir_tx_power);crc = crc_update(crc, ir_tx_power);pos = ir_frame_esc(ir_tx_framed_packet[id], pos, ir_tx_packet[id].type);crc = crc_update(crc, ir_tx_packet[id].type);
len = ir_tx_packet[id].len;pos = ir_frame_esc(ir_tx_framed_packet[id], pos, len);crc = crc_update(crc, len);
// packet payloadfor (i=0; i<len; i++) {
pos = ir_frame_esc(ir_tx_framed_packet[id], pos, ir_tx_packet[id].data[i]);crc = crc_update(crc, ir_tx_packet[id].data[i]);
}
// add crc bytepos = ir_frame_esc(ir_tx_framed_packet[id], pos, crc);
// finish packet with END char, save packet lengthir_tx_framed_packet[id][pos++] = END;ir_tx_len[id] = pos;
}
// IR send packets (packets framed using function ir_frame_packet())// Uses transmit mask ZYWXDCBA to determine which packets are to be sent on Tx// Based on Joseph's codevoid ir_send_packets(unsigned char mask){
unsigned char send_msk, i, j, max = 0;
// disable rx pin change interrupts (so not received on own rxs)PCICR = 0;
// Find max length of packets in buffersfor (i=0; i<4; i++) {
if ((mask & (1<<i)) && (ir_tx_len[i] > max))max = ir_tx_len[i];
}
// For all bytes waiting in bufferfor (i=0; i<max; i++) {
send_msk = mask & 0xF0; // ZYWX// find which of ABCD need to sendfor (j=0; j<4; j++) {
if ((ir_tx_len[j] > i) && (mask & (1<<j)))send_msk |= (1<<j);
else send_msk &= ~(1<<j);}
ir_send_bytes(send_msk, ir_tx_framed_packet[0][i], ir_tx_framed_packet[1][i], ir_tx_framed_packet[2][i], ir_tx_framed_packet[3][i]);
_delay_ms(INTERSYMBOL_DELAY); // intersymbol delay}
// reenable rx pin change interruptsPCICR = (1<<PCIE1)|(1<<PCIE0);
}
// Timer/counter 2 overflows on all Rx idle
74
ISR(SIG_OVERFLOW2){
TCCR2B = 0x00; // stops counter (no clk)TCNT2 = 0xFF; // set count
}
// ISR for IR Rx pin changesISR(SIG_PIN_CHANGE0){
ir_recv_pinchange();}
ISR(SIG_PIN_CHANGE1){
ir_recv_pinchange();}
// ISR function called for IR Rx pin changesvoid ir_recv_pinchange(){
unsigned char i;
ir_rx_delta = TCNT2;TCNT2 = 0;TCCR2B = (1<<CS22);
ir_rx_prev = ir_rx_in;ir_rx_in = (PINC & 0x3F) | ((PINB & 0x30)<<2) ;ir_rx_change = ir_rx_in ^ ir_rx_prev;
// update virtual timersfor (i=0; i<8; i++) {
if (ir_rx_timer[i] + ir_rx_delta < 253) ir_rx_timer[i] += ir_rx_delta;else {
ir_rx_timer[i] = 253;}
}
for (i=0; i<8; i++) {if (ir_rx_change & (1<<i)) {
if ((ir_rx_timer[i] > 226) && !(ir_rx_in & (1<<i))) {// Start bit from idle hiir_rx_bit_cnt[i] = 0;ir_rx_byte[i] = 0;ir_rx_timer[i] = 0;
}else if ((ir_rx_timer[i]>111) && (ir_rx_timer[i]<226) && (ir_rx_bit_cnt[i]<8)) {
if (!(ir_rx_in & (1<<i)))ir_rx_byte[i] = ir_rx_byte[i] | (1<<ir_rx_bit_cnt[i]);
ir_rx_bit_cnt[i]++;ir_rx_timer[i] = 0;
}
// byte receivedif (ir_rx_bit_cnt[i]==8) {
switch (ir_rx_byte[i]) {// packet complete, next byte will be from new packetcase END:
75
ir_rx_pktcnt[i] = 0;//ir_rx_status &= ~(1<<i);break;
// For ESC char set ESC flag for that Rx.case ESC:
ir_rx_esc |= (1<<i);//ir_rx_status |= (1<<i);break;
// Add ESC_END char to buffer.case ESC_END:
if (ir_rx_esc & (1<<i)) {ir_rx_esc &= ~(1<<i);ir_rx_byte[i] = END;ir_recv_packet(i);break;
}// Add ESC_ESC char to buffer.case ESC_ESC:
if (ir_rx_esc & (1<<i)) {ir_rx_esc &= ~(1<<i);ir_rx_byte[i] = ESC;ir_recv_packet(i);break;
}// Otherwise just add byte to buffer. In the event of an ESC, char // protocol violation just send the byte and reset the esc flag.
default:if (ir_rx_esc & (1<<i))
ir_rx_esc &= ~(1<<i);//ir_rx_byte[i] = ir_rx_byte[i]; ie. have left unchanged//ir_rx_status |= (1<<i);ir_recv_packet(i);break;
}
ir_rx_bit_cnt[i] = 0;}
}}
}
// based on joseph's codevoid ir_recv_packet(unsigned char id){
unsigned char i;
switch (ir_rx_pktcnt[id]){
case 0:ir_rx_packet[id].to = ir_rx_byte[id];ir_rx_packet[id].crc = 0;ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_packet[id].to);break;
case 1:ir_rx_packet[id].from = ir_rx_byte[id];ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_packet[id].from);break;
case 2:ir_rx_packet[id].txid = ir_rx_byte[id];ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_packet[id].txid);break;
case 3:
76
ir_rx_packet[id].txpow = ir_rx_byte[id];ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_packet[id].txpow);break;
case 4:ir_rx_packet[id].type = ir_rx_byte[id];ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_packet[id].type);break;
case 5:ir_rx_packet[id].len = ir_rx_byte[id];ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_packet[id].len);break;
default: // pktcnt > 5, ie. from start of data payload to end of packet// for bytes in data payload add themif ((ir_rx_pktcnt[id] - 6) < ir_rx_packet[id].len) {
ir_rx_packet[id].data[ir_rx_pktcnt[id] - 6] = ir_rx_byte[id];ir_rx_packet[id].crc = crc_update(ir_rx_packet[id].crc, ir_rx_byte[id]);
}else { // crc byte, packet complete
// CRC check, if incorrect discard packet (no time for ARQ)if(ir_rx_packet[id].crc != ir_rx_byte[id]) {
ir_rx_pktcnt[id] = 0xFF;}// Packet complete with correct CRC, send packet via UART to mainboardelse {
// discard packets not addressed to self, ALL or from self// otherwise send packet on UART with rxid appendedif ((ir_rx_packet[id].from != self_id) && ((ir_rx_packet[id].to == 0xFF) ||
(ir_rx_packet[id].to == self_id))) {uart_send_byte(END);uart_send_esc(ir_rx_packet[id].to);uart_send_esc(ir_rx_packet[id].from);uart_send_esc(ir_rx_packet[id].txid);uart_send_esc(ir_rx_packet[id].txpow);uart_send_esc(id); // rxid 0 to 7uart_send_esc(ir_rx_packet[id].type);uart_send_esc(ir_rx_packet[id].len);for (i=0; i<ir_rx_packet[id].len; i++) {
uart_send_esc(ir_rx_packet[id].data[i]);}uart_send_byte(END);
}// UART packet format (SLIP encoding)
<END><DA><SA><TXID><TXPOW><RXID><TYPE><LEN><DATA...><END>ir_rx_pktcnt[id] = 0xFF;
}}break;
}
(ir_rx_pktcnt[id])++;}
/* PWM_IR control used to modulate IR LED output to 38kHz */void pwm_ir(unsigned char s) {
if (s==0) {// set PWM_IR reg (PB1) to 0 (PNP logic 1)// ie. turn off all TxsPORTB &= ~(1<<PB1);// restore PB1 on output pinTCCR1A &= ~(1<<COM1A1);
}
77
else {// Duty cycle 50% of TC1 TOP to get 38kHzOCR1A = 262/2;// Output OC1A on PINB1 (output from counter comparator)TCCR1A |= (1<<COM1A1);
}}
/* PWM_V controls voltage delivered to LEDs and hence power Regulator delivers V_RAW (5V) + 1.2V = 6.2V at 100% duty Transistors cannot turn on significantly above 5V so restrict D to 80% */
void pwm_v(unsigned char D){
// Max duty cycle 80% to ensure VLED <= 5Vif (D>=80) D=80;
// Set duty cycle (D as %)OCR1B = (D*262)/100;
// Output OC1B on PINB2 (output from counter comparator)TCCR1A |= (1<<COM1B1);
}
/* Send byte to mainboard on UART */void uart_send_byte(unsigned char c){
// Wait for empty transmit bufferwhile (!(UCSR0A & (1<<UDRE0))) { // wait for write available flag
;}
// Put data into buffer & sendUDR0 = c;
}
// Send a single character with SLIP esc appendingvoid uart_send_esc(unsigned char byte) {
switch(byte) {// code data character with same value as END as 2 characters: ESC, ESC_ENDcase END:
uart_send_byte(ESC);uart_send_byte(ESC_END);break;
// code data character with same value as ESC as 2 characters: ESC, ESC_ESCcase ESC:
uart_send_byte(ESC);uart_send_byte(ESC_ESC);break;
// else send data character as isdefault:
uart_send_byte(byte);break;
}}
/* Send a packet of length len, starting at *data Largely taken from RFC1055 specification */
void uart_send_packet(unsigned char *data, unsigned char len) {
// initial END character to flush out receive bufferuart_send_byte(END);
78
// for each bytewhile(len--) {
switch(*data) {// code data character with same value as END as 2 characters: ESC, ESC_ENDcase END:
uart_send_byte(ESC);uart_send_byte(ESC_END);break;
// code data character with same value as ESC as 2 characters: ESC, ESC_ESCcase ESC:
uart_send_byte(ESC);uart_send_byte(ESC_ESC);break;
// else send data character as isdefault:
uart_send_byte(*data);break;
}data++;}
// append END character to end of packetuart_send_byte(END);
}
volatile unsigned char uart_rx_buffer[40];volatile unsigned char uart_rx_cnt; // global var as repeatedly called by ISR
/* ISR for receiving a byte from mainboard Do SLIP decoding and add to buffer Call command function on packet complete Need access control of serial UART to ensure only send one message at a time */
// MSB as ESC flag on packet length counterISR(SIG_USART_RECV){
unsigned char byte;byte = UDR0;switch (byte) {
case END:// packet complete, perform action specified (if no previous bytes, discard)if(uart_rx_cnt != 0) uart_cmd();uart_rx_cnt = 0;break;
case ESC:uart_rx_cnt |= 0x80; // set MSB ESC flagbreak;
case ESC_END:if(uart_rx_cnt & 0x80) {
uart_rx_cnt &= ~(0x80);uart_rx_buffer[uart_rx_cnt++] = END;break;
}case ESC_ESC:
if(uart_rx_cnt & 0x80) {uart_rx_cnt &= ~(0x80);uart_rx_buffer[uart_rx_cnt++] = ESC;break;
}// Otherwise just add byte to buffer. In the event of an ESC, char // protocol violation we'll just send the byte and reset the esc flag.
79
default:if(uart_rx_cnt & 0x80) uart_rx_cnt &= ~(0x80);uart_rx_buffer[uart_rx_cnt++] = byte;break;
}}
// Perform commands from mainboard on serial packet complete// uart_rx_buffer[0] specifys command type// case statement of action to perform based on uart_rx_buffer[0]void uart_cmd(){
unsigned char i, command, quadid; // quadid = ir tx buffers 0 to 3 (quad A..D)quadid = uart_rx_buffer[0] & 0x0F;command = uart_rx_buffer[0] & 0xF0;
switch(command){
case SET_TX_POW:ir_tx_power = uart_rx_buffer[1];pwm_v(uart_rx_buffer[1]);break;
case SET_TO:ir_tx_packet[quadid].to = uart_rx_buffer[1];break;
case SET_FROM:self_id = uart_rx_buffer[1];ir_tx_packet[0].from = uart_rx_buffer[1];ir_tx_packet[1].from = uart_rx_buffer[1];ir_tx_packet[2].from = uart_rx_buffer[1];ir_tx_packet[3].from = uart_rx_buffer[1];break;
case SET_TYPE:ir_tx_packet[quadid].type = uart_rx_buffer[1];break;
case SET_LEN: // use for hello packet (len 0)ir_tx_packet[quadid].len = uart_rx_buffer[1];
case SET_DATA: // automatically set len for data payloadfor(i=1; i<uart_rx_cnt; i++){
ir_tx_packet[quadid].data[i-1] = uart_rx_buffer[i];}ir_tx_packet[quadid].len = uart_rx_cnt - 1; // no. of payload bytes receivedbreak;
case SEND_ALL: // frame packets on all 4 buffers and set tx maskir_frame_packet(0, (uart_rx_buffer[1] & 0xF0) | 0x01);ir_frame_packet(1, (uart_rx_buffer[1] & 0xF0) | 0x02);ir_frame_packet(2, (uart_rx_buffer[1] & 0xF0) | 0x04);ir_frame_packet(3, (uart_rx_buffer[1] & 0xF0) | 0x08);
ir_global_tx_msk = uart_rx_buffer[1]; // ZYXWDCBAbreak;
}}
// Main function, sends bytes if global_tx_msk set// Interrupt driven IR tx/rx and uart tx/rxint main(){ initialise();
while (1) {
80
if (ir_global_tx_msk) {ir_send_packets(ir_global_tx_msk);ir_global_tx_msk = 0;
}}
}
D.3 Sens-r MainboardThe following gives the mainboard code which includes the configuration file ‘os_cfg.h’, the header file ‘includes.h’ and the program code ‘atmega128.c’.
D.3.1 os_cfg.h
/*********************************************************************************************************** uC/OS-II* The Real-Time Kernel** (c) Copyright 1992-2003, Jean J. Labrosse, Weston, FL* All Rights Reserved** uC/OS-II Configuration File for V2.7x** File : OS_CFG.H* By : Jean J. Labrosse** Modifications by Julius Luukko 2003-07-14 ([email protected]) for avr-gcc test program.** - CPU_CLOCK_HZ must be defined here (used in os_cpu_a.asm)* - defines a default stack size, which is used in all stack size definitions** Your applications must define CPU_CLOCK_HZ!** Modified by Leo Felix Priestnall 2008/09***********************************************************************************************************/
#ifndef OS_CFG_H#define OS_CFG_H
#define CPU_CLOCK_HZ 16000000 //3686400
#define OS_TASK_DEF_STK_SIZE 128 /* Default stack size */
/* ---------------------- MISCELLANEOUS ----------------------- */#define OS_ARG_CHK_EN 0 /* Enable (1) or Disable (0) argument checking */#define OS_CPU_HOOKS_EN 1 /* uC/OS-II hooks are found in the processor port files */
#define OS_DEBUG_EN 1 /* Enable(1) debug variables */
81
#define OS_EVENT_NAME_SIZE 32 /* Determine the size of the name of a Sem, Mutex, Mbox or Q */
#define OS_LOWEST_PRIO 15 //12 /* Defines the lowest priority that can be assigned ... */ /* ... MUST NEVER be higher than 63! */
#define OS_MAX_EVENTS 15 //3 /* Max. number of event control blocks in your application */#define OS_MAX_FLAGS 1 /* Max. number of Event Flag Groups in your application */#define OS_MAX_MEM_PART 1 /* Max. number of memory partitions */#define OS_MAX_QS 2 /* Max. number of queue control blocks in your application */#define OS_MAX_TASKS 5 //2 /* Max. number of tasks in your application, MUST be >= 2 */
#define OS_SCHED_LOCK_EN 1 /* Include code for OSSchedLock() and OSSchedUnlock() */
#define OS_TASK_IDLE_STK_SIZE OS_TASK_DEF_STK_SIZE /* Idle task stack size (# of OS_STK wide entries) */
#define OS_TASK_STAT_EN 1 /* Enable (1) or Disable(0) the statistics task */#define OS_TASK_STAT_STK_SIZE OS_TASK_DEF_STK_SIZE /* Statistics task stack size (# of OS_STK wide entries) */#define OS_TASK_STAT_STK_CHK_EN 1 /* Check task stacks from statistic task */
#define OS_TICK_STEP_EN 1 /* Enable tick stepping feature for uC/OS-View */#define OS_TICKS_PER_SEC 61 //50 /* Set the number of ticks in one second */
/* ----------------------- EVENT FLAGS ------------------------ */#define OS_FLAG_EN 0 /* Enable (1) or Disable (0) code generation for EVENT FLAGS */#define OS_FLAG_WAIT_CLR_EN 0 /* Include code for Wait on Clear EVENT FLAGS */#define OS_FLAG_ACCEPT_EN 0 /* Include code for OSFlagAccept() */#define OS_FLAG_DEL_EN 0 /* Include code for OSFlagDel() */#define OS_FLAG_NAME_SIZE 32 /* Determine the size of the name of an event flag group */#define OS_FLAG_QUERY_EN 0 /* Include code for OSFlagQuery() */
/* -------------------- MESSAGE MAILBOXES --------------------- */#define OS_MBOX_EN 0 /* Enable (1) or Disable (0) code generation for MAILBOXES */
82
#define OS_MBOX_ACCEPT_EN 0 /* Include code for OSMboxAccept() */#define OS_MBOX_DEL_EN 0 /* Include code for OSMboxDel() */#define OS_MBOX_POST_EN 0 /* Include code for OSMboxPost() */#define OS_MBOX_POST_OPT_EN 0 /* Include code for OSMboxPostOpt() */#define OS_MBOX_QUERY_EN 0 /* Include code for OSMboxQuery() */
/* --------------------- MEMORY MANAGEMENT -------------------- */#define OS_MEM_EN 0 /* Enable (1) or Disable (0) code generation for MEMORY MANAGER */#define OS_MEM_QUERY_EN 0 /* Include code for OSMemQuery() */#define OS_MEM_NAME_SIZE 32 /* Determine the size of a memory partition name */
/* ---------------- MUTUAL EXCLUSION SEMAPHORES --------------- */#define OS_MUTEX_EN 1 /* Enable (1) or Disable (0) code generation for MUTEX */#define OS_MUTEX_ACCEPT_EN 0 /* Include code for OSMutexAccept() */#define OS_MUTEX_DEL_EN 0 /* Include code for OSMutexDel() */#define OS_MUTEX_QUERY_EN 0 /* Include code for OSMutexQuery() */
/* ---------------------- MESSAGE QUEUES ---------------------- */#define OS_Q_EN 0 /* Enable (1) or Disable (0) code generation for QUEUES */#define OS_Q_ACCEPT_EN 0 /* Include code for OSQAccept() */#define OS_Q_DEL_EN 0 /* Include code for OSQDel() */#define OS_Q_FLUSH_EN 0 /* Include code for OSQFlush() */#define OS_Q_POST_EN 0 /* Include code for OSQPost() */#define OS_Q_POST_FRONT_EN 0 /* Include code for OSQPostFront() */#define OS_Q_POST_OPT_EN 0 /* Include code for OSQPostOpt() */#define OS_Q_QUERY_EN 0 /* Include code for OSQQuery() */
/* ------------------------ SEMAPHORES ------------------------ */#define OS_SEM_EN 1 /* Enable (1) or Disable (0) code generation for SEMAPHORES */#define OS_SEM_ACCEPT_EN 0 /* Include code for OSSemAccept() */#define OS_SEM_DEL_EN 0 /* Include code for OSSemDel() */
83
#define OS_SEM_QUERY_EN 0 /* Include code for OSSemQuery() */
/* --------------------- TASK MANAGEMENT ---------------------- */#define OS_TASK_CHANGE_PRIO_EN 0 /* Include code for OSTaskChangePrio() */#define OS_TASK_CREATE_EN 1 /* Include code for OSTaskCreate() */#define OS_TASK_CREATE_EXT_EN 1 /* Include code for OSTaskCreateExt() */#define OS_TASK_DEL_EN 1 /* Include code for OSTaskDel() */#define OS_TASK_NAME_SIZE 32 /* Determine the size of a task name */#define OS_TASK_PROFILE_EN 0 /* Include variables in OS_TCB for profiling */#define OS_TASK_QUERY_EN 0 /* Include code for OSTaskQuery() */#define OS_TASK_SUSPEND_EN 0 /* Include code for OSTaskSuspend() and OSTaskResume() */#define OS_TASK_SW_HOOK_EN 1 /* Include code for OSTaskSwHook() */
/* --------------------- TIME MANAGEMENT ---------------------- */#define OS_TIME_DLY_HMSM_EN 1 /* Include code for OSTimeDlyHMSM() */#define OS_TIME_DLY_RESUME_EN 0 /* Include code for OSTimeDlyResume() */#define OS_TIME_GET_SET_EN 0 /* Include code for OSTimeGet() and OSTimeSet() */#define OS_TIME_TICK_HOOK_EN 1 /* Include code for OSTimeTickHook() */
typedef INT8U OS_FLAGS; /* Data type for event flag bits (8, 16 or 32 bits) */
#endif
D.3.2 includes.h
/*********************************************************************************************************** uC/OS-II* The Real-Time Kernel** AVR Specific code** File : INCLUDES.H* By : Ole Saether* Port Version : V1.01** Modifications by Julius Luukko 07-21-2003 ([email protected]) to get this compiled with * uC/OS-II 2.52 and 2.70.*
84
* Modified by Leo Felix Priestnall 2008/09***********************************************************************************************************/
#include <avr/io.h>#define F_CPU (16000000UL)#include <avr/interrupt.h>#include <string.h>
/* If you have uC/OS-II version below 2.70 uncomment the includes for os_cpu.h and os_cfg.h */#include "os_cpu.h"#include "os_cfg.h"#include <ucos_ii.h>
/* ISR support macros */#include <avr_isr.h>
D.3.3 atmega128.c
// SENS-R MAINBOARD// 2008/09 Leo Felix Priestnall
#include "includes.h"#include <avr/io.h>#define F_CPU (16000000UL)#include <util/delay.h>#include <avr/interrupt.h>#include <string.h>#include <stdio.h>#include <stdlib.h>#include <math.h> // libm.a linked in config options#include <avr/pgmspace.h>
// Defines#define ANCHOR_X 0#define ANCHOR_Y 1
#define POWER_MAX 80 // absolute max 80#define POWER_MIN 2 // cannot be 0#define POWER_STEP 2
#define BETA_BASE 0.7 // max 1
// Buffers Sizes#define PC_MAX_TX_BUF 20#define PC_MAX_RX_BUF 20#define COMMS_MAX_TX_BUF 40#define COMMS_MAX_RX_BUF 40
// Buffersvolatile unsigned char pc_tx_buf[PC_MAX_TX_BUF];volatile unsigned char pc_tx_r, pc_tx_w, pc_tx_cnt;volatile unsigned char comms_tx_buf[COMMS_MAX_TX_BUF];volatile unsigned char comms_tx_r, comms_tx_w, comms_tx_cnt;volatile unsigned char comms_rx_buf[COMMS_MAX_RX_BUF];volatile unsigned char comms_rx_cnt;
// SLIP RFC1055 special character codes
85
#define END 192 // end of packet#define ESC 219 // byte stuffing#define ESC_END 220 // ESC ESC_END means END data byte#define ESC_ESC 221 // ESC ESC_ESC means ESC data byte
// Serial packet commands as per Joseph's code// serial packet format <COMMAND><PAYLOAD>...<END>#define SEND_ALL 0x10#define SET_TX_POW 0x20#define SET_TO 0x30#define SET_FROM 0x40#define SET_TYPE 0x50#define SET_DATA 0x60#define SET_LEN 0x70
// Functionsvoid initialise();void die(const char *errmsg);void check_error(int error_code, const char *errmsg);
void comms_send_byte(unsigned char c);void comms_send_esc(unsigned char byte);void comms_cmd(unsigned char command, unsigned char id, unsigned char *data, unsigned char len);void comms_recv_packet(unsigned char cnt);void pc_send_byte(char c);void pc_send_msg(char *data);void pc_send_msg_P(const char *data);void pc_send_errormsg(char *data);void pc_send_errormsg_P(const char *data);
void update_neighbour_table(unsigned char node, double x, double y, unsigned char hops);void update_bearing_table(unsigned char from, unsigned char to, double bearing);void update_position(unsigned char a, unsigned char b);void neighbour_update_tx();
void pc_print_neighbour_table();void pc_print_bearing_table();
// Stacks#define TASK_STACKSIZE 256
OS_STK idle_stk[TASK_STACKSIZE];OS_STK comms_stk[TASK_STACKSIZE];OS_STK hello_seq_stk[TASK_STACKSIZE];
// Task Priorities #define IDLE_PRIORITY 10#define HELLO_SEQ_PRIORITY 9#define COMMS_PRIORITY 8
// Semaphore/Mutex Priorities#define MUTEX_COMMS_MSG_TX_PRIORITY1#define MUTEX_PC_TX_PRIORITY 2#define MUTEX_COMMS_CMD_PRIORITY 0
// Semaphores/MutexsOS_EVENT * mutex_pc_tx;OS_EVENT * mutex_comms_msg_tx;OS_EVENT * sem_pc_tx;OS_EVENT * sem_comms_tx;
86
OS_EVENT * mutex_comms_cmd;OS_EVENT * sem_hello_seq;OS_EVENT * sem_comms_rx;
// Tasksvoid idle_task();void comms_task();void hello_seq_task();
// Global Variablesstatic unsigned char dip;unsigned char error;char msg[100];
// Hello Seq Receive Power Table (Single Buffer)volatile unsigned char hello_array[8]; // local Rxvolatile unsigned char hello_node_id; // 0bF000XXXX MSB 'in seq' flag, last four bits node ID
// Neighbour Table Structurevolatile struct neighbour {
unsigned char id; // node ID used in DA, SAunsigned char status; // anchor, known or unknowndouble x; // x positiondouble y; // y positionunsigned char hops; // max no. of hops to an anchor node
} neighbour_table[8]; // self
volatile unsigned char neighbour_table_p;
#define self neighbour_table[0]
// Bearing Tablevolatile double bearing_table[8][8] = {[0 ... 7]{[0 ... 7] = NAN}};
// Received packet buffer (put in comms task??)volatile struct comms_packet {
volatile unsigned char to, from, type, len, data[30], txid, txpow, rxid; //txno} comms_rx_packet;
// Send a string to the PC via UARTvoid pc_send_msg(char *data){
unsigned char i = 0;
while (data[i] != '\0'){
pc_send_byte(data[i++]);}
}
// Send a string to the PC via UART (for constant strings stored in program memory)void pc_send_msg_P(const char *data){
while (pgm_read_byte(data) != 0x00){
pc_send_byte(pgm_read_byte(data++));}
}
// Send a string to the PC via UARTvoid pc_send_errormsg(char *data)
87
{unsigned char i = 0;
while (data[i] != '\0'){
while (!(UCSR0A & (1<<UDRE0))){
;}
UDR0 = data[i++];}
}
// Send a string to the PC via UART (for constant strings stored in program memory)void pc_send_errormsg_P(const char *data){
while (pgm_read_byte(data) != 0x00){
while (!(UCSR0A & (1<<UDRE0))){
;}
UDR0 = pgm_read_byte(data++);}
}
// Send byte to PC on USART0void pc_send_byte(char c){
OSSemPend(sem_pc_tx, 0, &error);
OS_ENTER_CRITICAL();// write character to buffer and point to next array spacepc_tx_buf[pc_tx_w++] = c;// for end of buffer point back to topif (pc_tx_w == PC_MAX_TX_BUF)
pc_tx_w = 0;// no. of bytes to sendpc_tx_cnt++;// enable uart0 data reg empty interruptUCSR0B |= (1<<UDRIE0);
OS_EXIT_CRITICAL();}
UCOSISR(SIG_USART0_DATA){
PushRS(); // save all registersOSIntEnter(); // enter ucos ISRcli(); // disable interruptsif (OSIntNesting == 1)
// if 1st level interrupt save task stack ptr OSTCBOSTCBCur->OSTCBStkPtr = (OS_STK *) SP;
// if there are still bytes to sendif (pc_tx_cnt) {
pc_tx_cnt--;UDR0 = pc_tx_buf[pc_tx_r++]; // put byte into uart0 data reg
if (pc_tx_r == PC_MAX_TX_BUF)pc_tx_r = 0; // point to top of buffer
88
// can add another byte to bufferOSSemPost(sem_pc_tx);
}else UCSR0B &= ~(1<<UDRIE0); // disable data reg empty interrupt
sei(); // enable interruptsOSIntExit(); // exit ucos ISRPopRS(); // restore registers
}
// Print error and dievoid die(const char *errmsg){
pc_send_errormsg_P(errmsg);
exit(1);}
// Error function - checks and displays error if existsvoid check_error(int error_code, const char *errmsg){
char error_msg[50];
if (error_code != OS_NO_ERR){
OS_ENTER_CRITICAL();sprintf_P(error_msg, PSTR("FATAL ERROR - Code %d: "), error_code);pc_send_errormsg(error_msg);pc_send_errormsg_P(errmsg);OS_EXIT_CRITICAL();
exit(1);}
}
// Send byte to comms board on USART1void comms_send_byte(unsigned char c){
OSSemPend(sem_comms_tx, 0, &error);
OS_ENTER_CRITICAL();// write character to buffer and point to next array spacecomms_tx_buf[comms_tx_w++] = c;// for end of buffer point back to topif (comms_tx_w == COMMS_MAX_TX_BUF)
comms_tx_w = 0;// no. of bytes to sendcomms_tx_cnt++;// enable uart0 data reg empty interruptUCSR1B |= (1<<UDRIE1);
OS_EXIT_CRITICAL();}
UCOSISR(SIG_USART1_DATA){
PushRS(); // save all registersOSIntEnter(); // enter ucos ISRcli(); // disable interruptsif (OSIntNesting == 1)
// if 1st level interrupt save task stack ptr OSTCBOSTCBCur->OSTCBStkPtr = (OS_STK *) SP;
89
// if there are still bytes to sendif (comms_tx_cnt) {
comms_tx_cnt--;UDR1 = comms_tx_buf[comms_tx_r++]; // put byte into uart1 data reg
if (comms_tx_r == COMMS_MAX_TX_BUF)comms_tx_r = 0; // point to top of buffer
// can add another byte to bufferOSSemPost(sem_comms_tx);
}else UCSR1B &= ~(1<<UDRIE1); // disable data reg empty interrupt
sei(); // enable interruptsOSIntExit(); // exit ucos ISRPopRS(); // restore registers
}
// Send a single character with SLIP esc appendingvoid comms_send_esc(unsigned char byte) {
switch(byte) {// code data character with same value as END as 2 characters: ESC, ESC_ENDcase END:
comms_send_byte(ESC);comms_send_byte(ESC_END);break;
// code data character with same value as ESC as 2 characters: ESC, ESC_ESCcase ESC:
comms_send_byte(ESC);comms_send_byte(ESC_ESC);break;
// else send data character as isdefault:
comms_send_byte(byte);break;
}}
// Send commands to comms boardvoid comms_cmd(unsigned char command, unsigned char quadid, unsigned char *data, unsigned char len){
// quadid = quadrant to send on, 4 tx buffers 0..3 (A..D) (note: this is not txmask DCBA)
// command = command to give comms board// len is reqd for setting payload of a packet// txmask set on send_all cmdunsigned char i;
OSMutexPend(mutex_comms_cmd, 0, &error);comms_send_byte(END);
comms_send_esc(command | (quadid & 0x0F));for(i=0; i<len; i++) {
comms_send_esc(data[i]);}
comms_send_byte(END);OSMutexPost(mutex_comms_cmd);
}
90
/* ISR for receiving a byte from comms boardDo SLIP decoding and add to bufferCall function on packet complete to decode & act on contents */UCOSISR(SIG_USART1_RECV){
unsigned char byte;
PushRS(); // save all registersOSIntEnter(); // enter ucos ISRif (OSIntNesting == 1)
// if 1st level interrupt save task stack ptr OSTCBOSTCBCur->OSTCBStkPtr = (OS_STK *) SP;
byte = UDR1;
// SLIP decoding & packet formationswitch (byte) {
case END:// packet complete, signal receive task to perform actionsif(comms_rx_cnt != 0)
//OSMboxPost(mbox_comms_rx, (void *) comms_rx_buf);OSSemPost(sem_comms_rx);
comms_rx_cnt = 0;break;
case ESC:comms_rx_cnt |= 0x80; // set MSB ESC flagbreak;
case ESC_END:if(comms_rx_cnt & 0x80) {
comms_rx_cnt &= ~(0x80);comms_rx_buf[comms_rx_cnt++] = END;break;
}case ESC_ESC:
if(comms_rx_cnt & 0x80) {comms_rx_cnt &= ~(0x80);comms_rx_buf[comms_rx_cnt++] = ESC;break;
}// Otherwise just add byte to buffer. In the event of an ESC, char // protocol violation we'll just send the byte and reset the esc flag.default:
if(comms_rx_cnt & 0x80) comms_rx_cnt &= ~(0x80);comms_rx_buf[comms_rx_cnt++] = byte;break;
}
OSIntExit(); // exit ucos ISRPopRS(); // restore registers
}
// Initialisation functionvoid initialise(void){
/* Initialising portsDDxm PORTxn PUD State0 0 X Z0 1 0 1,0 depending on input0 1 1 Z1 0 X 0 out1 1 X 1 out */
91
// Port A (LEDs)DDRA = 0xFF;PORTA = 0x0;
// Port B (Motor Control, tristate all)DDRB = 0x00;PORTB = 0x00;
// Port C (GPIO, tristate all);DDRC = 0x00;PORTC = 0x00;
// Port D (Dip 7..4 & Serial Tx 3 Rx 2)DDRD = 0b00000000; // Tx/Rx overidden by Tx/Rx EnablePORTD = 0b11110000;
// Port E (Encoders tristate, PC Serial PDO 1 PDI 0)DDRE = 0x00; // assume Tx/Rx overidden by Tx/Rx EnablePORTE = 0x00;
// Timer/Counter 0: Periodic interrupt for ucos-II scheduler// fosc/1024TCCR0 = 0x07; // Initialise counterTCNT0 = 256 - (CPU_CLOCK_HZ/OS_TICKS_PER_SEC/1024);// Overflow interrupt enableTIMSK = (1<<TOIE0);
// Initialising UART to Comms// Baud Rate 19200, fosc 10MHz UBBR 32, fosc 16MHz UBRR 51UBRR1L = 51;UBRR1H = 0;// Enable Rx, Tx, Rx Complete InterruptUCSR1B = (1<<RXEN1)|(1<<TXEN1)|(1<<RXCIE1);// Set frame format: 8 data bits, 1 stop bit (0<<USBS1), 0 parity (0<<UPM10), asynch
mode (0<<UMSEL1)UCSR1C = (1<<UCSZ11)|(1<<UCSZ10);
// Initialising UART to PC// Baud Rate 19200, fosc 16MHz UBRR 51UBRR0L = 51;UBRR0H = 0;// Enable Rx, TxUCSR0B = (1<<RXEN0)|(1<<TXEN0);// Set frame format: 8 data bits, 1 stop bit (0<<USBS1), 0 parity (0<<UPM10), asynch
mode (0<<UMSEL1)UCSR0C = (1<<UCSZ11)|(1<<UCSZ10);
// Enable interruptssei();
}
int main(){
OSInit();initialise();
sem_pc_tx = OSSemCreate(PC_MAX_TX_BUF);if(sem_pc_tx == NULL)
die(PSTR("FATAL ERROR: Unable to initalize semaphore 'sem_pc_tx' in main\n\r"));
92
sem_comms_tx = OSSemCreate(COMMS_MAX_TX_BUF);if(sem_comms_tx == NULL)
die(PSTR("FATAL ERROR: Unable to initalize semaphore 'sem_comms_tx' in main\n\r"));
mutex_comms_cmd = OSMutexCreate(MUTEX_COMMS_CMD_PRIORITY, &error);if(mutex_comms_cmd == NULL)
die(PSTR("FATAL ERROR: Unable to initalize mutex 'mutex_comms_cmd' in main\n\r"));
mutex_comms_msg_tx = OSMutexCreate(MUTEX_COMMS_MSG_TX_PRIORITY, &error);if(mutex_comms_cmd == NULL)
die(PSTR("FATAL ERROR: Unable to initalize mutex 'mutex_comms_msg_tx' in main\n\r"));
// used as signalling mutexsem_hello_seq = OSSemCreate(1);if(sem_hello_seq == NULL)
die(PSTR("FATAL ERROR: Unable to initalize semaphore 'sem_hello_seq' in main\n\r"));
// used as signalling mutexsem_comms_rx = OSSemCreate(0);if(sem_comms_rx == NULL)
die(PSTR("FATAL ERROR: Unable to initalize semaphore 'sem_comms_rx' in main\n\r"));
error = OSTaskCreateExt(idle_task, NULL, (void *)&idle_stk[TASK_STACKSIZE], IDLE_PRIORITY, IDLE_PRIORITY, idle_stk, TASK_STACKSIZE, NULL, 0);
check_error(error, PSTR("Failed to create task 'idle_task'\n\r"));
error = OSTaskCreateExt(comms_task, NULL, (void *)&comms_stk[TASK_STACKSIZE], COMMS_PRIORITY, COMMS_PRIORITY, comms_stk, TASK_STACKSIZE, NULL, 0);
check_error(error, PSTR("Failed to create task 'comms_task'\n\r"));
pc_send_errormsg_P(PSTR("\n\r\n\r*GO!* "));OSStart();return 0;
}
// note: comms_cmd(unsigned char command, unsigned char quadid (buffer 0 to 3), unsigned char *data, unsigned char len)void hello_seq_task(){
unsigned char i;unsigned char pow, txmsk; // ZYWXDCBAunsigned char to = 0xFF, type = 'H', len = 0;
pc_send_msg_P(PSTR("-H tx-"));
while (1) {
for (i=0; i<4; i++) { // cycle on X, W, Y, Ztxmsk = (1<<(i+4)) | 0x0F;pow = POWER_MIN; // duty cycle 8 txpower
while (pow <= POWER_MAX) {
OSMutexPend(mutex_comms_msg_tx, 0, &error);
comms_cmd(SET_TO, i, &to, 1);comms_cmd(SET_FROM, 0, &self.id, 1);comms_cmd(SET_TYPE, i, &type, 1);comms_cmd(SET_LEN, i, &len, 1);
93
comms_cmd(SET_TX_POW, 0, &pow, 1);comms_cmd(SEND_ALL, 0, &txmsk, 1);
pow += POWER_STEP;
OSTimeDlyHMSM(0,0,0,200); // time to send hello packet
OSMutexPost(mutex_comms_msg_tx);}
}
// send seq at max power with tx_pow field 0xFF to indicate end of sequencefor (i=0; i<4; i++) { // cycle on X, W, Y, Z
txmsk = (1<<(i+4)) | 0x0F;pow = 0xFF; // duty cycle 8 txpower
OSMutexPend(mutex_comms_msg_tx, 0, &error);
comms_cmd(SET_TO, i, &to, 1);comms_cmd(SET_FROM, 0, &self.id, 1);comms_cmd(SET_TYPE, i, &type, 1);comms_cmd(SET_LEN, i, &len, 1);
comms_cmd(SET_TX_POW, 0, &pow, 1);comms_cmd(SEND_ALL, 0, &txmsk, 1);
OSTimeDlyHMSM(0,0,0,200); // time to send hello packet
OSMutexPost(mutex_comms_msg_tx);}
pc_send_msg_P(PSTR("-H tx END-"));
error = OSSemPost(sem_hello_seq);
OSTaskDel(OS_PRIO_SELF);}
}
void idle_task(){
unsigned char i, j;
OS_ENTER_CRITICAL();// INITIALISE TABLES// get DIP switch valuedip = ~(PIND>>4) & (0x0F);
// initialise self on neighbour tableself.id = dip;if (dip & 0x08) { // MSB bit on DIP switch
self.x = ANCHOR_X;self.y = ANCHOR_Y;self.hops = 0;self.status = 'A';
} else {
self.x = NAN;self.y = NAN;self.hops = 0xFF;self.status = 'U';
}
94
// initialise remaining rows on neighbour tablefor (i=1; i<8; i++) {
neighbour_table[i].status = 'U';neighbour_table[i].x = NAN;neighbour_table[i].y = NAN;neighbour_table[i].hops = 0xFF;
}OS_EXIT_CRITICAL();
// set self_id on comms boardcomms_cmd(SET_FROM, 0, &self.id, 1);
//sprintf_P(msg, PSTR("self id %d"), self.id);//pc_send_msg(msg);
while (1) {// rand 0 to 32767, so j = 0 to 64j = (rand())/500;
// 1 min delay + random (up to another 64s)for (i=0; i<(60+j); i++) {
PORTA++;OSTimeDlyHMSM(0,0,1,0);pc_send_msg_P(PSTR("."));
}
//start hello sequenceOSSemPend(sem_hello_seq, 0, &error);
// if we are receiving an rx seq wait until done receiving//while (hello_node_id & 0x80) {//OSTimeDlyHMSM(0,0,5,0);//}
error = OSTaskCreateExt(hello_seq_task, NULL, (void *)&hello_seq_stk[TASK_STACKSIZE], HELLO_SEQ_PRIORITY, HELLO_SEQ_PRIORITY, hello_seq_stk, TASK_STACKSIZE, NULL, 0);
check_error(error, PSTR("Failed to create task 'hello_seq_task'\n\r"));}
}
// Perform tasks based on messages from comms on serial packet complete// Packet format <DA><SA><TXID><TXPOW><RXID><TYPE><LEN><DATA...>// All packets from comms board have correct length and CRC check passed
void comms_task(){
unsigned char node;double num;int i, j, rxmin, den, rxangle;unsigned int rxbearing;int xint, yint;double x, y;double bearing;unsigned char minhops;unsigned char r;
while (1) {OSSemPend(sem_comms_rx, 0, &error);// Rx packet received
95
r = (rand())/6552; // 0 to 5
OS_ENTER_CRITICAL();comms_rx_packet.to = comms_rx_buf[0];comms_rx_packet.from = comms_rx_buf[1]; // node IDcomms_rx_packet.txid = comms_rx_buf[2]; // ZYXWDBCAcomms_rx_packet.txpow = comms_rx_buf[3];// quad c gnd w 0 bearing for tx, ir_in2 0 bearing for rxcomms_rx_packet.rxid = (9 - comms_rx_buf[4])%8; // rxid 0 - 7, converted to
account for comms wiringcomms_rx_packet.type = comms_rx_buf[5];comms_rx_packet.len = comms_rx_buf[6];
if (comms_rx_packet.len) {for (i=0; i<comms_rx_packet.len; i++) {
comms_rx_packet.data[i] = comms_rx_buf[i+7];}
}OS_EXIT_CRITICAL();
// add node to neighbour table if newfor (i=1; i<8; i++) {
if (neighbour_table[i].id == comms_rx_packet.from)break;
else if (neighbour_table[i].id == 0) {neighbour_table[i].id = comms_rx_packet.from;break;
}}
neighbour_table_p = i;
//sprintf_P(msg, PSTR("%c rx-"), comms_rx_packet.type);//pc_send_msg(msg);
switch (comms_rx_packet.type) {// hello packets have DA 0xFF as they are broadcast packets// has no data bytes, non valid power field OxFF indicates sequence endcase 'H':
//sprintf_P(msg, PSTR("H:n%d-rx:%d-p:%d-"), (int) neighbour_table[neighbour_table_p].id,
//(int) comms_rx_packet.rxid, (int) comms_rx_packet.txpow);//pc_send_msg(msg);
if (comms_rx_packet.txpow != 0xFF) {if (!(hello_node_id & 0x80)) { // start of new hello sequence
hello_node_id = comms_rx_packet.from | 0x80;hello_array[comms_rx_packet.rxid] = comms_rx_packet.txpow;
}else if ((hello_node_id & 0x0F) == comms_rx_packet.from) { // continue
sequence (discard hello packets from other nodes for now)// write power level to array if no previous value written (ie. store min
power for valid hello recv)if (hello_array[comms_rx_packet.rxid] == 0)
hello_array[comms_rx_packet.rxid]= comms_rx_packet.txpow;else if (comms_rx_packet.txpow < hello_array[comms_rx_packet.rxid])
hello_array[comms_rx_packet.rxid] = comms_rx_packet.txpow;}
//for (i=0; i<8; i++) {//sprintf_P(msg, PSTR("%d-"), (int) hello_array[i]);//pc_send_msg(msg);
96
//}
}// end of sequence txpow 0xFF, calculate bearing & ignore subsequent end seq
packets by resetting flagelse if ((hello_node_id & 0x80) && ((hello_node_id & 0x0F) ==
comms_rx_packet.from)) { hello_node_id &= 0x7F; // reset hello seq flagnum = 0;den = 0;rxmin = 0;
// calculate bearing based on modified hoyt's centroid method// passive, using only rx info (doesn't require reply msgs)// set rx with lowest (nonzero) pwr value to be bearing 0// calculate delta from that bearing & add actual rx angle to get bearing
// find rx with lowest received pwrfor (i=1; i<8; i++) { // rx
if (hello_array[i] != 0) {if ((hello_array[i] < hello_array[rxmin]) || (hello_array[rxmin] == 0))
rxmin = i;}
}
//sprintf_P(msg, PSTR("\n\rrxmin: %d-t: "), rxmin);//pc_send_msg(msg);
for (i=0; i<8; i++) {
// rxmin assigned as bearing 0j = (rxmin + i)%8;
// sequence 0, 45, 90, 135, 180, -135, -90, -45if (i<=4)
rxangle = 45*i;if (i>4)
rxangle = -180 + 45*(i-4);
//sprintf_P(msg, PSTR("%d:%d "), j, (int) hello_array[j]);//pc_send_msg(msg);
if (hello_array[j] != 0) {num += ((POWER_MAX - hello_array[j]) * rxangle);den += (POWER_MAX - hello_array[j]);
}
hello_array[j] = 0; // reset hello array
}
// update bearing table, self to tx nodebearing = num/den + rxmin*45; // delta (from rx) + local bearing of rxif (bearing < 0)
bearing += 360;
//sprintf_P(msg, PSTR("\n\r%3.2f"), bearing);//pc_send_msg(msg);
update_bearing_table(0, comms_rx_packet.from, bearing);
// calc new posn for all 1 hop triangles with new node bearing
97
// do position estimate if not an anchor nodeif (self.status != 'A') {
for (i=1; i<8; i++) {if (neighbour_table[i].id == 0)
break;
if (neighbour_table[neighbour_table_p].status != 'U') {if ((i != neighbour_table_p) && (neighbour_table[i].status != 'U')) {
if (!(isnan(bearing_table[0][neighbour_table_p]) || isnan(bearing_table[0][i])
|| isnan(bearing_table[neighbour_table_p][0]) || isnan(bearing_table[i][0])
|| isnan(bearing_table[neighbour_table_p][i]) || isnan(bearing_table[i][neighbour_table_p])))
update_position(neighbour_table_p, i);}
}}
}
// send update neighbour table packetif (self.status != 'U') {
OSTimeDlyHMSM(0,0,(r+1),0);neighbour_update_tx();
}
//pc_print_neighbour_table();//pc_print_bearing_table();
}
break;
// neighbour table update packets, give a nodes neighbour table info// used to update bearing table, own neighbour table and calc own posn// neighbour update msg payload
<x1><x2><y1><y2><hops><n1id><n1bearing1><n1bearing2>...<nNid><nNbearing1><nNbearing2>case 'N':
sprintf_P(msg, PSTR("N:%d-"), (int) neighbour_table[neighbour_table_p].id);pc_send_msg(msg);
// X and Y received as 16 bit signed words in terms of cm, store as double in mxint = (((comms_rx_packet.data[0])<<8) & 0xFF00) | (comms_rx_packet.data[1] &
0x00FF);yint = (((comms_rx_packet.data[2])<<8) & 0xFF00) | (comms_rx_packet.data[3] &
0x00FF);
x = (double) xint/100;y = (double) yint/100;
sprintf_P(msg, PSTR("x:%3.2f-y:%3.2f"), x, y);pc_send_msg(msg);
// update neighbour table with new position dataupdate_neighbour_table(neighbour_table_p, x, y, comms_rx_packet.data[4]);
// update hops info if neighbour is closer to anchor than other neighboursif (self.status != 'A') {
minhops = 0xFF;for (i=1; i<8; i++) {
if (neighbour_table[i].id == 0)break;
else if (neighbour_table[i].hops < minhops)
98
minhops = neighbour_table[i].hops;}
self.hops = minhops + 1;}
// decode bearing datai = 5;while (i < (comms_rx_packet.len-1)) {
node = comms_rx_packet.data[i++];// bearing received as unsigned 16 bit word, range 0 to 360, step size
360/65565// convert to double in degreesrxbearing = ((comms_rx_packet.data[i])<<8) | (comms_rx_packet.data[i+1]);bearing = ((double) 360/65535)*rxbearing;i+= 2;
sprintf_P(msg, PSTR("n:%d-b:%3.2f-"), (int) node, bearing);pc_send_msg(msg);
update_bearing_table(neighbour_table_p, node, bearing);}
// do position estimate if not an anchor node// need to know bearings to and from each node to form triangle// as well as other 2 nodes positionsif (self.status != 'A') {
for (i=1; i<8; i++) {if (neighbour_table[i].id == 0)
break;
if (neighbour_table[neighbour_table_p].status != 'U') {if ((i != neighbour_table_p) && (neighbour_table[i].status != 'U')) {
if (!(isnan(bearing_table[0][neighbour_table_p]) || isnan(bearing_table[0][i])
|| isnan(bearing_table[neighbour_table_p][0]) || isnan(bearing_table[i][0])
|| isnan(bearing_table[neighbour_table_p][i]) || isnan(bearing_table[i][neighbour_table_p])))
update_position(neighbour_table_p, i);}
}}
}
// send update neighbour table packetif (self.status != 'U') {
OSTimeDlyHMSM(0,0,(r+1),0);neighbour_update_tx();
}
//pc_print_neighbour_table();//pc_print_bearing_table();
break;}
}}
void update_neighbour_table(unsigned char i, double x, double y, unsigned char hops){
neighbour_table[i].x = x;
99
neighbour_table[i].y = y;neighbour_table[i].hops = hops;if (neighbour_table[i].hops == 0)
neighbour_table[i].status = 'A';else
neighbour_table[i].status = 'K';}
void update_bearing_table(unsigned char i, unsigned char to, double bearing){
unsigned char j;
for (j=0; j<8; j++) {if ((neighbour_table[j].id == to))
break;}
// if haven't yet received a packet from one node return without changing bearing table
if ((neighbour_table[j].id == to) && (neighbour_table[j].id != 0))bearing_table[i][j] = bearing;
}
void update_position(unsigned char a, unsigned char b){
double dAB, dAC, dBC, dx, dy, dPB, dPC;double acb, cba, bac;double alpha, beta, d;double x1, x2, y1, y2, xp, yp;double x_est = NAN;double y_est = NAN;char z;
pc_send_msg_P(PSTR("\n\r\n\r-p-"));//sprintf_P(msg, PSTR("a:%d-xa:%3.2f-ya:%3.2f"), (int) a, neighbour_table[a].x,
neighbour_table[a].y);//pc_send_msg(msg);//sprintf_P(msg, PSTR("b:%d-xb:%3.2f-yb:%3.2f"), (int) b, neighbour_table[b].x,
neighbour_table[b].y);//pc_send_msg(msg);
dx = neighbour_table[b].x - neighbour_table[a].x;dy = neighbour_table[b].y - neighbour_table[a].y;dAB = sqrt(square(dx) + square(dy));
//sprintf_P(msg, PSTR("dx:%3.2f-dy:%3.2f-dAB:%3.2f-"), dx, dy, dAB);//pc_send_msg(msg);
acb = fmod((bearing_table[0][b] - bearing_table[0][a] + 360), 360);cba = fmod((bearing_table[b][a] - bearing_table[b][0] + 360), 360);bac = fmod((bearing_table[a][0] - bearing_table[a][b] + 360), 360);
//sprintf_P(msg, PSTR("acb:%3.2f-cba:%3.2f-bac:%3.2f-"), acb, cba, bac);//pc_send_msg(msg);
// if colinear angles do not update position (noise sensitivity is too great)if ((acb <= 20) || ((acb >= 160) && (acb <= 200)) || (acb >= 340) ||(cba <= 20) || ((cba >= 160) && (cba <= 200)) || (cba >= 340) ||(bac <= 20) || ((bac >= 160) && (bac <= 200)) || (bac >= 340))
return;
if (fabs(dx) >= fabs(dy)) { // use x for disambiguation
100
if (neighbour_table[a].x < neighbour_table[b].x) { // A left of Bif(acb < 180) // CW
z = 1; // choose small y;else // CCW
z = 2; // choose large y;}else { // A right of B
if(acb < 180) // CWz = 2; // choose large y;
else // CCWz = 1; // choose small y;
}}else { // use y for disambiguation
if (neighbour_table[a].y < neighbour_table[b].y) { // A above Bif(acb < 180) // CW
z = 3; // choose large x;else // CCW
z = 4; // choose small x;}else { // A below B
if(acb < 180) // CWz = 4; // choose small x;
else // CCWz = 3; // choose large x;
}}
//sprintf_P(msg, PSTR("z:%d-"), (int) z);//pc_send_msg(msg);
acb *= (M_PI/180);cba *= (M_PI/180);bac *= (M_PI/180);
//sprintf_P(msg, PSTR("RAD acb:%3.2f-cba:%3.2f-bac:%3.2f-"), acb, cba, bac);//pc_send_msg(msg);
// sine rule to find dAC, dBCdAC = (dAB*sin(cba))/sin(acb);dBC = (dAB*sin(bac))/sin(acb);
//sprintf_P(msg, PSTR("dAC:%3.2f-dBC:%3.2f-"), dAC, dBC);//pc_send_msg(msg);
dPB = (square(dBC) - square(dAC) + square(dAB))/(2*dAB);dPC = sqrt(square(dBC) - square(dPB));
xp = neighbour_table[b].x - (dPB*(neighbour_table[b].x - neighbour_table[a].x))/dAB;yp = neighbour_table[b].y - (dPB*(neighbour_table[b].y - neighbour_table[a].y))/dAB;
x1 = xp + ((dPC*(neighbour_table[a].y - neighbour_table[b].y))/dAB);x2 = xp - ((dPC*(neighbour_table[a].y - neighbour_table[b].y))/dAB);y1 = yp - ((dPC*(neighbour_table[a].x - neighbour_table[b].x))/dAB);y2 = yp + ((dPC*(neighbour_table[a].x - neighbour_table[b].x))/dAB);
//sprintf_P(msg, PSTR("x1:%3.2f-y1:%3.2f-x2:%3.2f-y2:%3.2f"), x1, y1, x2, y2);//pc_send_msg(msg);
switch (z) {case 1: // choose small y
101
if (y1 > y2) {x_est = x2;y_est = y2;
}else {
x_est = x1;y_est = y1;
}break;
case 2: // choose large yif (y1 > y2) {
x_est = x1;y_est = y1;
}else {
x_est = x2;y_est = y2;
}break;
case 3: // choose large xif (x1 > x2) {
x_est = x1;y_est = y1;
}else {
x_est = x2;y_est = y2;
}break;
case 4: // choose small xif (x1 > x2) {
x_est = x2;y_est = y2;
}else {
x_est = x1;y_est = y1;
}break;
}
if (isnan(x_est) || isnan(y_est))return;
sprintf_P(msg, PSTR("x_est:%3.2f-y_est:%3.2f"), x_est, y_est);pc_send_msg(msg);
// adjust weightingd = (neighbour_table[a].hops + neighbour_table[b].hops)+2;
sprintf_P(msg, PSTR("d:%d-"), (int) d);pc_send_msg(msg);
beta = (double) BETA_BASE/d;alpha = 1 - beta;
//sprintf_P(msg, PSTR("beta:%.3f-alpha:%.3f-"), beta, alpha);//pc_send_msg(msg);
// take 1st estimate as initial positionif (isnan(self.x))
self.x = x_est;
102
if (isnan(self.y))self.y = y_est;
// EWMA with adjustable weighting (AHDWL)self.x = alpha * self.x + beta * x_est;self.y = alpha * self.y + beta * y_est;
if (!(isnan(self.x) || isnan(self.y)))self.status = 'K';
}
void neighbour_update_tx(){
// neighbour update msg payload <x1><x2><y1><y2><hops><n1id><n1bearing1><n1bearing2>...<nNid><nNbearing1><nNbearing2>
unsigned char neighbour_update_tx_payload[30];unsigned char i = 0, type = 'N', pow = 80;unsigned char j;unsigned char x1, x2, y1, y2;unsigned char node, pheta1, pheta2;double tempd;int tempi;unsigned int tempui;unsigned char quad, gnd, txmsk;
// convert x and y from double into 2 unsigned chars// 16 bits, represents x/y in cm signedtempd = self.x * 100;tempi = (lround(tempd)) & 0xFFFF;
x1 = (tempi>>8) & 0xFF;x2 = tempi & 0xFF;
tempd = self.y * 100;tempi = (lround(tempd)) & 0xFFFF;
y1 = (tempi>>8) & 0xFF;y2 = tempi & 0xFF;
neighbour_update_tx_payload[i++] = x1;neighbour_update_tx_payload[i++] = x2;neighbour_update_tx_payload[i++] = y1;neighbour_update_tx_payload[i++] = y2;neighbour_update_tx_payload[i++] = self.hops;
sprintf_P(msg, PSTR("-N tx-x:%3.2f-y:%3.2f-d:%d-"), self.x, self.y, (int) self.hops);pc_send_msg(msg);//pc_send_msg_P(PSTR("-N tx-"));
// add all nodes for which we have bearing data to packet payloadfor (j=1; j<8; j++) {
if (!isnan(bearing_table[0][j])) {node = neighbour_table[j].id;tempui = (lround(((double) 65535/360)*bearing_table[0][j])) & 0xFFFF;pheta1 = (tempui>>8) & 0x00FF;pheta2 = tempui & 0x00FF;
neighbour_update_tx_payload[i++] = node;neighbour_update_tx_payload[i++] = pheta1;neighbour_update_tx_payload[i++] = pheta2;
103
sprintf_P(msg, PSTR("n:%d-b:%3.2f-"), (int) neighbour_table[j].id, bearing_table[0][j]);
pc_send_msg(msg);}
}
// send update neighbour table packet to all nodes which we have bearing data forfor (j=1; j<8; j++) {
if (!isnan(bearing_table[0][j])) {tempi = round(bearing_table[0][j]);quad = ((tempi/90)+2)%4; // A, B, C or D, quad C gnd W is 0 bearinggnd = (tempi%90)/22.5; // W, X, Y or Z
txmsk = ((1<<(gnd+4)) | (1<<quad));node = neighbour_table[j].id;
// send update msgs one at a timeOSMutexPend(mutex_comms_msg_tx, 0, &error);
comms_cmd(SET_TO, quad, &node, 1);comms_cmd(SET_FROM, 0, &self.id, 1);comms_cmd(SET_TYPE, quad, &type, 1);comms_cmd(SET_DATA, quad, neighbour_update_tx_payload, i+1);comms_cmd(SET_TX_POW, 0, &pow, 1);comms_cmd(SEND_ALL, 0, &txmsk, 1);
OSTimeDlyHMSM(0,0,0,300);
OSMutexPost(mutex_comms_msg_tx);
//sprintf_P(msg, PSTR("txid q%d g%d-"), quad, gnd);//pc_send_msg(msg);
}}
}
void pc_print_neighbour_table(){
unsigned char i;
pc_send_msg_P(PSTR("\n\r\n\r*NT*\n\r"));
for (i=0; i<8; i++) {if (neighbour_table[i].id == 0)
break;sprintf_P(msg, PSTR("s: %c\tID: %d\tx: %3.2f\ty: %3.2f\td: %d\n\r"),
neighbour_table[i].status, (int) neighbour_table[i].id, neighbour_table[i].x, neighbour_table[i].y, (int)
neighbour_table[i].hops);pc_send_msg(msg);
}}
void pc_print_bearing_table(){
unsigned char i,j;
pc_send_msg_P(PSTR("\n\r*BT*\n\r"));
for (i=0; i<8; i++) {if (i == 0) {
for (j=0; j<8; j++) {
104
sprintf_P(msg, PSTR("\t%d"), (int) neighbour_table[j].id);pc_send_msg(msg);
}
pc_send_msg_P(PSTR("\n\r"));}
sprintf_P(msg, PSTR("%d\t"), (int) neighbour_table[i].id);pc_send_msg(msg);
for (j=0; j<8; j++) {
sprintf_P(msg, PSTR("%3.2f\t"), bearing_table[i][j]);pc_send_msg(msg);
}
pc_send_msg_P(PSTR("\n\r"));}
}