Diver Positioning System - My FIT

Development and Testing of an Acoustic Based
Underwater Positioning System
By Douglas Palmer Bowlus
Bachelor of Science
Electrical Engineering
Ohio University
2005
A thesis submitted to
Florida Institute of Technology
in partial fulfillment of the requirements
for the degree of
Master of Science
In
Ocean Engineering
Melbourne, Florida
May, 2008
The undersigned committee hereby recommends that the attached document be
accepted as fulfilling in part the requirements for the degree of Master of Science in
Ocean Engineering.
Development and Testing of an Acoustic Based Underwater Positioning
System
by
Douglas Palmer Bowlus
George Maul, Ph.D.
Program Chair, Ocean Engineering
Stephen Wood, Ph.D., P.E.
Assistant Professor, Ocean Engineering – Major Advisor
Lee Harris, Ph.D., P.E.
Associate Professor, Ocean Engineering – Committee Member
Hector Gutierrez, Ph.D., P.E.
Associate Professor, Mechanical Engineering – Outside Committee Member
Abstract
Development and Testing of an Acoustic Based Underwater Positioning
System
Author:
Douglas Palmer Bowlus
Advisor:
Stephen Wood, Ph.D., P.E.
The locating of objects on the seafloor is a common problem among
SCUBA divers today. Above the ocean’s surface, today’s GPS technology allows
for reasonably accurate positioning of a boat, but, unless conditions are near
perfect, divers still have difficulty finding small objects underwater. Conditions
such as poor visibility, strong currents and inaccurate coordinates compound the
difficulty of this task.
The work contained in this thesis develops an aid to help divers navigate to
underwater objects i.e., an “underwater GPS” system. Unlike traditional methods
of underwater location which involve rope and local landmarks, this system uses an
un-tethered acoustic transponder operating at frequencies of 35 kHz and 45 kHz to
reference the diver’s position to absolute GPS coordinates. A surface buoy uses
super-short baseline navigation and positioning technology to link the diver and the
GPS satellites.
iii
This paper discusses the design, construction and testing of the “underwater
GPS” prototype known as “BADNeS” (Buoy Assisted Diver Navigation System).
Many current technologies make up the BADNeS and are described. The design of
the BADNeS, including buoy orientation sensors, acoustic processor, and
transponder along with the results of the prototype testing are presented.
iv
Table of Contents
List of Figures ....................................................................................................... viii
List of Tables ............................................................................................................x
List of Abbreviations ............................................................................................. xi
Acknowledgements ................................................................................................ xii
1
Introduction ......................................................................................................1
1.1
Objectives ...................................................................................................3
1.2
Scope of Work............................................................................................3
1.3
Background ................................................................................................4
1.3.1
Long Baseline (LBL) .........................................................................5
1.3.2
Short Baseline (SBL) .........................................................................7
1.3.3
Super Short Base Line (SSBL) ..........................................................8
1.3.4
Ultra Short Baseline (USBL) ...........................................................10
1.4
Commercially Available Products ...........................................................11
1.4.1
SOSI DiverGPS-30 ..........................................................................11
2
1.4.2
RJE DLS-1 .......................................................................................12
1.4.3
XIOS EyeSea (USBL)......................................................................13
The BADNeS Design ......................................................................................13
2.1
BADNeS System Description ..................................................................14
2.2
Prototype Functionality ............................................................................16
2.3
Potential Applications ..............................................................................16
2.4
System Accuracy......................................................................................17
2.4.1
GPS Accuracy ..................................................................................18
2.4.2
Acoustic Positioning System Accuracy ...........................................19
2.5
Electronics ................................................................................................24
2.5.1
Buoy .................................................................................................24
2.5.2
Diver Carried Transponder ..............................................................41
v
2.6
Microcontroller Software .........................................................................43
2.7
Transducers ..............................................................................................48
2.7.1
Environmental Factors Affecting Transducer Design......................48
2.7.2
Transducer Design Parameters .........................................................53
2.7.3
Transducer Design ...........................................................................54
2.7.4
Array Design ....................................................................................58
3
Construction ...................................................................................................61
3.1
Electronics ................................................................................................61
3.2
Transducers ..............................................................................................65
3.3
Buoy .........................................................................................................70
3.4
Diver Unit ................................................................................................74
4
Calibration ......................................................................................................75
4.1
Offsets ......................................................................................................75
4.2
Tilt ............................................................................................................76
4.3
Heading ....................................................................................................76
4.4
Electronic Delay .......................................................................................80
5
Post Processing ...............................................................................................81
6
System Performance ......................................................................................86
6.1
GPS Receiver ...........................................................................................86
6.1.1
Data Collection.................................................................................87
6.1.2
Data Processing ................................................................................89
6.2
Magnetometer ..........................................................................................91
6.2.1
Data Collection.................................................................................92
6.2.2
Data Processing ................................................................................93
6.3
Tilt Sensor ................................................................................................95
6.3.1
Data Collection.................................................................................96
6.3.2
Data Processing ................................................................................96
6.4
Super Short Base Line..............................................................................99
6.5
Overall System Performance..................................................................100
7
Conclusion.....................................................................................................103
vi
References .............................................................................................................105
Appendix A: Circuit Diagrams ...........................................................................108
Appendix B: PCB Layout ....................................................................................112
Appendix C: PIC Assembly Code.......................................................................122
Appendix D: MATLAB Code .............................................................................217
Appendix E: Supporting Test Documents .........................................................227
vii
List of Figures
Figure 1 – Buoy Assisted Diver Navigation System in Action .................................4
Figure 2 - An Example of a Long Base Line System. ...............................................5
Figure 3 - An Example of a Short Base Line System With 4 Receiving Elements. ..7
Figure 4 - An Example of a Super Short Base Line System ......................................8
Figure 5 – An Example of an Ultra Short Base Line System. .................................10
Figure 6 – SOSI DiverGPS-30 (Sound Ocean Systems, 2006) ...............................11
Figure 7 – RJE DLS-1 (RJE, 2002) .........................................................................12
Figure 8 – XIOS EyeSea (Divernet, 2002) ..............................................................13
Figure 9 - BADNeS System Diagram ......................................................................14
Figure 10 - Range Error Estimate ............................................................................20
Figure 11 - Tilt Error Estimate .................................................................................22
Figure 12 - Heading Error Estimate .........................................................................23
Figure 13 – GPS Circuit ...........................................................................................26
Figure 14 – Magnetometer Circuit ...........................................................................27
Figure 15 – Tilt Sensor Circuit ................................................................................28
Figure 16 – Transducer Driving Circuit ...................................................................30
Figure 17 – First Stage Buffer / Amplifier ...............................................................32
Figure 18 – 8th Order Filter Circuit ..........................................................................33
Figure 19 - 35 kHz Filter Response .........................................................................34
Figure 20 – Post Gain Amplifier Circuit ..................................................................35
Figure 21 – Tone Decoder Circuit ...........................................................................36
Figure 22 – Acoustic Processor Circuitry ................................................................37
Figure 23 – Data Logger Circuit ..............................................................................38
Figure 24 – Bluetooth Circuit ..................................................................................39
Figure 25 – RS232 Circuit .......................................................................................40
Figure 26 - 45 kHz Band Pass Filter Response ........................................................42
Figure 27 - Main Buoy Processor Flow Chart .........................................................46
Figure 28 - Acoustic Processor Flow Chart .............................................................47
Figure 29 - Transponder Flow Chart ........................................................................48
Figure 30 - Background Noise Levels (Waite, 2002) ..............................................50
Figure 31 - Noise Due to Rain (Waite, 2002) ..........................................................51
Figure 32 - Basic Transducer Design (NDT Resource Center, 2008) .....................55
Figure 33 - Single Transducer Array .......................................................................58
Figure 34 - Two Transducer Array ..........................................................................59
Figure 35 - Three Transducer Array ........................................................................60
Figure 36 – Transponder Board With Solder Paste .................................................63
Figure 37 – Transponder Board After Component Placement ................................63
viii
Figure 38 – Finished Transponder Board.................................................................64
Figure 39 - Drill Template .......................................................................................66
Figure 40 - Drilling Screw Hole ..............................................................................66
Figure 41 - Hole for Wire.........................................................................................67
Figure 42 - Soldering Wires to Piezo Element.........................................................67
Figure 43 - Screw in Cap .........................................................................................68
Figure 44 - Piezo Element Glued to Washer............................................................69
Figure 45 - Measuring Piezo Height ........................................................................69
Figure 46 - After the Resin Pour ..............................................................................70
Figure 47 – Buoy With Electronics Box ..................................................................72
Figure 48 – The Acoustic Array Mount ...................................................................73
Figure 49 – Final Constructed Buoy ........................................................................73
Figure 50 - Diver Carried Transponder ....................................................................74
Figure 51 – Offsets Required to Reference Transponder Position to Buoy Position
..........................................................................................................................75
Figure 52 – Example calibration data (PNI Corporation, 2004) ..............................77
Figure 53 – Data after offsets are applied (PNI Corporation, 2004) ........................78
Figure 54 - Calibrated Magnetometer Output ..........................................................79
Figure 55 - Wave Propagation Over the Acoustic Array (Frey, 2003) ....................83
Figure 56 - Tripod with Tribrach Set Over Point 6 ..................................................88
Figure 57 - Tripod with Tribrach Used to Position GPS Receiver Over Point 6 .....88
Figure 58 - Magnetometer Testing Setup.................................................................92
Figure 59 - Sample Heading Data ............................................................................93
Figure 60 - Plot of Heading Error ............................................................................94
Figure 61 - Plot of Standard Deviation ....................................................................95
Figure 62 - Tilt Sensor Testing With CodaOctopus F180 .......................................96
Figure 63 - Sample Tilt Data....................................................................................97
Figure 64 - Mean Tilt Error......................................................................................98
Figure 65 - Tilt Standard Deviation .........................................................................98
ix
List of Tables
Table 1 - Approximate Coefficients for Speed of Sound (Waite, 2002) .................20
Table 2 – GPS Receiver Testing Results .................................................................91
Table 3 - Computed Offsets .....................................................................................92
Table 4 - Overall Heading Statistics ........................................................................95
Table 5 - Overall Tilt Sensor Statistics ....................................................................99
Table 6 - Overall System Performance Estimate ...................................................102
x
List of Abbreviations
ADC
AGC
BADNeS
DSP
FAT
FIT
GPS
HDOP
HDTP
IC
I2C
I/O
LBL
LED
MIPS
NGS
PC
PCB
PPS
PPT
PVC
RMS
RS232
RTK
SBL
SCUBA
SD
SMD
SPI
SPS
SPST
SVP
SSBL
USBL
WAAS
WGS84
Analog to Digital Converter
Auto Gain Control
Buoy Assisted Diver Navigation System
Digital Signal Processor
File Allocation Table
Florida Institute of Technology
Global Positioning System
Horizontal Dilution of Precision
Horizontal Time Dependant Positioning
Integrated Circuit
Inter-Integrated Circuit
Input - Output
Long Base Line
Light Emitting Diode
Millions of Instructions Per Second
National Geodetic Survey
Personal Computer
Printed Circuit Board
Pulse Per Second
Parts Per Thousand
Polyvinyl Chloride
Root Mean Square
Recommended Standard 232
Real Time Kinematic
Short Base Line
Self Contained Underwater Breathing Apparatus
Secure Digital
Surface Mount Device
Serial Peripheral Interface
Standard Positioning Service
Single-Pole Single-Throw
Sound Velocity Profile
Super Short Base Line
Ultra Short Base Line
Wide Area Augmentation System
World Geodetic System 84
xi
Acknowledgements
I would like to thank the following people:
My family for always being there for me. I wouldn’t have made it this far without
them.
My friends for their support, encouragement and fun times. Specifically I’d like to
mention: Ashley Naimaster for always putting up with me and keeping me
informed of the latest gossip. Adam Priest for always putting a smile on my face
and teaching me to surf. John O’Connell for teaching that all the people from
Miami aren’t from South Beach. Maila Sepri for engaging in nerd talk with me and
always having a good technical joke to share. Missy Aycock for encouraging me
and immersing me in the NOLA culture so I could get a glimpse at what I might
otherwise never have seen… The TNA girls for their great cooking, endless
excitement and support. Ruth Sharp for her willingness to listen no matter how
boring I was and her endless supply of bad jokes that sometimes make me laugh.
Evan Hughes for helping me to relax every once in a while by watching movies
that should’ve never been made.
My professors for instilling their knowledge in me. Specifically I’d like to
mention: Tim Bambeck for all the help and advice he has given me over the years.
He has gone beyond his duties as a professor to help me and for that I cannot thank
him enough. Dr. Harris for providing me work at Florida Tech and other great
locations along the coast. Dr. Swain for loaning me his first surfboard so I could
take a break every once in a while. Dr. Wood for his excitement about my project
and his help in getting it finished. Dr. Gutierrez for his input as my outside
committee member.
My co-workers at Fugro for helping me gain a better understanding of all aspects of
my project and making me feel welcome at my new home in California. I’d
specifically like to thank those I work with on a daily basis: Annick Tardif, Chris
Ham, Cindi Knight, Cindy Pratt, Eddie Stutts, Gilbert Suarez, Herb Tovar, Jeff
Carothers, Robin Villa and Trisha Mouton.
ASR Ltd. for giving me an experience and friends I’ll never forget.
Lastly, I’d like to thank Seaworld for all the good times I had there.
xii
1
1 Introduction
A problem facing divers today is knowing their precise position underwater.
Most scuba divers carry compasses, however, compasses only approximate
positions when their bearings references a landmark and the diver estimates the
distance to the landmark. If adverse conditions such as strong currents or low
visibility arise the landmark can easily be lost. The traditional methods of
circumventing this use lines that can either be laid out between landmarks or
attached to a weight in order to swim circular search patterns. Although these
methods work, they require the diver to be tethered and only provide a position
relative to local objects. The work contained in this thesis develops an aid to help
divers navigate to underwater objects i.e., an “underwater GPS” system. This
paper discusses the design, construction and testing of the “underwater GPS”
prototype known as “BADNeS” (Buoy Assisted Diver Navigation System).
Several solutions to the problem of underwater positioning have already
been designed and are discussed in detail in section 1.3: Long Base Line (LBL),
Short Base Line (SBL), Super Short Base Line (SSBL) and Ultra Short Base Line
(USBL). Many of these solutions use sound coupled with computer processing to
determine position though the scale of each solution is widely varies and has its
own pros and cons.
2
While these solutions may all work, they are not easily adapted into a small
unit that a diver can carry. For example, Long Base Line requires a network of
equipment spanning vast distances to accurately determine the position an
underwater object. This is not practical for diving given that it requires a large
amount of time deploying equipment for each location. Short Base Line requires
that hydrophones be placed at different points on a boat, which is also not very
conducive to recreational diving. Super Short Base Line (SSBL) and Ultra Short
Base Line (USBL) are the two acoustic positioning techniques available with a
small footprint. These require only a small array of hydrophones, and reference
objects with respect to the position of the array. These features make SSBL and
USBL very suitable for a recreational diver’s positioning system. The small array
size allows for easy deployment while the reference method makes transformation
to an absolute positioning system such as the Global Positioning System (GPS)
straightforward.
The technology required for an underwater positioning system already
exists, however, the cost of off the shelf system components is prohibitively high
for personal use. For example, LinkQuest Inc. sells their lowest grade USBL
navigation system starting at $15,000 USD (TrackLink Models, 2005). This price
does not include the GPS, motion sensors and laptop required for a fully
functioning positioning system. In addition to the high cost, the system portability
is less than ideal as each component is housed separately and must be reassembled
3
in a known orientation each time it is to be used. By building all the required
components into a single unit, the portability and cost of an underwater positioning
could become feasible for personal use.
1.1 Objectives
The objective of this project was to develop a portable, low-cost prototype
system capable of providing a diver’s absolute position to a fair degree of accuracy.
The prototype system is known as the “BADNES” (Buoy Assissted Diver
Navigation System). The portability of the BADNES was to be such that it can be
carried by one person and fit on a standard dive boat. The total cost of system
components was to be $1,500 or less for the proto-type unit making the cost of a
mass-produced unit substantially less. The system was desired to have a working
range of 100m (diver to buoy) and an overall horizontal positioning accuracy better
than 10 meters Root Mean Square (RMS).
1.2 Scope of Work
The scope of the project included designing, building, and testing the
conceived proto-type underwater positioning system as depicted in Figure 1. A
board level design was devised based on GPS and acoustic positioning technology.
The proto-type unit was then constructed. Finally, the system functionality was
determined empirically through testing of the proto-type unit.
4
Figure 1 – Buoy Assisted Diver Navigation System in Action
1.3 Background
As mentioned earlier there are currently four popular techniques utilized to
position objects underwater via acoustics: Long Base Line, Short Base Line, Ultra
Short Base Line and Super Short Base Line. These technologies are similar in that
they use the travel time of sound in water, but differ in their spatial requirements
and accuracies.
5
1.3.1 Long Baseline (LBL)
Figure 2 - An Example of a Long Base Line System.
In a long baseline system as seen in Figure 2, three (3) or more beacons are
placed in known locations and used to compute the diver’s position. The beacons
can be mounted in fixed locations or placed on floating buoys which encode their
GPS coordinates in the beacon’s signal. The time it takes sound to travel from the
beacons to the diver is measured. Using the travel time, the range of the diver from
each beacon is computed by assuming a fixed speed of sound in water. By drawing
a circle centered at each beacon with a radius equal to the range to the diver, the
diver’s position is given by the intersection of the three circles. This is essentially
the same principle that GPS (Global Positioning System) uses.
6
The system is termed “long” baseline because the distance between the
beacons for this technique are large and encompass the entire region where
positioning is to be done. Long baselines have more accuracy than other
techniques due to the larger distances and hence longer signal travel times. A
typical accuracy for a long baseline system after calibration would be one (1) meter
(Lurton, 2002). This accuracy can be improved in more advanced systems which
utilize methods such as coded wide band signals (Austin, 1994).
Major disadvantages of the long baseline technique are time and money.
Larger, more expensive transmitters are required to cover the large distances
necessary for accurate positioning. In addition, each transmitter must be set in
location and their coordinates determined before a target position can be found.
For these reasons long baseline is economical for jobs of short duration or noncommercial use.
7
1.3.2 Short Baseline (SBL)
Figure 3 - An Example of a Short Base Line System With 4 Receiving Elements.
Short baseline systems use multiple receiving elements to determine the
distance and bearing of a single transmitter. The bearing is computed by
comparing the difference in arrival time of the transmitter’s signal between
receiving elements. For this to work, the orientation of the receiving elements must
be known. The range is then determined by applying an estimate of the speed of
sound in water to the roundtrip travel time of the signal.
8
Unlike a LBL system, the reference frame is not necessarily fixed. In the
system illustrated in Figure 3, the receiving elements are mounted at different
positions on the bottom of a ship. After a transmitter’s signal is received, the SBL
system will determine a distance and a bearing to the transmitter. Since the SBL
system is attached to the ship, any movement of the ship must be taken into account
to compute the exact position of the transmitter.
A major disadvantage of a SBL system is its setup. The receiving elements
must be mounted and surveyed on each vessel so their orientation relative to each
other is known. The extra time spent mobilizing and demobilizing the SBL system
may not be worth it for a casual diver.
1.3.3 Super Short Base Line (SSBL)
Figure 4 - An Example of a Super Short Base Line System
9
Super short base line (see Figure 4) is the same as short base line except that
the receiving elements are mounted closer together. The difference in arrival time
between the elements is still used to compute the bearing as in SBL, however, these
arrival time differences are much smaller.
The advantages of a SSBL system over a SBL system include easier setup
and smaller space requirement. Instead of mounting and surveying multiple
receiving elements, the elements can be fixed to a small frame. Fixing all the
elements to a frame makes it easy to change vessels since only one point on the
frame needs to be known in reference to the vessel along with the heading of the
frame.
Although SSBL has several advantages, the accuracy is slightly
compromised. Due to the fact that the receiving elements are closer together than
in SBL systems, the resolution of the time of arrival measurements becomes very
important and has a large impact on system accuracy.
10
1.3.4 Ultra Short Baseline (USBL)
Figure 5 – An Example of an Ultra Short Base Line System.
Similar to a SBL system, an ultra short baseline system (shown in Figure 5)
has multiple receiving elements. The difference is that the elements in an USBL
system are contained in one housing with very close spacing. The spacing of each
element is within a wavelength of the others. This allows the phase differences of
the incoming signals between the different receiving elements to be used in order to
find the bearing (as opposed to the arrival time). The range is found the same as
with SBL.
As with SSBL, the main disadvantage of USBL is that of accuracy. A
typical USBL system has positioning accuracies of approximately 10 meters
11
(Lurton, 2002). This accuracy can be improved through the use of advanced
techniques which utilize coded, wide band signals instead of conventional tone
bursts (Austin, 1994).
1.4 Commercially Available Products
A large number of commercially available underwater navigation units were
researched and reviewed. Many similar products to those listed exist but were not
listed to avoid redundancy. Also note that LBL commercial products were omitted
due to their high cost and long mobilization times.
1.4.1 SOSI DiverGPS-30
Figure 6 – SOSI DiverGPS-30 (Sound Ocean Systems, 2006)
The SOSI DiverGPS-30 (shown in Figure 6) is essentially a waterproofed
GPS unit whose antenna is attached to a buoy with a cable.
12
Manufacturer: Sound Ocean Systems, Inc.
http://www.soundocean.com/
Price:
Estimated < $1000 USD
Pros:
-Simple design
-Full featured mapping
-Portability
Cons:
-Tethered antenna
-Positioning is based on antenna
location not diver location
1.4.2 RJE DLS-1
Figure 7 – RJE DLS-1 (RJE, 2002)
The RJE DLS-1 (shown in Figure 7) is used to find an underwater
transmitter. Range and bearing are found by listening for a change in tone while
pointing the device and turning in a circle. While absolute bearing could be
obtained by a diver using a compass, this unit doesn’t lend itself to an automated
diver location system.
Manufacturer: RJE International
http://www.rjeint.com/
Price:
Estimated $5000 USD
Pros:
-Portability
Cons:
-No direct range and bearing information
-No absolute positioning information
13
1.4.3 XIOS EyeSea (USBL)
Figure 8 – XIOS EyeSea (Divernet, 2002)
After attaching a transmitter to an object underwater, the wrist worn
receiver (shown in Figure 8) gives the diver distance and bearing information to the
transmitter.
Manufacturer: XIOS
Price:
$700 USD
Pros:
-Portability
-Provides range and bearing information
Cons:
-No longer available
-No GPS integration
2 The BADNeS Design
The design of the “BADNeS” (Buoy Assisted Diver Navigation System)
prototype incorporated the good qualities of the systems described above. The
functionality, accuracy, hardware and software considerations pertaining to the
design of the BADNeS are described in this section.
14
2.1 BADNeS System Description
Figure 9 - BADNeS System Diagram
By combining the technologies used in the above mentioned commercially
available products, a better diver positioning system was designed. More
specifically, a range and bearing system similar to the XIOS EyeSea unit was
combined with the absolute positioning power of a floating GPS system resembling
the SOSI DiverGPS-30.
The BADNeS positioning system acts as an underwater GPS and consists of
two parts, a buoy and a waterproof transponder (see Figure 9). The buoy holds a
standard GPS unit, orientation sensors (tilt and heading), an acoustic array and a
15
processing unit. The waterproof transponder responds to signals sent from the
surface unit and is carried with the diver or object to be located.
The BADNeS operates in the following basic manner. First, the absolute
position of the buoy is determined via the GPS unit. The acoustic array is then
used to find the transponder’s distance and bearing from the buoy. This is
accomplished by transmitting a signal from the buoy to the transponder which then
responds with a similar signal. Using the roundtrip travel time and an estimate of
the speed of sound in water, the range is determined. The acoustic array also uses
the signal from the transponder to determine the azimuth and elevation bearings of
the transponder relative to the buoy with the super-short baseline technique. The
range and bearing estimate of transponder position is then referenced to the GPS
position using the orientation sensors. The heading sensor is used to reference the
bearing of the transponder to north. The tilt sensors are used to remove any effects
of the buoy’s tilt on the transponder’s position. This process is repeated to
constantly update the transponder’s position.
More functionality can be added to the BADNeS prototype in the future by
incorporating a few extra components. The incorporation of an acoustic modem
into the buoy and transponder enables many possibilities. By adding a depth sensor
to the transponder, the acoustic modems will enable the transponder to send a depth
measurement to the surface unit. The depth measurement can be used to improve
the accuracy of the acoustic positioning calculations. The addition of the modem
16
would also allow the surface unit to send the latest position estimate to the diver
unit for display on a screen if real-time processing were done on the buoy.
2.2 Prototype Functionality
The purpose of the prototype unit was to determine the feasibility of a diver
positioning system as described above and if possible determine the accuracy and
precision of the various components. The prototype design contains all the sensing
capabilities of the system described above: GPS, heading and tilt orientation
sensors, and a super-short baseline acoustic positioning system. The prototype logs
data from the sensors and requires post-processing performed on a PC to determine
the diver position. Real-time processing wasn’t implemented on the prototype to
allow for a quicker design time and more flexibility in the data processing.
2.3 Potential Applications
There are many immediate applications for which the BADNeS could be
used. The ability for a diver to know his absolute position underwater allows for
uses such as:
•
Marking the position of an underwater instrument / artifact / hazard (very
useful in underwater monitoring applications such as for artificial reefs)
•
Finding an underwater instrument / artifact / hazard
17
•
Following a course underwater (this would be very useful in search and
rescue operations)
•
Finding your way to an exit in ice covered areas
•
Avoiding potential hazards
•
Underwater archeology
If BADNeS units were then networked together with real time data transfer,
diver and object positions could be shared instantly. This would provide for even
more applications such as:
•
Locating a lost dive buddy
•
Locating a surface vessel
•
Performing large coordinated searches
•
Alerting others of potential hazards
•
Sharing sightings of artifacts and exotic sea creatures
•
Coordinating underwater efforts such as coral transplantation
2.4 System Accuracy
The performance of the GPS and acoustic positioning system determine the
accuracy of the BADNeS. The accuracy of GPS and the components comprising
the acoustic positioning system are detailed in this section.
18
2.4.1 GPS Accuracy
The positioning accuracy of raw GPS measurements is approximately 10-20
meters (Lekkerkerk, 2007, Vol. 1); this accuracy can be increased by using
Differential GPS (DGPS). Standard DGPS works by setting up a GPS receiver at a
known location to act as a reference station. The reference station measures pseudo
ranges to the satellites while at the same time calculating theoretical pseudo ranges
using its known location. By comparing the theoretical ranges to the measured
ranges the reference station is able to compute a number of corrections. In a DGPS
system, these corrections are transmitted to other nearby GPS receivers allowing
them to calculate their position to within 2 meters (Lekkerkerk, 2007, Vol. 1).
More complicated DGPS systems exist, namely Real Time Kinematic (RTK),
which can provide centimeter level accuracy (Lekkerkerk, 2007, Vol. 1), however,
these systems are prohibitively expensive for the BADNeS.
The Wide Area Augmentation System (WAAS) is a type of DGPS system
comprised of a number of ground based reference stations and a number of
geostationary satellites. In this particular system, the corrections are sent out using
the geostationary satellites allowing corrections to be received by any GPS
receivers within view of one of the satellites. The accuracy of a standard off the
shelf GPS unit is 5m with WAAS enabled (406 GPS Receiver Engine Board,
2006).
19
It is not feasible for a recreational diver to own and setup a base station,
however, many DGPS reference networks, like WAAS, exist that can be used.
Depending on the company running the reference network, special equipment may
be needed, a service fee may apply and the service area may be limited. The
prototype unit has the capability to accept DGPS corrections, though none other
than WAAS were used as it required no additional hardware and is freely available
in the United States of America.
2.4.2 Acoustic Positioning System Accuracy
The accuracy of the acoustic positioning systems is affected by many error
sources. Error sources include: sound velocity, tilt angle, heading, sampling and
false detections. The sources of these errors as well as their consequences are
described below in Sections 2.3.2.1 - 2.3.2.5.
2.4.2.1
Sound Velocity Error
Sound velocity in water is determined by temperature, salinity and depth.
The relative effect of each factor can be seen in Table 1. Given that these factors
vary from one place to another in the ocean, the speed of sound also varies. It is
impossible to know the speed of sound at every point in the ocean so an assumption
is made. In practice the speed of sound is typically assumed to be constant in all
locations or varying only with depth. The latter case makes use of a sound velocity
profile (SVP), a plot of sound speed versus depth.
20
Table 1 - Approximate Coefficients for Speed of Sound (Waite, 2002)
Temperature +3.4 m/s per °C
Salinity
+1.2 m/s per ppt
Depth
+1.7 m/s per 100m
The assumption about sound velocity leads directly to errors in the acoustic
positioning system. Range estimate error is error caused by using the assumed
sound velocity to turn the round distance travel time of sound, from the buoy to the
transponder and back, into a distance. The amount of this error is:
Range Estimate Error =
TRT
× (c r − c a )
2
[1]
Where:
Range Estimate Error is given in meters
TRT = Round trip travel time in seconds
cr
ca
= Real average sound velocity in meters per second
= Assumed average sound velocity in meters per second
Figure 10 - Range Error Estimate
21
As can be seen from Figure 10, range error at 100 meters becomes significant if a
proper sound velocity is not assumed. Refraction error is the other error source due
to an assumed sound velocity though its affects can be reduced if an SVP is
available. Refraction is the bending of sound waves due to changes in the speed of
sound as the waves propagate. If speed of sound is relatively constant in a location
then refraction will not be a problem, however, if a thermocline or halocline is
present refraction can become a significant error source. A SVP can be used to
calculated and remove the effects of refraction from the calculated position
estimate.
2.4.2.2
Tilt Angle Error
Tilt error is the difference between the actual tilt of the SSBL array and the
measurement returned by the tilt sensor. Since any position determined by the
acoustic positioning system is in reference to the acoustic array, the tilt
measurement must be used in order to compute the position in an absolute
coordinate system. Tilt error is introduced by both roll and pitch measurements.
The effect tilt error has on the position estimate increases with range so that at large
ranges it becomes a major source of error (see Figure 11).
22
Figure 11 - Tilt Error Estimate
2.4.2.3
Heading Error
Heading error is the difference between the SSBL array’s actual heading
and that derived from the magnetometer readings. The heading, like tilt, is used to
reference the position of the transponder determined by the SSBL to an absolute
coordinate system such as WGS84, the geodetic datum used by GPS. The effect of
heading error on the position estimate is dependant on the transponder’s range from
the buoy and the elevation bearing as shown in Figure 12.
23
Figure 12 - Heading Error Estimate
2.4.2.4
Sampling Error
Sampling error affects both the range and bearing estimates. This is
because the range and bearing estimates are determined by detecting time
differences between several transducers. The processor samples the signal received
from the transducer a finite number of times per second. The gap in time between
the transducer receiving a signal and the processor sampling this signal is sampling
error. The higher the sampling rate, the lower the sampling error will be. The
sampling rate must be chosen such that the effect it has on the range and bearings is
within the desired tolerance.
24
2.4.2.5
False Detection
Signals from sources other than the transponder can be falsely detected as
the response pulse from the transducer. This occurs when a sound source of the
right frequency is loud enough to be detected and arrives in the interval between the
transmission of the interrogation pulse and the reception of the response pulse.
Unless there is a constant source of noise at the reception frequency, false detection
error is unlikely. If false detection does occur, it is likely the position reported will
be far enough from other measurements to warrant suspicion.
2.5 Electronics
This section describes how the electronics designed for this project came to
be. This includes information on component selection and circuit design for the
buoy and diver carried transponder.
2.5.1 Buoy
The buoy electronics were designed to accomplish five tasks: determine the
position and orientation of the buoy, determine the position of the diver relative to
the buoy, save any data collected and provide an interface for the user.
25
2.5.1.1
Determining Position of the Buoy
The position of the buoy was determined utilizing GPS. A variety of GPS
modules were available to choose from; two of them were incorporated in the
design of the buoy electronics.
Originally the EM-406 by the GlobalSat Technology Corporation was to be
used and was tested on a prototype board. The EM-406 was selected from
Sparkfun Electronics’ available devices for two reasons. At the time it had the
SiRF III chipset, the newest GPS chipset of the available modules (406 GPS
Receiver Engine Board, 2006). It was also the only module with this chipset that
came with a built-in antenna. This rid of the need to match an antenna to the GPS
module and therefore simplified the design.
The second GPS module which can be connected to the main circuit board
is the EB-85A by Etek Navigation, Inc. It was introduced after the EM-406 was
prototyped but before the main circuit board design was finished. While the EM406 specifications list 10 meter 2D RMS error (no WAAS), the EB-85A datasheet
claims 3.3 meter CEP error (un-aided). 2D RMS error is approximately 1.2 times
the CEP error (Misra, 2004) which means the EB-85A has approximately 4 meter
2D RMS error. The EB-85A also features a Pulse Per Second (PPS) output which
allows for precise timekeeping and an update rate of up to 5 Hz. The EM-406 has
no PPS output on its connector and only updates at a maximum rate of 1 Hz. The
26
circuitry for both GPS units was included on the final circuit board but the EB-85A
was used because of the PPS output and lower 2D RMS error.
The circuitry required for the GPS modules (shown in Figure 13) is
relatively basic. The 5 Volt power going to each GPS module is controlled via an
Analog Devices ADG801 digitally controlled SPST switch. This provides the
ability to save power by turning off the GPS module when not in use. The GPS
modules communicate via RS-232 which requires a two-wire interface to the host
microcontroller. The 1PPS output of the EB-85A is connected to an interrupt line
on the host microcontroller and the battery backup line is connected via a jumper to
a 3V lithium coin cell battery.
Figure 13 – GPS Circuit
27
2.5.1.2
Determining the Orientation of the Buoy
The orientation of the buoy consists of its heading, pitch, and roll. The
heading can be determined by measuring the strength of the Earth’s magnetic field
while the pitch and roll can be measured using the Earth’s gravitational field.
A 3-axis magnetometer (MicroMag3) measures the strength of earth’s
magnetic field to determine the buoy’s heading. This sensor provides a high
resolution magnetic field measurement in three axes. The MicroMag3 interfaces
with a microcontroller via an SPI interface and only requires one external
decoupling capacitor (see Figure 14). This device is capable of measuring ± 1100
μT with a resolution of 0.015 μT (PNI Corporation, 2006). This range is sufficient
since the Earth’s magnetic field strength ranges from approximately 20 μT to 65 μT
(National GeoSpatial-Intelligence Agency, 2005).
Figure 14 – Magnetometer Circuit
28
An Analog Devices ADIS16201 dual-axis inclinometer measures the
buoy’s tilt. This sensor uses Earth’s gravitational pull to determine the angle of the
sensor in two dimensions with an accuracy of ± 0.25 degrees for small tilt angles
(Analog Devices, 2006). The ADIS16201 outputs tilt and acceleration data via SPI
when instructed by the host processor. An onboard filter can be adjusted to
decrease the effect of buoy accelerations on the tilt measurement. The sensor can’t
differentiate between static acceleration due to gravity and dynamic acceleration
due to other forces, therefore, a low pass filter is used to minimize this effect. The
ADIS16201 incorporates its own decoupling capacitors so no external components
were required (as shown in Figure 15).
Figure 15 – Tilt Sensor Circuit
2.5.1.3
Determining the Diver’s Relative Position
In order to ascertain the diver’s position relative to the buoy, the super short
baseline technique is used. This technique requires that an interrogation signal is
29
sent to the transponder and the time of arrival of the response pulse is recorded for
each transducer in the receiving array.
The transducer driver sends the interrogation pulse using an Allegro A3950
full bridge motor driver. The A3950 provides a simple method of providing a
maximum of 2.8 amps at up to 36 volts to the transducer as well as ability to
reverse current direction quickly. By reversing current direction, as opposed to
turning the current to the transducer on and off, the effective voltage delivered to
the transducer is doubled. The transducer has high input impedance, therefore,
every bit of voltage helps to deliver a suitable amount of acoustic energy through
the water.
A basic 3.3 volt digital interface is required to control the A3950 full bridge
motor driver. An enable line turns the current to the transducer on when driven
high. A phase line controls the current direction. The interrogation transmit pulse
is generated by toggling the phase line at 45kHz, the interrogation pulse frequency,
while setting the enable line high for the desired pulse length.
The typical application circuit from the A3950 datasheet was used as it
detailed all the required external components. These components provide for
digital line pull-ups, voltage regulation and charge pump capacitance as seen in
Figure 16.
30
Figure 16 – Transducer Driving Circuit
A series of circuits is required to measure the time of arrival of the response
pulse from the transducer array. Since the output of a transducer is high impedance
and low voltage, the first circuit needed is an amplifying buffer circuit. Next a
filter is used to reduce noise and pass only the response pulse frequency. After the
filter, another amplifier with programmable gain is used to boost the voltage to a
suitable level for the next circuit, the tone decoder. The tone decoder circuit detects
a single frequency and sets a digital output alerting the microcontroller if a tone is
detected. The microcontroller then captures the time at which the response pulse is
received.
31
The amplifying buffer circuit incorporates an ultra low-noise Analog
Devices AD745 op-amp as well as a Maxim 5436 digital potentiometer for
programmable gain. The recommended hydrophone circuit given in the AD745
datasheet was used. This circuit provides balanced inputs to the op-amp, an
extremely high input impedance and low output impedance. The Maxim 5436
digital potentiometer is used to vary the feedback current from the output of the opamp to the input. This effectively controls the gain of the first stage amplifier. The
5436 digital potentiometer has a range of approximately 2-52kΏ. The gain for the
first stage amplifier is given by Equation 2.
Gain = Feedback Resistance / R63
[2]
With resistor R63 (shown in Figure 17) set to 100Ώ, this gives a range of
amplification of approximately 20-520 in steps of 40. The step size is due to the
128 finite values the digital potentiometer can take on. This value is set by the
microcontroller via an SPI interface.
32
Figure 17 – First Stage Buffer / Amplifier
A Maxim MAX274 active filter IC is used to implement an 8th order band
pass filter. This IC was chosen because of its ability to implement high order filters
with different center frequencies and bandwidths. Using a software program,
released by Maxim, it’s just a matter of selecting the filter response desired and the
software calculates the 16 external resistors required (see Figure 18). Since this
same circuit was to be used in the diver transponder which uses a different
frequency, this was a major plus. Size was another reason this IC was chosen. It
33
comes in a small surface mount package and even with all the external resistors, it
doesn’t take up much board space. By design the filter has a bandwidth of
approximately 1 kHz and a center frequency of 35 kHz as shown in Figure 19.
This attenuates the 45 kHz interrogation pulse while allowing the 35 kHz response
pulse through without much loss.
Figure 18 – 8th Order Filter Circuit
34
Figure 19 - 35 kHz Filter Response
As with the front-end amplifier circuit, an op-amp and digital potentiometer
is used for the post gain amplifier (see Figure 20). A Maxim Max412 op-amp was
used for the post-amp. This op-amp is low-noise and contains two op-amps. One
op-amp is used for the amplifier, the other for a voltage converter which is
discussed later. The same 5436 digital potentiometer is used in a manner identical
to the first stage amplifier. This circuit also has the same gain capabilities given
that resistor R7 is the same value as R63 from previous.
35
Figure 20 – Post Gain Amplifier Circuit
A LM567 IC is utilized to perform the tone decoding. This chip accepts an
AC input signal and connects the output terminal to ground when a tone of the set
frequency is detected. The typical application circuit from the datasheet was used
with only a few value changes to set the detection frequency and bandwidth (see
Figure 21). R134 is a manually adjusted potentiometer that is used to fine tune the
peak detected frequency. C76 sets the bandwidth. The value of C76 was chosen
such that the bandwidth of detection was set to approximately 10% of the center
36
frequency. This results in a bandwidth of approximately 3.5 kHz. An LED was
added to provide a visual indication when valid signals are received.
Figure 21 – Tone Decoder Circuit
Lastly, the microcontroller is used to detect the differences in the time of
arrival of the response pulse from the 3 transducers in the array. The
microcontroller used for this application is a Microchip dsPIC 33FJ128GP306
(shown in Figure 22). This microcontroller was chosen for its speed (up top 40
MIPS), abundance of on-board peripherals and digital signal processing capability.
The fast instruction cycle time, 25ns, provides the necessary time resolution needed
to discern small changes in the diver’s position. The on-board peripherals such as
SPI and ADCs allow for communication with other circuit boards and the ability to
add Auto Gain Control (AGC) respectively. Being that this microcontroller is in
37
fact a Digital Signal Processor (DSP) as well, it has the capability to provide signal
filtering and data processing which will be useful in future revisions of this device.
Figure 22 – Acoustic Processor Circuitry
2.5.1.4
Saving the Captured Data
In order to save data collected from various sensors for post processing, a
Secure Digital (SD) card data logger was used. The Logomatic V1.0 from Spark
Fun Electronics was chosen for its ease of integration, FAT16 file format and low
cost. Data is sent to the Logomatic via an asynchronous serial interface similar to
38
that used by a computer’s serial port. The data sent is stored in a computer
readable text file which can be downloaded to a PC via a card reader.
The schematic for the Logomatic (shown in Figure 23) has a few
mentionable devices in it. The ADG801 is an electronic switch that allows the
microcontroller to switch power to the Logomatic on and off. This can be used to
save power when the Logomatic is not in use. Two transistors are also used to
electronically activate the pushbuttons on the Logomatic. One button stops the
logging and closes the created data file. The other pushbutton resets the
Logomatic. This is used to begin logging data after it has previously been stopped.
Figure 23 – Data Logger Circuit
2.5.1.5
User Interface
Three methods of user interface were designed into the buoy electronics.
These include Bluetooth, RS-232 and I2C. A Bluetooth connection provides the
39
most efficient interface because it is fast and wireless. The Bluetooth interface
allows for a quick method of obtaining and setting various system parameters
without opening the casing of the buoy electronics. The RS-232 interface is the
same as the Bluetooth interface except that it requires a 9-pin serial cable between
the buoy electronics and the computer. The I2C interface was designed to connect
to a separate user interface circuit board. This board would be fitted with an LCD,
a few LEDs and some pushbuttons. This interface would require the user to be at
the buoy and would not be as efficient as the other two interfaces, however, no
computer would be required.
The design of the Bluetooth interface incorporates a module purchased from
Spark Fun Electronics. An LED was added to visually indicate an active Bluetooth
connection. A pull-up resistor was used on the reset line while a pull-down resistor
was used on PIO4 (as shown in Figure 24) as per the datasheet.
Figure 24 – Bluetooth Circuit
40
The RS-232 interface (shown in Figure 25) uses a standard 9-pin connector
and a MAX3233E. The MAX3233E IC translates the voltage levels between the
microcontroller and the computer. The resistors in the circuit limit the amount of
current in the event of a short circuit. Several jumpers are also installed in the RS232 circuit. These are used to direct communication with the computer between the
microcontroller and the GPS unit. These jumpers are generally installed to allow
communication with the microcontroller but can be switched to allow for
differential corrections to be sent to the GPS.
Figure 25 – RS232 Circuit
Two ADG804 multiplexers are used to route serial data transmissions to
and from the microcontroller. These are necessary since the microcontroller only
provides two serial interface ports. One of the two ports was dedicated to GPS
communications, leaving one available port to be shared by the data logger,
Bluetooth communication and RS-232 communication. Since only the RS-232 or
41
Bluetooth communications are used at any single time, the RS-232 communication
is connected unless an active Bluetooth connection exists. The data logger
connects only when the microcontroller is sending data for logging, resulting in no
loss of function with minimal hardware and software.
2.5.2 Diver Carried Transponder
Transponder electronics were designed to accomplish two tasks:
determining when a signal from the buoy is received and sending out a response
pulse. The circuits used to accomplish these tasks are almost mirror copies of those
from the buoy electronics with a few exceptions.
2.5.2.1
Detecting a Interrogation Pulse
The interrogation pulse detection circuitry varies slightly from the response
pulse detection circuitry described in Section 2.4.1.3. The primary difference is
frequency. The interrogation pulse has a frequency of 45 kHz as opposed to the 35
kHz response pulse. Detection of the interrogation pulse requires that the band
pass filter’s center frequency as well as the tone decoder’s detection frequency be
45 kHz. Both of these changes were implemented with simple component value
changes which were calculated with formulas provided by the component’s
datasheet. The response of the band pass filter is shown in Figure 26. The
microcontroller used on the transponder is a dsPIC33FJ12GP202. This
42
microcontroller is in the same family as the one used on the buoy electronics,
however, it is cheaper as it has fewer features and I/O pins.
Figure 26 - 45 kHz Band Pass Filter Response
2.5.2.2
Sending a Response Pulse
The transmit circuitry for the response pulse generator is essentially the
same as the interrogation pulse generator, however, the frequency of the response
pulse is 35 kHz. This frequency is controlled by a software delay on the
microcontroller and required no hardware change although a cheaper
microcontroller is used. This is the same microcontroller used to detect the
interrogation pulse.
43
2.6 Microcontroller Software
The BADNeS is comprised of three separate circuit boards, each containing
its own microcontroller. These microcontrollers do their part to determine the diver
position by running individual software. The main processor and the acoustic
board processor are located on the surface buoy while the transponder processor is
in the handheld diver unit.
The purpose of the main processor is to control all the sensors and store the
data. As shown in Figure 27, the microcontroller waits for the PPS pulse output
from the GPS before collecting data from the sensors. This provides a consistent
time base for the measurements and also ensures that data is only collected when
the GPS has positioning capability. After receiving the PPS pulse, the
microcontroller communicates with each sensor via several standard
communication protocols such as RS232, I2C and SPI. The sensors are instructed
to collect a measurement and when the data is ready the measurements are read into
the microcontroller. A time-stamp is collected prior to each measurement so that
latency can be examined. After all sensor measurements have been collected the
data is stored on an SD card for post-processing. This cycle starts again upon
reception of the next PPS pulse.
The acoustic processor handles the underwater acoustic tasks of the surface
buoy unit. The flow chart representation of the acoustic processor software is
44
shown in Figure 28. After initialization, the acoustic processor sits idle until it
receives a command from the main processor. The flow chart shows two such
commands. The “set parameter” command allows the user to change calibration
values. The “get measurement command” is used to obtain the range and bearing
of the diver unit. When this command is issued, the acoustic processor sends out
an interrogation pulse and starts a timer. If no response pulse is heard within the
specified “big timeout” period then the processor will return an error condition to
the main board and await another command. The timeout detection prevents the
processor from locking up when no transponder is within range. If a response
signal is received at any one of the transducers then a “small timeout” is set. The
“small timeout” setting ensures that the processor will not lock up when only one
or two of the transducers detect the response pulse. As with the “big timeout”, an
error is sent to the main processor and the acoustic processor await another
command. If all the transducers receive the response pulse, the data collected is
sent to the main processor. The acoustic processor then awaits another command.
The processor in the diver carried transponder carries out a basic function as
diagrammed in Figure 29. Upon reception of an interrogation pulse, the
transponder responds with a response pulse and then waits for the next
interrogation pulse.
The software was written and programmed into the microcontrollers using
the manufacturer’s development tools. The dsPIC series of microcontrollers made
45
by Microchip Technology Inc. were used as mentioned in Section 2.6. The software
was programmed in assembly language, compiled and uploaded using the MPLAB
IDE v7.60 development suite in conjunction with the ICD2 development tool.
MPLAB IDE was available for free from Microchip’s website
(http://www.microchip.com), while the ICD2 was purchased from Microchip.
46
Main Buoy
Processor
Initialize
Everything
No
New PPS Received?
Yes
Time Stamp
AP_STIME
Send Get
Measurement
Command to
Acoustic
Processor (AP)
Time Stamp
MAG_TIME
Collect
Magnetometer
Reading
Time Stamp
TILT_TIME
Collect Tilt
Readings
Read AP
Measurement
When Ready
Time Stamp
AP_ETIME
No
User Request Data
Collection?
Yes
Store Readings on
SD Memory Card
Figure 27 - Main Buoy Processor Flow Chart
47
Acoustic
Processor
Initialize
Everything
Has a command
been received?
Yes
No
Get Measurement
Command?
No
Set Parameters
Command?
Yes
Setup Big Timeout
Set the received
parameters
Send Interrogation
Pulse
No
Yes
Has Response
Pulse Arrived
Yet?
Has Big
Timeout
Occurred?
Set Small
Timeout
Have All
Transducers
Received
Response?
No
Yes
No
Yes
Has Small
Timeout
Occurred?
Yes
Send Partial Signal
Return Condition to
Main Board
Send Data back to
Main Board
Perform No Signal
Return to Main
Board
Figure 28 - Acoustic Processor Flow Chart
No
48
Transponder
Processor
Initialize
Everything
Has an Interrogation
Pulse Been Received?
No
Yes
Send a Response
Pulse
Figure 29 - Transponder Flow Chart
2.7 Transducers
An important part of the SSBL system is the acoustic array which is formed
by multiple transducers. Several factors influenced the design of the transducers
and the acoustic array. These factors are discussed as well as the procedures used
to build the array.
2.7.1 Environmental Factors Affecting Transducer Design
Several environmental factors affect the transmission of sound through
water. The factors which were found to be pertinent at the operating frequency
49
range of 35 – 45 kHz were examined prior to designing the transducer and are
discussed in this section.
2.7.1.1
Background Noise
Several factors in the ocean environment contribute to background noise
which must be taken into consideration for the underwater acoustic system design.
For a frequency range of 35 – 45 kHz, the following noise sources are relevant:
surface noise, noise from rain and noise from marine mammals.
Surface noise is noise induced by wave action at the surface. Figure 30
shows a plot of surface noise for various sea states plotted versus frequency.
Assuming recreational diving would not be common in sea states greater than 4
(approximately 1.2 meter waves), then the surface noise will be 42 dB (Waite,
2002) maximum.
50
Figure 30 - Background Noise Levels (Waite, 2002)
Rain, although intermittent, can dominate as the lead source of noise given
heavy rain conditions. Figure 31 shows a plot of rain induced noise versus
frequency for several different rainfall rates. At a rain range of 30 to 100 mm/hr,
the rain induced noise level is approximately 60 dB. When rain induced noise is
combined with surface noise using incoherent addition, the overall noise level is
still approximately 60 dB. Thus, when a heavy rain occurs, the background noise
will be at its greatest, 18 dB greater than when it is not raining.
51
Figure 31 - Noise Due to Rain (Waite, 2002)
The last source of background noise is echolocation from marine mammals
such as dolphins. Typical echolocation frequencies range between 50 – 200 kHz.
Although these sounds are outside the range of interest, echolocation clicks can
reach levels as high as 200 dB (Lurton, 2002). At these high levels, there is a
chance that some lower frequency energy will affect the super short base line
measurements. For this project it will be assumed that marine mammals are not
contributing to the background noise affecting the BADNeS.
52
2.7.1.2
Reverberation
Reverberation is the return of sound back to the source from anything other
than the transponder (Lurton, 2002). Reverberation was considered during the
design of the BADNeS so it would not cause a problem. Two possible
consequences of reverberation are 1) picking up the interrogation signal on the
buoy’s receivers rather than picking up the response signal and 2) picking up multipath reflections of the signal. In the first case, the receivers are set to listen too
soon causing them to pickup the interrogation signal. This problem can be solved
by a simple delay, or in the case of the BADNeS, separate interrogation and
response frequencies can be used. The second problem is solved by the use of two
different frequencies as well. If a single frequency was used, a reflection of the
interrogation signal could bounce off a nearby object and reach the buoy’s receivers
well before the response pulse. This would effectively make the diver appear to be
located at the object causing the reflection. Since two separate frequencies are
used, the first signal to reach the transponder will be the original interrogation pulse
since reflected signals will incur longer travel times. The transponder will not
respond to signals picked up shortly after the original interrogation pulse as it will
be transmitting a response pulse. The same logic applies to the returned response
pulse. Thus reverberation should not be a problem.
53
2.7.2 Transducer Design Parameters
The transducer design parameters were derived from the desired
functionality of the BADNeS system. The design parameters established were
frequency, bandwidth, beam pattern and power output / sensitivity.
Two different frequencies were used for the interrogation and response
pulses for reasons discussed in Section 2.6.1.2. The use of two separate
frequencies was recommended by Lee Frey, a Florida Institute of Technology
master’s degree graduate, after his experience with an acoustic compass. The two
frequencies chosen were 35 kHz and 45 kHz. These were chosen because they are
not heard by divers, have relatively low loss over short distances and are different
enough in frequency to be discerned from one another.
For a narrowband application such as this, the bandwidth should be the
reciprocal of the transmit pulse length (Waite, 2002). Assuming a pulse length of 1
millisecond, a 1 kHz bandwidth would be appropriate.
The ideal beam pattern for this application is a hemi-spherical beam pattern.
This would allow the buoy and transponder to communicate regardless of the
diver’s position in reference to the buoy. The only restriction being that the diver
must be at or below the depth of the transducers on the buoy.
The required transducer power output / sensitivity was calculated by
predicting spreading and absorption loss then adding the predicted background
54
noise. Assuming a 45 kHz maximum frequency and a maximum distance from the
buoy to the diver of 100 meters the losses were calculated to be:
Spherical Spreading Loss = 20 × log(r ) = 20 × log(100) = 40.0dB
(
Absorption Loss = α × D = (0.05 × f 1.4 )× D = 0.05 × (45)
1.4
[3]
100 
)×  1000
 = 1.0dB [4]


Propagation Loss = Spreading Loss + Absorption Loss = 40.0 + 1.0 = 41.0dB [5]
(Waite, 2002)
Where:
r
α
D
f
= Radius in meters
= Absorption coefficient approximation in dB per km (sea water)
= Distance of travel in km
= Frequency of sound in kHz
Adding the propagation loss to the background noise gives an approximate
minimum figure for transducer power output. For this system the minimum
required power output varies from 83 – 101 dB depending on rain condition. In
practice, however, the transmitted power level should be greater than this figure so
that the signal to noise ratio is greater than 1.
2.7.3 Transducer Design
At the inception of this project, commercially available transducers with the
design parameters previously described were to be used for the prototype.
However, after researching the available transducers, none were found that suited
the application. In addition, the commercially available transducers found that
matched the design specifications the closest were prohibitively expensive (greater
55
than $1,000 each). Since the prototype unit requires six (6) transducers, cost alone
was a limiting factor. Therefore, the transducers had to be constructed in an
economical fashion. A future revision of the prototype should incorporate properly
designed transducers which when mass produced would become economical.
In order to construct six (6) transducers in a short period of time with a
small budget, only very basic design principles were utilized in the design. The
NonDestructive Testing (NDT) Resource Center was found to contain the basic
design formula needed for the task. A transducer has the following basic parts: a
backing, an active element and a matching layer (NDT Resource Center, 2008).
These parts are shown in Figure 32 along with the ideal thicknesses of both the
active element and the matching layer.
Figure 32 - Basic Transducer Design (NDT Resource Center, 2008)
The active element should be a half-wavelength thick for maximum energy
transfer. The matching layer provides an acoustic impedance match between the
active element and the water. In order to provide this, the matching layer should be
56
comprised of a material with acoustic impedance between that of the active element
and water. It should also be a quarter-wavelength thick in order to keep any
reflected waves in phase.
The backing layer affects the bandwidth of the transducer. Choosing a
backing layer with similar acoustic impedance as the active element produces a
transducer with a wide bandwidth. A mismatch in the acoustic impedance causes
the bandwidth to narrow. Since this application requires narrow bandwidth and no
exact equations for calculating the bandwidth versus acoustic impedance mismatch
were given, the selection of the backing layer was deemed not critical. Ultimately,
stainless steel was used for a backing layer.
Off the shelf piezo elements were used for the active element. A large
supply of extremely low-cost piezo elements were found by a previous Florida
Institute of Technology graduate student, Mischa Dembicki. Although these
elements weren’t ideal, they were used in the interest of saving time. By using
non-ideal piezo elements, efficiency is lost in the transfer from electrical energy to
acoustic energy and vice-versa.
The matching layer was designed to be a quarter-wavelength thick and have
acoustic impedance between water and the active element. Assuming a speed of
sound in seawater of 1500 m/s, the wavelength of a 40 kHz signal (mean of 35 kHz
and 45 kHz) is calculated to be:
57
λ=
c 1500m / s
=
= 0.0375m
f 40000 Hz
[6]
Where:
λ
c
f
= Wavelength in meters
= Assumed speed of sound in water in meters per second
= Frequency of sound in Hz
Dividing this result by four, results in a matching layer 9.4mm thick. The acoustic
impedance (Z) of a material is calculated by the following formula:
Z = pV
[7]
Where:
Z
p
V
= Acoustic impedance in Pascal seconds per meters
= Material density in kilograms per meters cubed
= Velocity of sound through the material in meters per second
Since the acoustic impedances of most materials are not published (as well as the
velocity of sound through them), the following logic was used to determine the
matching layer material. If it is assumed that the acoustic impedance of a material
is determined by density, then a material with a density between water and the
piezo element should suffice. The density of seawater is known to be
approximately 1,035 kg/m3. The density of a piezo-ceramic element is
approximately 7,600 kg/m3 (APC International Ltd., 2006). Since the transducer
had to be potted anyways, epoxy resin makes a practical matching layer. For West
System Epoxy Resin type 106/206, the density is 1180 kg/m3 (Gougeon Brothers,
Inc., 2007). This density may not provide for the best matching layer but it is
within the acceptable limits according to the basic design.
58
Ideally the transducer design would be tested for its power output /
sensitivity by simulation or prototype testing. Both of these methods require
extremely specialized software / equipment not readily available, therefore, the
actual power output / sensitivity of the transducers remains unknown.
2.7.4 Array Design
An array of transducers is needed to determine the azimuth and heading of
the transponder from the buoy. An incremental approach was taken to determine
the number of transducers required in the array. If the array is constructed with
only one transducer, no directional information could be ascertained as shown in
Figure 33.
Water Surface
Transducer
Possible
Transducer
Transponder
Positions
Bottom View
Side View
Figure 33 - Single Transducer Array
If an array consisting of two transducers oriented in a line is used,
directional information would be available, however, some ambiguity would still
59
exist. Figure 34 shows two pairs of transducer positions that would appear
identical to the array processor. The black circles represent one set of identical
looking positions while the grey circles represent another possibility.
Possible
Transponder
Positions
Water Surface
Transducers
Bottom View
Transducers
Side View
Figure 34 - Two Transducer Array
If an array consisting of three transducers arranged in an equilateral
triangular is used, no directional ambiguity will exist as shown in Figure 35. This
arrangement places the water surface above the array. Without this boundary, an
ambiguity as to whether the signal arrived above or below the array would exist.
60
Water Surface
Transducers
Transducers
Side View
Bottom View
Figure 35 - Three Transducer Array
More transducers can be added to the array to increase the accuracy, however, this
also increases the complexity and cost. It is for these reasons that the three
transducer array design was chosen.
For the three transducer array to work the transducers must be arranged in
an equilateral triangle. This requires that the transducers are arranged at 120
degrees angles from one another at the same distance from a central point. The
actual distance from the central point isn’t critical so long as the distance is
sufficiently large to produce a noticeable time difference for small changes in
transponder position. Matlab was used to simulate the time of arrival differences
between the three receiving elements. The angle at which the transponder was
offset from the transducer array was changed by one degree and the difference in
relative arrival times was noted. With a central point to transducer distance of
0.254 meters and a detection time of 25 nanoseconds, a 0.1 degree resolution is
61
achievable in an ideal world. With the transponder at a distance of 60 meters from
the buoy this translates into a linear distance error of less than 0.2 meters. This is
more than sufficient given the accuracy of the GPS position. Therefore, the central
point to transducer distance specification for the prototype was set at 0.25 meters or
greater.
3 Construction
Many pieces were custom fabricated for this prototype. Methods used to
create the components making up the final prototype are discussed in the following
sections. It should be noted that in a mass produced version, different construction
techniques would be employed.
3.1 Electronics
The construction of the electronics presented a few challenges, mainly due
to the small size and close proximity of the components. The first challenge was to
build the required circuit boards with extremely tight tolerances. Solder paste then
had to be placed on all the pads and the respective SMD components positioned on
top. The board then had to be heated so that the SMD connections were soldered.
Finally, the thru-hole components were soldered using standard techniques.
A professional board house was used in order to obtain circuit boards with
the required tolerances. The company Advanced Circuits was chosen as the
62
supplier because of customer recommendations, their more then sufficient
capabilities and the great student discount. The CAD files for each of the circuit
boards were uploaded via Advanced Circuits website. Two weeks later the boards
arrived as designed and for only $45 per board (including shipping).
The next task was to cover all the SMD component pads with a thin layer of
solder paste. Zephpaste by Zephyrtronics was used as the solder paste. This paste
dispensed out the tip of a needle mounted to a syringe when pressure was placed on
the syringe’s plunger. A variety of needle sizes were tested until one was found
that produced a small dot of solder paste without an extraordinary amount of
applied pressure. Normally stencils would be used to apply the paste to the
required areas; however, getting a stencil made for each board isn’t economical for
just one board. Instead of stencils, steady hands, patience and a little trick was
used. The paste was carefully placed by a stead hand on each individual pad (see
Figure 36). For the pads which were too small and too close together to place
individual beads of paste, a trick was devised. Each row of small, tightly spaced
pads was coated with one single bead of solder paste. A soldering iron was then
run over the pads. This melted the paste and forced it to split itself up among the
pads. These pads were then inspected for solder bridges which were removed if
found.
63
Figure 36 – Transponder Board With Solder Paste
With the solder paste placed, the SMD components then had to be placed.
The trick to this was a good pair of tweezers. Each component was identified,
checked for orientation and then placed gently on top of its pads (see Figure 37).
The board was sitting on top of a turn table which helped during this step.
Figure 37 – Transponder Board After Component Placement
The last SMD step was to heat the solder paste until it reflowed, electrically
and physically bonding the SMD components to the circuit board. An ingenious
method for doing exactly this was outlined on Spark Fun Electronics Website
64
(Sparkfun Electronics, 2006). This method involved pre-heating a store bought
skillet to its maximum temperature of 400 °F. The circuit board was then placed
(component side up) on the hot skillet using small bits of wire tied to the corners of
the board. The board was allowed to sit until the reflow of the solder paste was
observed. It was required that the board was moved around in order to get all the
components to reflow. The wires were then again used to gently lift the board off
the skillet and place it on the ground to cool.
The thru-hole components were soldered to the circuit board using
traditional techniques. This involved first placing the components. Next, a
soldering iron was used to heat the component lead, pad and a bit of solder. The
soldering iron was then removed when the solder had flowed between the
component lead and pad. Finally, any protruding component leads were trimmed.
The final board is pictured in Figure 38.
Figure 38 – Finished Transponder Board
65
3.2 Transducers
The transducers were constructed according to the design outlined in
Section 2.6.3 and a few other considerations. The design requires that a measured
thickness of resin be over top the piezo element and a piece of metal be directly
behind the piezo element. Mounting considerations require that a screw thread
come out the back of the transducer and common sense requires that all the
electrical connections be waterproof. The sensors should also be small so as to not
require much resin. The following steps were taken in building each transducer:
Note: Since this unit was constructed in the U.S.A, items and tools were used which
were sized according to standard units. In order to avoid confusion in the event
these instructions are used, the measurements have not been converted to metric.
1. A template was used to mark the center of a 1 inch PVC pipe cap (see Figure
39). A sharp pointed tool was then used to make an indentation on the plastic
cap.
66
Figure 39 - Drill Template
2. A drill was used to make a quarter inch hole in the center of the pipe cap (where
the mark was placed) as shown in Figure 40. It was discovered that a pre-drill
step with a small bit was required prior to using the quarter inch drill bit to
accurately position the hole.
Figure 40 - Drilling Screw Hole
67
3. A hole was drilled in the side of the plastic cap towards the bottom (see Figure
41). This hole is the entry point for the wire and was drilled so as to be just
large enough. Pre-drilling would also be advised for this step.
Figure 41 - Hole for Wire
4. The wire was fed through the side hole and soldered to the piezo element. The
soldered connections were covered with heat shrink tubing to avoid a short
circuit (see Figure 42).
Figure 42 - Soldering Wires to Piezo Element
68
5. A quarter inch stainless steel screw with a nut threaded on it was placed into the
hold in the bottom of the cap. The nut and head of the screw were installed
inside of the cap as shown in Figure 43.
Figure 43 - Screw in Cap
6. A stainless steel washer was placed on top of the screw head and the piezo
element was glued to the center of this washer (see Figure 44). A quick setting
super glue was used for this step. The orientation of the piezo element was
such that the metal surface was in contact with the washer while the actual
piezo material was facing upwards.
69
Figure 44 - Piezo Element Glued to Washer
7. The height of the piezo element from the top of the cap was adjusted such that
it conformed to the design. The depth measurement portion of a dial caliper
was used to make the measurement as shown in Figure 45. Adjustments were
made by turning the screw while pressing the nut against the cap bottom.
Figure 45 - Measuring Piezo Height
8. The assembly was prepared for pouring. The wire was glued so that only
insulation was showing on the outside. The washer was placed at an angle on
top of the screw head so as to allow resin to first flow to the bottom of the cap.
The screw protruding from the bottom of the cap was used to secure the whole
assembly to a rigid base. A second nut was then threaded on to lock the
assembly into place.
70
9. The epoxy resin was prepared. The resin used was a two part West System
epoxy resin: type 105 resin and type 206 hardener. The resin and hardener were
measured according to the directions and mixed thoroughly.
10. The resin was poured into the caps. Once enough resin was poured such that
the area below the washer was full, the washer was rested on the screw head.
More resin was poured until it reached the top of the cap (see Figure 46). As
the resin set up the caps were monitored. Any air bubbles were removed using
a toothpick to bring them to the surface.
Figure 46 - After the Resin Pour
3.3 Buoy
The buoy was required for two purposes, to float the GPS unit and to mount
the transducers. The GPS unit has to be above the surface of the water and have an
unobstructed view of the sky. The transducers have to be mounted so that they are
submerged and have an unobstructed view of the transponder. They also have to be
71
mounted in such a way as to meet the acoustic array design criteria specified in
Section 2.6.4.
The base of the buoy was constructed from an inner tube and some wood.
The inner tube was picked up from a local tire shop and selected such that its outer
diameter was approximately 0.6 meters. This size was selected because it is
slightly bigger than the acoustic array so as to provide protection and still be small
enough to fit easily on a dive boat. A bit of plywood was cut into a circle roughly
the size of the inner tube. A few 2 by 4’s were then screwed into the plywood and
cut such that a pelican case could be secured. The pelican case was used to house
the buoy electronics including the GPS unit. A series of holes were drilled around
the outside of the wooden base. The majority of holes were made to secure the
base to the inner tube. The other two holes were made for transducer wires to run
through to the center of the buoy. After the wooden base was finished, it was
coated in epoxy resin in order to waterproof it. The base was then painted and
secured to the inner tube with bungee cord as shown in Figure 47.
72
Figure 47 – Buoy With Electronics Box
The transducer mount was constructed out of aluminum. Aluminum was
chosen for three key factors: strength, rigidity and being non-ferrous. Strength is
required since the entire weight of the buoy rests on the mount when it isn’t in the
water. Rigidity is required to keep the transducers from moving relative to the GPS
antenna and one another. The mount needs to be non-ferrous so as to not interfere
with the magnetometer mounted atop the buoy. The mount was built by welding
some bits of flat aluminum stock to a central aluminum tube. The flat aluminum
pieces were cut and welded so as to adhere to the transducer array design
calculations. After welding these it was discovered that the flat bar could bend
rather easily so a few supporting members were added. A mounting plate was then
welded to the opposite end of the central tube. Finally, four holes were drilled into
the mounting plate. The finished mount is pictured in Figure 48.
73
Figure 48 – The Acoustic Array Mount
The transducer array mount was attached to the underside of the wooden
base. This was accomplished by using screws to secure the mounting plate on the
transducer array to the underside of the wooden base. The array was mounted in
the center of the wooden base with an arbitrary orientation. The orientation doesn’t
matter so long as it is known. The final constructed buoy is shown in Figure 49.
Figure 49 – Final Constructed Buoy
74
3.4 Diver Unit
Figure 50 - Diver Carried Transponder
The diver carried transponder (shown in Figure 50) was constructed out of a
3250 Series Otter Box. This polycarbonate housing has ample space for the
batteries, electronics and transducers and is also rated to 30 meter (100 foot) water
depth (Otter Products, LLC., 2008). The batteries and electronic circuit board were
secured by use of double-sided adhesive strips.
The transducers for the transponder were molded to the top of the housing.
Steps similar to that described in Section 3.2 were used to accomplish this. Instead
of West System Epoxy Resin, a 3M Marine Adhesive Sealant (type 5200) was
used. 3M 5200 Sealant was used because it doesn’t generate a lot of heat like the
West System Resin. Excessive heat could warp the polycarbonate housing and
compromise its structural integrity.
75
4 Calibration
Before the BADNeS could be used some calibration parameters had to be
measured. These calibration parameters are required to correct the raw
measurements and calculate the diver’s absolute location.
4.1 Offsets
Several offsets must be known to reference the transponder position to the
GPS datum, WGS84. Figure 50 shows the required offsets. Offset A and D are the
GPS antenna’s center offset from the center of the SSBL array. Offset B is the
vertical offset of the GPS antenna from the SSBL array. Offset C is the distance of
the SSBL transducers from the center of the array. The offset angle of the acoustic
array and the magnetometer is also required.
A
Interrogation
Transducer
C
GPS Antenna
A
B
D
C
C
GPS Antenna
Receiving
Transducers
Transducer
C
Top View
C
Side View
Bottom View
Figure 51 – Offsets Required to Reference Transponder Position to Buoy Position
76
4.2 Tilt
The tilt sensor must be calibrated before use to remove any tilt angle present
in the sensor mounting. The calibration procedure requires that the SSBL array is
level in both the pitch and roll axes. The sensor calibration command is then sent
to the tilt sensor. This command instructs the sensor to measure the tilt and save it
as an offset. Every raw measurement following the calibration will have this offset
subtracted from it.
4.3 Heading
By performing a simple calibration, the direction of magnetic north and thus
the buoy’s heading, can be deduced. The calibration procedure described in PNI
Corporation’s multipoint calibration application note requires that data from the
magnetometer be collected while it is rotated slowly a full 360 degrees. An
example of this collected data is shown in Figure 51.
77
Figure 52 – Example calibration data (PNI Corporation, 2004)
From the data collected the maximum and minimum X and Y values are
determined. Two offsets and two gains are then calculated via the following
formulas:
Xoffset = ( Xmax + Xmin ) / 2
Yoffset = ( Ymax + Ymin ) / 2
Xrange = ( Xmax – Xmin )
Yrange = ( Ymax – Ymin )
[8]
[9]
[10]
[11]
The offsets and gains are applied to the raw data by the following method (see
Figure 52):
1) Subtract the offsets.
Xvalue = Xraw – Xoffset
Yvalue = Yraw – Yoffset
2) Scale the data.
If Xrange > Yrange Then,
[12]
[13]
78
Yvalue = ( Yvalue * Xrange ) / Yrange
Else,
Xvalue = ( Xvalue * Yrange ) / Xrange
[14]
[15]
Figure 53 – Data after offsets are applied (PNI Corporation, 2004)
79
Figure 54 - Calibrated Magnetometer Output
Figure 53 depicts the expected magnetometer output after calibration for various
values of heading with respect to magnetic north. The heading is computed from
the calibrated X and Y values by the following method:
1) Calculate A in degrees.
A = arctan( Xvalue / Yvalue)
[16]
2) Find the heading based upon the quadrant.
If X >= 0 & Y >= 0 Then, Heading = Angle + 270
If X < 0 & Y >= 0 Then, Heading = Angle + 270
If X < 0 & Y < 0 Then, Heading = Angle + 90
If X >= 0 & Y < 0 Then, Heading = Angle + 90
[17]
[18]
[19]
[20]
80
While only two-axes are needed to determine heading, data from the third axis
could be used for additional heading correction when roll and pitch are severe.
4.4 Electronic Delay
Electronic delay affects both the range and bearing measurements of the
acoustic processor by increasing the time of arrival at the transducers. The delay
originates from a propagation delay in the electronic filter and amplifiers. This is
present in both the transponder electronics and the acoustic processor electronics.
To find out the electronic delay present in the path of each receiving transducer, the
transponder is placed at a known depth directly below and in the center of the
SSBL array. The acoustic processor is issued a get measurement command and the
arrival times at each transducer recorded. An accurate speed of sound
measurement should be taken shortly before or after this measurement is taken.
Post-processing is then performed to find the delay present in the arrival time of
each transducer’s measurement by subtracting the estimated roundtrip travel time
from each arrival time. These calculated delays are the electronic propagation
delays and should be subtracted from the arrival times on all subsequent
measurements. It should be noted that each transducer will experience a unique
propagation delay. While all the circuitry is the same for each receiving element,
tolerances in component values affect signal propagation causing unique delays.
81
5 Post Processing
Post processing is used in order to calculate the final position of the
transponder given raw measurements from the GPS, orientation sensors and
acoustic array. The following steps are taken during the post-processing:
1) Compute the Range (R) of the transponder from acoustic array. The
range is computed using the roundtrip travel time of sound from the buoy to
the transponder and back. The following formula is used:
 T 

R =  RT  − Delay  × C avg
 2 

[21]
Where:
Range = Distance from transducer to buoy in meters
= Round trip travel time in seconds
TRT
Delay = Calibration coefficient to make up for any delays in travel time
Cavg
measurement given in units of seconds.
= Measured or assumed average velocity of sound in water given in units
of meters per second.
2) Compute the Elevation Angle ( β ) and Heading Angle ( α ) of the
transponder from the acoustic array. The elevation and heading angles
are computed using the difference in arrival time between the three
transducers making up the acoustic array. These are computed with the
following formulas (see the corresponding diagram in Figure 54):
 ∆T 
α =  bc  × 60° + A
 ∆Tac 
[22]
82
For odd-numbered zones (1,3,5)
 ∆T 
 × 60° + A

α =  ab
 ∆Tac
[23]
For even-numbered zones (2,4,6)
 ∆Tac 

∆
T
 ac max 
β = cos −1 
[24]
Where
∆Tab
= Time difference between the first and second transducers to detect the
∆Tbc
signal.
= Time difference between the second and last transducers to detect the
∆Tac
signal.
= Time difference between the first and last transducers to detect the
signal.
∆Tac max = Maximum time difference between the first and last transducers to
A
detect the signal.
= Lower angle limit of the zone (e.g. for Zone 5, A = 240)
(Frey, 2003)
83
Figure 55 - Wave Propagation Over the Acoustic Array (Frey, 2003)
84
3) Using the orientation sensors and measured offsets, determine the
offset of the transponder from the buoy in Cartesian coordinates.
4) Convert the GPS coordinates of the buoy to Cartesian coordinates. The
conversion from GPS coordinates to Cartesian coordinates is performed
with the following formulas:
X GPS = (v + h ) × cos(φ ) × cos(λ )
[25]
YGPS = (v + h ) × cos(φ ) × sin(λ )
[26]
(
)
Z GPS = (v × 1 − e 2 + h ) × sin(φ )
[27]
(Misra, 2004)
Where:
X GPS = X Cartesian coordinate of GPS position in meters
YGPS = Y Cartesian coordinate of GPS position in meters
Z GPS = Z Cartesian coordinate of GPS position in meters
φ
λ
h
= GPS latitude in degrees
= GPS longitude in degrees
= GPS height above the ellipsoid in meters
v =
(1 − e
a
2
)
× sin 2 (φ )
1/ 2
[28]
= WGS84 ellipsoid semi major axis = 6,378,137.0 meters
a
2
e = WGS84 first ellipsoid eccentricity squared = 0.00669437999013
5) Add the offset of the transponder from the buoy to the GPS
coordinates.
85
X T = X GPS + X TB
[29]
YT = YGPS + YTB
[30]
Z T = Z GPS + Z TB
[31]
Where:
XT
YT
ZT
X TB
YTB
Z TB
= X Cartesian coordinate of the transponder (diver) in meters
= Y Cartesian coordinate transponder (diver) in meters
= Z Cartesian coordinate transponder (diver) in meter
= X Cartesian coordinate of the transponder offset in meters
= Y Cartesian coordinate transponder offset in meters
= Z Cartesian coordinate transponder offset in meter
6) Convert the transponder’s coordinates back to the WGS-84 coordinate
system. The conversion from Cartesian Coordinates to WGS-84 is
performed with the following formulas:
 YT
 XT
λT = tan −1 
hT =



[32]
p
− vT
cos(φT )
Z
φT = tan  T
 p

−1

 vT
1 − e 2 
v +h

T
 T

[33]

 


−1




[34]
(Misra, 2004)
Where:
φT
λT
= Transponder longitude in degrees
hT
= Transponder height above the ellipsoid in meters
= Transponder latitude in degrees
86
vT =
p=
(1 − e
a
2
× sin
2
(φT ))1 / 2
X T2 + YT2
[35]
[36]
Note: Iterations are required to find latitude and height to the required precision.
6 System Performance
The proto-type unit was tested to get an estimate of the overall system’s
positioning accuracy and precision. The system performance estimate was
obtained by finding the contribution of each sensor to the overall positioning error.
In order to do this, each sensor’s accuracy and precision had to be tested
individually. The testing procedure and results are described below along with the
final overall system performance estimate.
6.1 GPS Receiver
The GPS receiver was tested by placing its antenna at a known location and
logging its computed positions. The logged data was post-processed in MATLAB
to determine the accuracy and standard deviation of the GPS receiver in terms of
2D RMS positioning error. Since the accuracy of the GPS receiver depends on the
orientation of the GPS Satellites which vary with time and location, the GPS
receiver was tested at nine (9) different times and at two locations.
87
6.1.1
Data Collection
The GPS data was collected at two locations with known coordinates. The
first location was in the parking lot of Fugro West, Inc., a survey company in
Ventura, California. The location, known to Fugro West as “point 6”, was marked
by a nail head embedded in asphalt. The coordinates of point 6 were provided by
Fugro West, Inc. The provided coordinates were referenced to the original GPS
datum (original WGS 84) which differs slightly from the reference now used by the
GPS, WGS 84 (ITRF00). In order to transform the coordinates to the new
reference, the Horizontal Time Dependent Positioning (HTDP) utility available in
the Geodetic Tool Kit on the National Geodetic Survey (NGS) Website
(http://geodesy.noaa.gov/) was used. The second location was found using the
datasheets search on the NGS website. This search provided a list of known points
in the local area for which the NGS had datasheets. RINCON (PID: EW7931) was
chosen as a second test location due to its proximity to the ocean and its recently
noted good condition. The RINCON datasheet (see Appendix E) listed its
coordinates in the current WGS 84 epoch so no transformation was required.
The GPS receiver was positioned over the known location with a tripod
equipped with a tribrach. First the tripod was positioned over the known landmark
and roughly leveled. The tribrach was then adjusted to make it level and its optical
plummet positioned directly over the landmark as shown in Figure 53.
88
Figure 56 - Tripod with Tribrach Set Over Point 6
Figure 57 - Tripod with Tribrach Used to Position GPS Receiver Over Point 6
89
The GPS receiver’s antenna was then positioned directly over the known location
using the target on the tribrach’s optical plummet as shown in Figure 54. With the
GPS in place, the box containing the GPS was taped in place and the tripod
removed.
Data logging was performed by switching on the power switch to the buoy
electronics. The buoy electronics logged the GPS data to an SD card for postprocessing in MATLAB.
6.1.2 Data Processing
The data collected from the GPS receiver was imported into MATLAB and
compared against the coordinates of the known point to find the precision and
accuracy of the GPS receiver. The following steps were taken for each of the data
sets:
1) Both the known and logged GPS coordinates were converted from latitudes
and longitudes to Cartesian Coordinates (see Section 5 for this formula).
An elevation of 0.0 meters was used for the height.
2) The distance between each of the logged positions and the known position
was calculated using the distance formula. This distance is also the 2D
RMS positioning error.
2 D _ RMS _ Error =
(X K
− X L ) + (YK − YL ) + (Z K − Z L )
2
2
3) The average 2D RMS Error was computed and recorded.
2
[37]
90
4) The maximum 2D RMS Error was found and recorded.
5) The standard deviation of the 2D RMS Error was computed and recorded.
6) The fix type was recorded. The Standard Positioning Service (SPS) fix type
indicates a regular GPS position. The Differential fix type indicates the use
of a WAAS satellite to obtain differential corrections.
7) The duration of the data collected was also recorded.
8) Trimble Planning Software version 2.74 was used to obtain a predicted
Horizontal Dilution of Precision (HDOP). The HDOP is a unit-less
measure of GPS data quality based on the orientation of the GPS satellites.
Lower HDOP values indicate better satellite orientations and position
estimates. By entering the date, time, position and obstruction information
into the Trimble Planning Software, a list of HDOP values were output (see
Appendix E).
The results of the GPS Receiver testing are shown in Table 2.
91
Table 2 – GPS Receiver Testing Results
Location
Fugro
Point 6
Fugro
Point 6
Fugro
Point 6
Fugro
Point 6
Fugro
Point 6
Fugro
Point 6
RINCO
N
RINCO
N
RINCON
Date/Time
(P.S.T.)
12/26/2007
5:23 PM
12/26/2007
5:33 PM
12/27/2007
1:48 PM
12/27/2007
2:00 PM
12/28/2007
7:48 AM
12/28/2007
7:58 AM
12/29/2007
9:57 AM
12/29/2007
10:35 AM
12/29/2007
11:08 AM
Mean
2DRMS
Error
(m)
Max
2DRMS
Error
(m)
Standard
Deviation
(m)
Predicted
HDOP
Fix
Type
Record
Length
(min)
3.95
8.99
0.83
1.62
SPS Fix
10
4.69
6.78
0.58
1.58
SPS Fix
8
24.35
33.71
3.46
2.57
SPS Fix
11
13.78
18.63
1.65
2.35
Diff
9
5.16
8.33
1.77
1.36
SPS Fix
9
4.87
8.89
1.97
1.41
SPS Fix
7
2.86
4.27
0.86
1.05
Diff
32
3.92
4.59
0.31
1.44
Diff
32
5.88
18.23
3.61
3.21
SPS Fix
28
6.2 Magnetometer
Accuracy of the magnetometer was tested and verified with an S.G. Brown
TSS Meridian Surveyor Gyro. The devices were mounted on a fiberglass cart, to
prevent disrupting the magnetometer, as shown in Figure 55. This setup allowed
both devices to be rotated simultaneously. This ensured that the gyro and
magnetometer maintained a constant heading offset from one another. The gyro
has a listed accuracy of ±0.05 degrees (S G Brown, 2003) and outputs headings
referenced to true north. For this reason, the gyro’s heading was assumed to be the
actual heading.
92
Figure 58 - Magnetometer Testing Setup
6.2.1 Data Collection
Before a heading could be determined from the magnetometer output, a
magnetometer calibration had to be done as described in Section 4.3. Three
calibrations were performed to ensure that the results obtained could be reproduced.
The gyro heading was recorded at the start of each calibration in order to obtain the
heading offset between the gyro and the magnetometer. The offset was computed
by subtracting the magnetometer heading at the start of the calibration from the
gyro heading. The offsets computed for each of the three (3) calibrations are listed
in Table 3.
Table 3 - Computed Offsets
Calibration
Offset
Number
(degrees)
1
-12.9
2
-10.6
3
-37.6
93
After the magnetometer calibrations, data was collected at 24 degree
increments. Approximately ten (10) seconds of data from the magnetometer was
logged to an SD card at each heading while the gyro heading, which remained
constant, was read from its display and recorded. A sample of the data recorded
from the magnetometer is shown in Figure 56.
Figure 59 - Sample Heading Data
6.2.2 Data Processing
MATLAB was used to process the collected data. The heading error was
computed by subtracting the average magnetometer heading from the gyro heading
for each of the fifteen (15) data records. The heading error was computed using the
94
calibration parameters from each of the calibrations. The results are plotted in
Figure 57. The standard deviations were also calculated and found to vary only
slightly among the different calibrations. For this reason, only the standard
deviation values obtained using calibration two were plotted (see Figure 58).
Table 4 shows the average statistics for each of the calibrations.
Figure 60 - Plot of Heading Error
95
Figure 61 - Plot of Standard Deviation
Table 4 - Overall Heading Statistics
Mean Heading
Error (degrees)
Mean Standard
Deviation (degrees)
Calibration Calibration Calibration
1
2
3
-0.17
-0.53
0.16
0.12
0.13
0.13
6.3 Tilt Sensor
Accuracy of the tilt sensor was tested and verified with a CodaOctopus
F180 Attitude and Positioning System. The devices were mounted to a sheet of
plywood so that the change in tilt was the same for both devices as shown in Figure
96
61. The F180 was assumed to be the actual tilt as its listed accuracy is better than
0.025 degrees (CodaOctopus, 2008).
Figure 62 - Tilt Sensor Testing With CodaOctopus F180
6.3.1 Data Collection
Tilt sensor data was logged to an SD card and F180 data was read from a
computer display and recorded for various tilt angles. Small wood pieces were
placed under different areas of the plywood on which the tilt sensor and F180 were
mounted to create the various tilt angles.
6.3.2 Data Processing
MATLAB was used to process the collected data tilt sensor data (see Figure
59 for a sample tilt record). First, the X and Y tilt offsets between the tilt sensor
and the F180 were calculated. These offsets were then added to all of the tilt sensor
97
measurements. The X and Y tilt errors were computed by subtracting the average
tilt sensor readings from the F180’s roll and pitch readings for each of the various
tilt angles. The results are plotted in Figure 60. The standard deviations were also
calculated and plotted in Figure 61. Table 5 shows the average statistics for each of
the calibrations.
Figure 63 - Sample Tilt Data
98
Figure 64 - Mean Tilt Error
Figure 65 - Tilt Standard Deviation
99
Table 5 - Overall Tilt Sensor Statistics
X Tilt Y Tilt
Mean Tilt
Error (degrees)
Mean Tilt
Standard Deviation
(degrees)
0.05
0.00
0.30
0.29
6.4 Super Short Base Line
The SSBL portion of the BADNeS prototype was unable to be tested for
accuracy and precision due to a hardware problem discovered in the acoustic
processor. While tuning the three (3) tone decoders on the acoustic processor, a
coupling effect between the tone decoders was found. The problem was shown to
Mike Barth and Larry of Subsea Systems, both experts in underwater acoustics.
After some probing, noise coupling through the power supply was suspected to be
the likely cause. This can be fixed by running each receiver circuit from separate
voltage regulators and isolating the ground planes of each receiver. The ground
planes still need to be connected to one another but only at a single point to prevent
unwanted current loops. While these issues can be fixed, they will require a new
acoustic processor to be constructed.
The working distance of the acoustic system was tested in a limited manner
due to the problem in the receivers. With the inputs to two of the three receivers
grounded, it was discovered that the third receiver could be used with the pre and
post-amp gains set to their minimums. One transducer was connected to the
100
working receiver while another was connected to the output of the transponder
board. Both transducers were placed in a body of salt water with the face of each
transducer facing one another With the transponder board programmed to send out
a 1 millisecond pulse approximately 3 times per second, the receiving transducer
was moved further away until no signal was showing on the receiver’s LED. This
distance turned out to be approximately two (2) meters.
A few things can be done to improve the working distance of the BADNeS
proto-type. Amplifying the received signal more then the minimum amount will
help to increase the detection distance of the BADNeS. This can be done once the
noise-coupling issue between the receivers is solved. Increasing the voltage used to
drive the transducer will also increase the working distance. This can be
accomplished with the use of a properly designed transformer or a new transducer
driving circuit capable of higher voltages. The transducer could also be changed to
increase the working distance. A professionally designed and tested transducer
made specifically to suit this application would be ideal. These options should be
further pursued in future revisions of the BADNeS.
6.5 Overall System Performance
Since the overall performance of the BADNeS prototype couldn’t be
directly tested, the overall performance was estimated. The performance estimate
was calculated by summing up the expected positioning error of each error source.
101
The magnitude of the error sources were calculated if testing data was available, or
estimated in the case of sound velocity and sampling error. If test data was
available, the mean error and standard deviation of the measurements was added to
estimate an average maximum expected error. The error estimates were converted
into induced horizontal positioning errors using the plots in Section 2.3.2. Worst
case errors were assumed where the error depended on more than one factor. The
induced positioning errors from each error source were summed to produce the
overall system performance estimate. All the error estimates as well as the overall
system performance estimate are shown in Table 6.
102
Table 6 - Overall System Performance Estimate
Error
Source
Estimated
Horizontal
Position
Mean Standard
Error
Error
Error Deviation Estimate (meters)
GPS
3.92m
0.31m
4.23m
4.2
Sound
Velocity
10m/s
0
0.7m
0.7
Heading
-0.53°
0.13°
1.2°
2.4
X Tilt
0.05°
0.3°
0.85°
1.6
Y Tilt
0.00°
0.29°
0.79°
1.5
Total:
10.4
Notes
The error statistics of
the median
differential fix GPS
test were used.
Mean error is an
assumed value. The
estimated position
error was found using
Figure 10. Position
Error estimate based
on an assumed 0°
elevation bearing.
Error estimate
includes a 0.5°
sampling error.
Figure 12 was used to
find estimated
position error. Error
estimate based on an
assumed 0° elevation
bearing.
Error estimate
includes a 0.5°
sampling error.
Figure 11 was used to
find estimated
position error.
Error estimate
includes a 0.5°
sampling error.
Figure 11 was used to
find estimated
position error.
103
7 Conclusion
The goal of this project was to develop and test a low-cost underwater
positioning system suitable for recreational and scientific diving. It was desired
that the developed prototype unit cost less than $1,500, be easily transportable and
have a horizontal positioning accuracy of better than 10 meters. It is believed that
these goals have been met even though the first prototype, known as BADNeS, was
not fully functional. The prototype was constructed for approximately $800,
substantially less than current commercial systems which start at $15,000. The
prototype is also small in size allowing for easy transport on board small vessels.
Due to noise coupling in the receiver circuits, the prototype could not be directly
tested to determine its accuracy. Instead, each sensor that made up the prototype
was individually tested using survey grade equipment. Based on the test results, a
worst-case horizontal positioning error was estimated to be 10.4 meters based on a
GPS accuracy of 4.2 meters. On average the positions should be more accurate
than the worst case estimate making the desired 10 meter accuracy achievable.
In conclusion, the BADNeS prototype showed that an underwater
positioning system with all necessary components integrated is feasible for
personal use. The use of new board level sensor technology allowed for accurate
measurements at a fraction of the cost and size of commercially available sensors.
The cost of the prototype system was nearly 20 times cheaper than that of a
104
commercially available system. In addition, the developed prototype system could
be easily carried with one hand and fit in a circle of diameter just over a half meter.
Although this prototype was not fully functional, problems were identified and
their likely causes determined such that a future revision of the prototype will be
completely operational. Overall, the project was a success and the desired goals
were achieved.
105
References
"Acoustic Positioning Techniques." 2006. Sonardyne International Ltd. 11 Sept.
2006 <http://www.sonardyne.co.uk/Support/PositioningTechniques/index.html>.
Allegro MicroSystems Inc. A3950 DMOS Full-Bridge Motor Driver Datasheet.
2006. 27 Jan. 2008 <http://www.allegromicro.com/en/Products/Part_Numbers/
3950/3950.pdf>.
Analog Devices. ADIS16201 Programmable Dual-Axis Inclinometer /
Accelerometer Data Sheet. June 2006. 27 Jan. 2008
<http://www.analog.com/UploadedFiles/Data_Sheets/ADIS16201.pdf>.
APC International Ltd. Properties of APC Materials. 2006. 27 Jan. 2008
<http://www.americanpiezo.com/materials/apc_properties.html>.
Austin, T.C. “The Application of Spread Spectrum Signaling Techniques to
Underwater Acoustic Navigation,” Proceedings, A W ‘94, IEEE Conference on
Autonomous Underwater Vehicles, Cambridge MA, July, 1994.
CodaOctopus Products Ltd. CodaOctopus F180 Technical Brief. 11 Feb. 2008
<http://www.codaoctopus.com/motion/f180/techspec.asp>.
"Diver Hand-held GPS System." . 2006. Sound Ocean Systems, Inc.. 11 Sept. 2006
<http://www.soundocean.com/diverGPS-30.htm>.
"DLS-1 Diver Locator Sonar." . 2002. RJE International, Inc.. 11 Sept. 2006
<http://www.rjeint.com/pdf/DLS1.pdf>.
"EM-406 GPS Receiver Engine Board." 27 MAR 2006. GlobalSat Technology
Corp.. 15 Nov 2006
<http://www.usglobalsat.com/downloads/engineboards/EM406.pdf>.
Etek Navigation, Inc. Smart GPS Reciever Model: EB-85A. 10 Feb. 2008
<http://www.sparkfun.com/datasheets/GPS/EB-85A_Catalog.pdf>.
Frey, C.L.; Wood, S.L., "Development of an autonomous underwater vehicle for
sub-ice environmental monitoring in Prudhoe Bay, Alaska," OCEANS 2003.
Proceedings , vol.2, no., pp. 1161-1173 Vol.2, 22-26 Sept. 2003
106
"Gear Tests." . Mar. 2002. Divernet. 11 Sept. 2006
<http://www.divernet.com/equipment/0302divertests.htm>.
Gougeon Brothers, Inc. Typical Physical Properties. 25 Sept. 2007. 27 Jan. 2008
<http://www.westsystem.com/frames/tier2/productinfo/typicalphysical
properties.htm>.
Lekkerkerk, Huibert-Jan, et al. Handbook of Offshore Surveying:
Volume 1 (Preparation and Positioning). London: Oilfield Publications Limited,
2007.
Lekkerkerk, Huibert-Jan, et al. Handbook of Offshore Surveying:
Volume 2 (Acquisition & Processing). London: Oilfield Publications Limited,
2007.
Lurton, Xavier. An Introduction to Underwater Acoustics. Chichester: Praxis
Publishing, 2002.
Misra, Pratap, and Per Enge. Global Positioning System Signals, Measurements,
and Performance. Lincoln: Ganga-Jamuna Press, 2004.
National GeoSpatial-Intelligence Agency. Magnetic Total Intensity at 2005.0 from
the World Magnetic Model 2005. 3 Nov. 2005. 27 Jan. 2008
<http://www.ngdc.noaa.gov/seg/WMM/data/wmm-F05.pdf>.
"Navigation and Positioning ." AUV DesignInfo Page. 2001. International
Submarine Engineering Ltd.. 15 Nov 2006
<http://www.ise.bc.ca/WADEnavandpos.html>.
NDT Resource Center Characteristics of Piezoelectric Transducers. 27 Jan. 2008
<http://www.ndt-ed.org/EducationResources/CommunityCollege/Ultrasonics/
EquipmentTrans/characteristicspt.htm>.
Otter Products, LLC. OtterBox.Com Knowledge Base. 27 Jan. 2008
<http://www.otterbox.com/knowledge_base/questions/39/To+what+depth+are+
OtterBox+waterproof+to%3F>.
107
PNI Corporation. Application Note: Multipoint Calibration Primer. 13 Jan. 2004.
27 Jan. 2008 <https://www.pnicorp.com/downloadResource/c40c/manuals/53/
Ap+Note+Multipoint+Calibration+Primer+%28APNOTE+-+1001766++R01%29.pdf>.
PNI Corporation. MicroMag3 Data Sheet. PNICorp.com. June 2006. 27 Jan. 2008
<https://www.pnicorp.com/downloadResource/c40c/manuals/110/
MicroMag3+3-Axis+Sensor+Module_June+2006.pdf>.
S G Brown, ed. Meridian Surveyor System Manual. Issue 2.6 ed. 2003.
Sparkfun Electronics. Reflow Skillet. 2006. 27 Jan. 2008
<http://www.sparkfun.com/commerce/present.php?p=Reflow%20Skillet>.
"TrackLink Models." May 2005. LinkQuest Inc.. 01 Dec 2006
<http://www.link-quest.com/html/models2.htm>.
Waite, A. D. SONAR for Practising Engineers. 3rd ed. Chichester: John Wiley &
Sons Limited, 2002.
108
Appendix A: Circuit Diagrams
(Note: Larger printouts included in the envelope attached to the back cover.)
109
110
111
112
Appendix B: PCB Layout
113
Main Board – Top Layer
114
Main Board – Bottom Layer
115
Main Board – Silk Screen
116
Acoustic Processor – Top Layer
117
Acoustic Processor – Bottom Layer
118
Acoustic Processor – Silk Screen
119
Transponder – Top Layer
120
Transponder – Bottom Layer
121
Transponder – Silk Screen
122
Appendix C: PIC Assembly Code
123
;**********************************************************************************
;************************* MAIN BOARD MICROCONTROLLER CODE ************************
;**********************************************************************************
.equ __33FJ128GP306, 1
.include "p33FJ128GP306.inc"
;..............................................................................
;Configuration bits:
;..............................................................................
config __FOSC, FCKSM_CSDCMD & OSCIOFNC_ON & POSCMD_EC
;Turn off clock switching and
;fail-safe clock monitoring and
;use the External Clock as the
;system clock
config __FWDT, FWDTEN_OFF & WINDIS_OFF;Turn off Watchdog Timer
config __FPOR, FPWRT_PWR128
;Set Power on Timer 128 mSec
config __FOSCSEL, FNOSC_PRIPLL & IESO_OFF & TEMP_OFF
;Setup Oscillator (w/PLL)
config __FGS, GSS_OFF & GCP_OFF & GWRP_OFF ;Disable Code protect
config __FSS, RSS_NO_RAM & SSS_NO_FLASH & SWRP_WRPROTECT_OFF ;No security
config __FBS, RBS_NO_RAM & BSS_NO_FLASH & BWRP_WRPROTECT_OFF ;
;..............................................................................
;Program Specific Constants (literals used in code)
;..............................................................................
;----DATALOGGER
.equ
.equ
.equ
.equ
.equ
EQUATES--------------------------DL_PORT,PORTC
;
DL_TRIS,TRISC
;
DL_RST,1
;RESET LINE
DL_STOP,14
;STOP LINE
DL_ENABLE,13
;ENABLE LINE
;----GPS (UART 1) EQUATES------------------------.equ
GPS_PORT,PORTF
;
.equ
GPS_TRIS,TRISF
;
.equ
GPS_ENABLE,6
;
.equ
.equ
.equ
BAUD_NUM1,51
;Baud Rate Number (51=9600baud,103=4800)
;19200=25,38400=12
U1MODE_C,0b1000000000000000
;U1MODE Init Value - BRGH = 0
U1STA_C,0b0000000000000000
;U1STA Init Value
;----DATALOG,COMPUTER,BLUETOOTH (UART 2) EQUATES-.equ
BAUD_NUM2,12
;Baud Rate Number (51=9600baud,103=4800)
;19200=25,38400=12
.equ
U2MODE_C,0b1000000000000000
;U2MODE Init Value - BRGH = 0
.equ
U2STA_C,0b0000000000000000
;U2STA Init Value
;----MISC INTERRUPT EQUATES----------------------.equ
INT_PORT,PORTD
;
.equ
INT_TRIS,TRISD
;
.equ
GPS_INT,11
;GPS 1PPS OUTPUT
.equ
BT_PIO5,0
;BLUETOOTH AUX IO
.equ
BT_CONN,10
;BLUETOOTH CONNECTION ACTIVE
.equ
RTC_INT,9
;RTC INTERRUPT PIN
124
.equ
.equ
.equ
.equ
UI_AIO2,8
UI_AIO3,5
UI_PORT,PORTC
UI_AIO1,15
;USER INTERFACE AUX IO
;
;
;USER INTERFACE AUX IO
;----UART 2 SOURCE SELECT EQUATES----------------.equ
U2SS_PORT,PORTD
;
.equ
U2SS_TRIS,TRISD
;
.equ
RXSS0,1
;RECIEVE SOURCE SELECT 0
.equ
RXSS1,2
;RECIEVE SOURCE SELECT 1
.equ
TXSS0,3
;TRANSMIT SOURCE SELECT 0
.equ
TXSS1,4
;TRANSMIT SOURCE SELECT 1
;----USER INTERFACE EQUATES----------------------;----I2C EQUATES (RTC, UI)-----------------------.equ
I2C_PORT,PORTG
;
.equ
I2C_SCL,2
;
.equ
I2C_SDA,3
;
;----TILT SENSOR EQUATES-------------------------.equ
XL_PORT,PORTB
;
.equ
XL_TRIS,TRISB
;
.equ
XL_SCLK,12
;
.equ
XL_DOUT,13
;
.equ
XL_DIN,14
;
.equ
XL_CS,15
;
.equ
XL_DIO0,11
;
;----ACOUSTIC PROCESSOR EQUATES------------------.equ
AP_PORT,PORTG
;
.equ
AP_TRIS,TRISG
;
.equ
AP_SS,9
;
.equ
AP_SDO,7
;
.equ
AP_SCK,6
;
.equ
AP_SDI,8
;
.equ
AP_APORT,PORTC
;
.equ
AP_AIO1,2
;
;----AUX CONNECTION EQUATES----------------------.equ
AUX_PORT,PORTG
;
.equ
AUX_1,0
;
.equ
AUX_2,14
;
.equ
AUX_3,12
;
.equ
AUX_4,13
;
.equ
AUX_5,15
;
;----MAGNETOMETER SPI EQUATES--------------------.equ
MSSPORT,PORTB
;Magnetometer SS Port
.equ
MSSTRIS,TRISB
;Tris
.equ
MSSPIN,3
;pin
.equ
.equ
.equ
MRESPORT,PORTB
MRESTRIS,TRISB
MRESPIN,5
;Magnetometer Reset Port
;Tris
;pin
.equ
.equ
.equ
MDRDYPORT,PORTB
MDRDYTRIS,TRISB
MDRDYPIN,4
;Magnetometer DRDY Port
;Tris
;pin
.equ
.equ
.equ
MSCLKPORT,PORTB
MSCLKTRIS,TRISB
MSCLKPIN,0
;Magnetometer SCLK Port
;Tris
;pin
125
.equ
.equ
.equ
MMOSIPORT,PORTB
MMOSITRIS,TRISB
MMOSIPIN,2
;Magnetometer MOSI Port
;Tris
;pin
.equ
.equ
.equ
MMISOPORT,PORTB
MMISOTRIS,TRISB
MMISOPIN,1
;Magnetometer MISO Port
;Tris
;pin
;..............................................................................
;Global Declarations:
;..............................................................................
;
.global __reset
.global _main
;The label for the first line of code.
;required to invoke data initialization
.global
.global
.global
.global
.global
;Declare UART 2 Interrupt
;Declare UART 1 Interrupt
;
;
;
__U2RXInterrupt
__U1RXInterrupt
__SPI2Interrupt
__SPI2ErrInterrupt
__INT4Interrupt
;.global __T1Interrupt
;Declare Timer 1 ISR name global
;..............................................................................
;Constants stored in Program space
;..............................................................................
;
.section .myconstbuffer, code
;
.palign 2
;Align next word stored in Program space to an
;
;address that is a multiple of 2
;ps_coeff:
;
.hword
0x0002, 0x0003, 0x0005, 0x000A
;..............................................................................
;Uninitialized variables in X-space in data memory
;..............................................................................
.section .xbss, bss, xmemory
GPSMSG1: .space 80
;First GPS Message Buffer
GPSMSG2: .space 80
;Second GPS Message Buffer
GPSBUF1CNT: .space
2
;Buffer 1 Byte Count
GPSBUF2CNT: .space
2
;Buffer 2 Byte Count
GPSFLAGS: .space 2
;GPS Flags
;bit 0: 1 = Message in progress , 0 = Message not in progress
;bit 1: 1 = Buffer Full Error , 0 = No Buffer Full Error
;bit 2: 1 = buffer 1 is full , 0 = buffer 1 is available
;bit 3: 1 = buffer 1 requires writing , 0 = buffer 1 does not require writing
;bit 4: 1 = buffer 2 is full , 0 = buffer 2 is available
;bit 5: 1 = buffer 2 requires writing , 0 = buffer 2 does not require writing
;bit 6: 1 = using buffer 1, 0 = using buffer 2
;bit 7:
1 = GPS Data Written, 0 = No GPS Data Written
U1RECEIVE: .space 2
;received byte storage
XMAG:
.space 2
;X-axis magnetometer measurement
YMAG:
.space 2
;Y-axis magnetometer measurement
ZMAG:
.space 2
;Z-axis magnetometer measurement
TEMP:
.space 2
;
XTilt: .space 2
;
YTilt: .space 2
;
XAccel: .space 2
;
YAccel: .space 2
;t
SPIRNUM:
.space 2
;Number of Bytes Received via SPI
SPITNUM:
.space 2
;Number of Bytes to transmit via SPI
126
SPIFLAGS:
.space 2
;SPI Flags
SPITRANSMIT:
.space 30
;SPI Transmit Buffer
SPIRECEIVE: .space 20
;SPI Receive Buffer
AP_COMMAND: .space 2
;
AP_RANGEH: .space 2
;
AP_RANGEL: .space 2
;
AP_TRANS1: .space 2
;
AP_TRANS2: .space 2
;
AP_TRANS3: .space 2
;
AP_TRANSORDER: .space 2
;
AP_TPNC:
.space 2
;default #45 (1ms)
AP_PRE1:
.space 2
;default 0x007F
AP_PRE2:
.space 2
;default 0x007F
AP_PRE3:
.space 2
;default 0x007F
AP_POST1:
.space 2
;default 0x007F
AP_POST2:
.space 2
;default 0x007F
AP_POST3:
.space 2
;default 0x007F
AP_BIGTOH: .space 2
;default 0x0131
AP_BIGTOL: .space 2
;default 0x2D00
AP_SMALLTOH:
.space 2
;default 0x0000
AP_SMALLTOL:
.space 2
;default 0x4E20
MAIN_FLAGS:
.space 2
;bit 0: 1 = 1PPS Has Occurred 0 = 1PPS Hasn't Occurred Yet
;bit 1: 1 = Debug Mode, 0 = Not Debug Mode
;bit 2: 1 = Debug Mode (Don't Wait for PPS), 0 = Wait for PPS
;bit 3: 1 = Record Data, 0 = Don't Record Data
;bit 4: 1 = Send Data out to Computer on Store, 0 = Don't send data out
;bit 5: 1 = Echo RS232 Characters, 0 = Don't Echo
;bit 6: 1 = GPS COM MODE, 0 = Not GPS COM MODE
;bit 7: 1 = Read Incoming GPS Data, 0 = Do NOT read incoming gps data
;bit 8: 1 = Datalog COM Mode, 0 = Not Datalog COM Mode
;bit 9: 1 = Bluetooth Connected, 0 = Bluetooth Not Connected
AP_STIMEH: .space 2
;
AP_STIMEL: .space 2
;
AP_ETIMEH: .space 2
;
AP_ETIMEL: .space 2
;
TILT_TIMEH: .space 2
;
TILT_TIMEL: .space 2
;
MAG_TIMEH: .space 2
;
MAG_TIMEL: .space 2
;
MAG_CSV:
TILT_CSV:
AP_CSV:
TIME_CSV:
UI2NUMT:
.space
.space
.space
.space
30
40
40
50
.space 10
U2RECEIVE: .space 30
U2RNUM:
.space 2
U2FLAGS:
.space 2
;
;
;
;
;
;UART 2 Received words
;UART 2 Received Number of words
;UART 2 FLAGS
;bit 0 - Interrupt Occured Flag? 1= No, 0= Yes
;bit 1: 1 = Last data sent to datalogger, 0 = Last
;data sent to computer
TESTH:
.space 2
TESTL:
.space 2
;..............................................................................
;Uninitialized variables in Y-space in data memory
;..............................................................................
;
;y_input:
.section .ybss, bss, ymemory
.space 2*SAMPLES
127
;..............................................................................
;Initialized variables in data memory
;..............................................................................
;----Debug Display Constants-----------------------------.section .data
DEBUG_MAG: .asciz "MAG X
Y
Z "
DEBUG_MAGT: .asciz "
MAGTIME: "
DEBUG_TILT: .ascii "TILT X
Y
"
DEBUG_TILTT: .ascii " TILTTIME: "
DEBUG_A1: .ascii "AP - RANGE: "
DEBUG_A2: .ascii "
ORDER: "
DEBUG_A3: .ascii "
TRAN1: "
DEBUG_A4: .ascii "
TRAN2: "
DEBUG_A5: .ascii "
TRAN3: "
DEBUG_A6: .ascii "
STIME: "
DEBUG_A7: .ascii "
ETIME: "
DEBUG_CMD: .ascii "'q' - Exit Debug Mode 's' - Setup Parameters 'g' - GPS COM Mode
'l' - Loopback Mode 'o' - Datalog Mode 'm' - OS DL "
MAIN_CMD: .ascii "'r' - RECORDPAUSE 's' - STARTSTOP Debug Out 'd' - Debug Mode "
UI2NUMR:
.ascii "Please Enter a 00 digit number. "
UI2NUMERR: .ascii "Number is too large, try again"
VALDISP1: .ascii "1 AP_PRE1: XXX
[MAX: 127]"
VALDISP2: .ascii "2 AP_PRE2: XXX
[MAX: 127]"
VALDISP3: .ascii "3 AP_PRE3: XXX
[MAX: 127]"
VALDISP4: .ascii "4 AP_POST1: XXX
[MAX: 127]"
VALDISP5: .ascii "5 AP_POST2: XXX
[MAX: 127]"
VALDISP6: .ascii "6 AP_POST3: XXX
[MAX: 127]"
VALDISP7: .ascii "7 AP_TPNC: XXXXX
[65535]"
VALDISP8: .ascii "8 AP_BIGTO: XXXXXXXXXX [MAX]"
VALDISP9: .ascii "9 AP_SMLTO: XXXXXXXXXX[FFFF]"
DISPVAL_INS: .ascii "'q' - Exit (1-9) - Change Parameter "
GPSBUFFULL: .ascii "ERROR: GPS BUFFER FULL"
GPSDATAERR: .ascii "ERROR: GPS RX DATA"
GPSPPS:
.ascii "[PPS]"
U1ORERR:
.ascii "ERROR: U1OR "
U1FRERR:
.ascii "ERROR: U1FE "
U2ORERR:
.ascii "ERROR: U2OR "
U2FRERR:
.ascii "ERROR: U2FE "
;GPSSETRATE: .ascii "$PSRF103,00,00,01,01*25",<0xd>,<0xa>
;
;..............................................................................
;Uninitialized variables in Near data memory (Lower 8Kb of RAM)
;..............................................................................
;
.section .nbss, bss, near
.section vars,bss,address(0x900)
;..............................................................................
;Code Section in Program Memory
;..............................................................................
.text
;Start of Code section
__reset:
MOV
#__SP_init, W15
;Initalize the Stack Pointer
MOV
#__SPLIM_init, W0
;Initialize the Stack Pointer Limit Register
MOV
W0, SPLIM
;
NOP
;Add NOP to follow SPLIM initialization
_main:
;Initialize PLL to proper frequency
;PLL will work on startup if 4mHz<Fin<8mHz
;(it was checked that PLL settings will within
;limits during this PLL update process)
mov
#0x00C0,W0
;PLLPRE=2,PLLPOST=8 (IC = 8mhz)
mov
W0,CLKDIV
;
128
mov
mov
#0x003E,W0
W0,PLLFBD
;Set PLL Multiplier=64 (IC = 8mhz)
;
;Disable all modules
mov
#0xFFFF,W0
mov
W0,PMD1
mov
W0,PMD2
mov
W0,PMD3
;
;
;
;
;Disable all open-drains
mov
#0x0000,W0
mov
W0,ODCD
mov
W0,ODCF
mov
W0,ODCG
;
;Port D
;Port F
;Port G
;Disable Analog Function of I/O Pins
mov
#0xFFFF,W0
;
mov
W0,AD1PCFGH
;
mov
W0,AD1PCFGL
;
call
call
call
call
call
call
call
call
call
call
call
call
call
call
call
call
main_init
bt_receive_check
bt_transmit_check
UART1_Init
UART2_Init
gps_init
gps_enable
data_init
data_enable
AP_INIT
SPI_INIT
Magnetometer_Init
Tilt_init
Tilt_Setup
Timer23_init
OnePPS_init
;Initialize UART2 Select pins
;Selects Bluetooth / Computer Reception
;Selects Bluetooth / Computer Transmission
;Initialize GPS
;Initialize RS232 / Bluetooth
;Initialize GPS Enable pins
;Enable GPS
;Initialize Datalogger Pins
;Enable Data Logger
;
;
;Initialize Magnetometer
;Initialize Tilt Sensor
;Setup the tilt sensor
;Initialize 32-bit timer
;Initialize 1PPS Functionality
clr
clr
clr
clr
clr
clr
clr
mov
mov
mov
mov
mov
mov
mov
mov
clr
clr
clr
clr
clr
clr
clr
TEMP
MAIN_FLAGS
AP_RANGEH
AP_RANGEL
AP_TRANS1
AP_TRANS2
AP_TRANS3
#0x0001,W0
W0,AP_STIMEH
#0x0001,W0
W0,AP_STIMEL
#0x0001,W0
W0,AP_ETIMEH
#0x0001,W0
W0,AP_ETIMEL
TILT_TIMEH
TILT_TIMEL
MAG_TIMEH
MAG_TIMEL
U2FLAGS
U2RNUM
GPSFLAGS
;Used for GPS/Magnetometer Flag
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
call
call
delay100ms
delay100ms
;
;
129
call
call
call
call
call
call
call
call
call
call
call
call
call
call
call
call
call
call
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
delay100ms
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;
;******************************************************************************
;******************************************************************************
main_loop:
;Ensure bluetooth data can come through
call
bt_receive
;Selects Bluetooth Reception
call
bt_conn_check
;Check if Bluetooth Connection Is Active, set flag
;accordingly
;Computer Mode or Datalog Mode?
btsc
MAIN_FLAGS,#9
goto
main_comloop
;Check if bluetooth connection is active
;yes, connection is active, computer mode
;no active bluetooth connection, Datalog Mode
;:DATALOG MODE:DATALOG MODE:DATALOG MODE:DATALOG MODE:DATALOG MODE:DATALOG MODE:
;Datalog Mode
call
data_enable
;enable datalogger
call
data_settx
;Select datalogger as TX device
;
;Collect Readings?
btss
MAIN_FLAGS,#0
;Wait for 1PPS Pulse (Indicates GPS Fix)
goto
main_loop
;No 1PPS yet, loop again
;1PPS has occurred, Get sensor data
bset
MAIN_FLAGS,#7
;Read Incoming GPS data
call
FIND_POSITION
;Get Sensor Data
;Store Readings
call
sensor_data_write
;record Sensor
call
delay100ms
;
call
delay100ms
;
call
gps_data_write
;record GPS data
mov
call
mov
call
#0x000D,W0
U2_Transmit
#0x000A,W0
U2_Transmit
;<cr> <lf>
;
;
;
bclr
MAIN_FLAGS,#0
;Reset 1PPS Flag-Wait for another GPS Reading
goto
main_loop
;
;:DATALOG MODE:DATALOG MODE:DATALOG MODE:DATALOG MODE:DATALOG MODE:DATALOG MODE:
;:COMPUTER MODE:COMPUTER MODE:COMPUTER MODE:COMPUTER MODE:COMPUTER MODE:COMPUTER
main_comloop:
call
data_stop
;Stop Data Logger
call
delay1s
;delay
call
data_disable
;Disable Data Logger
call
bt_settx
;Select bluetooth as TX device
130
call
mode_check
btsc
goto
MAIN_FLAGS,#2
main_debug
;Collect Readings?
btss
MAIN_FLAGS,#0
goto
main_loop
bset
MAIN_FLAGS,#7
call
FIND_POSITION
;Store Readings
call
sensor_data_write
call
delay100ms
call
delay100ms
call
gps_data_write
mov
call
mov
call
nop
bclr
goto
;:COMPUTER
;Check
;Debug
;Check
;enter
for change of modes
Mode?
for debug mode
debug mode
;Wait for 1PPS Pulse (Indicates GPS Fix)
;No 1PPS yet, loop again
;1PPS has occurred, Get sensor data
;Read Incoming GPS data
;Get Sensor Data
;record Sensor
;
;
;record GPS data
#0x000D,W0
U2_Transmit
#0x000A,W0
U2_Transmit
;<cr> <lf>
;
;
;
;store other data
MAIN_FLAGS,#0
;Reset 1PPS Flag - Wait for another GPS Reading
main_loop
;
MODE:COMPUTER MODE:COMPUTER MODE:COMPUTER MODE:COMPUTER MODE:COMPUTER
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
main_sdo:
call
HOME_DISPLAY
;Reset Cursor Position
call
ODEBUG_DISPLAY
;Display Orientation Data
call
ADEBUG_DISPLAY
;Display Acoustic Data
call
OT_DISPLAY
;Display Orientation Time
return
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
mode_check:
cp0
U2RNUM
;Check if any commands received
bra
NZ,mode_check_cmd
;
return
;no, command, return
mode_check_cmd:
clr
U2RNUM
;Clear receive byte count
mov
cp.b
bra
#0X0064,W0
U2RECEIVE
Z,mode_check_dbg
;'d' - Debug
;
;Debug Command Received
mov
cp.b
bra
#0X0072,W0
U2RECEIVE
Z,mode_check_rec
;'r' - record
;
;record Command Received
mov
cp.b
bra
#0X0073,W0
U2RECEIVE
Z,mode_check_sd
;'s' - Send out debug data
;
;Send out Debug Data Command Received
;Invalid Command, send out instructions
MAIN_CMD_SO:
call
CLEAR_DISPLAY
;
mov
mov
#MAIN_CMD,W2
#6,W1
;Send out Instruction line
;"'r' - "
131
call
STRING_DISPLAY
;
btsc
MAIN_FLAGS,#3
;RECORDING?
mov
#MAIN_CMD+#12,W2
;yes, display "PAUSE " instead of "RECORD"
mov
#6,W1
;
call
STRING_DISPLAY
;
mov
#MAIN_CMD+#17,W2
;" 's' - "
mov
#7,W1
;
call
STRING_DISPLAY
;
btsc
MAIN_FLAGS,#1
;Sending Data out?
mov
#MAIN_CMD+#29,W2
;yes, display "STOP " instead of "START"
mov
#5,W1
;
call
STRING_DISPLAY
;
mov
#MAIN_CMD+#33,W2
;" Data Out 'd' - Debug Mode"
mov
#27,W1
;
call
STRING_DISPLAY
;
mov
#0x0001,W1
;New line
call
NEWLINE_DISPLAY
;
return
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
mode_check_dbg:
bset
MAIN_FLAGS,#2
;set debug mode flag
return
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
mode_check_rec:
btg
MAIN_FLAGS,#3
;set record mode flag
call
MAIN_CMD_SO
;update display
return
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
mode_check_sd:
btg
MAIN_FLAGS,#1
;set send data mode flag
call
MAIN_CMD_SO
;update display
return
;
;******************************************************************************
;******************************************************************************
main_debug:
call
CLEAR_DISPLAY
;
bclr
MAIN_FLAGS,#7
;Don't store GPS data
bclr
IEC0,#U1RXIE
;Clear U1 Recieve Interrupt
bclr
MAIN_FLAGS,#1
;Turn off [PPS] Message Output
main_debugloop:
call
bt_receive_check
;Selects Bluetooth / Computer Reception
call
FIND_POSITION
;Get Sensor Data
call
call
call
call
HOME_DISPLAY
ODEBUG_DISPLAY
ADEBUG_DISPLAY
OT_DISPLAY
;Reset Cursor Position
;Display Orientation Data
;Display Acoustic Data
;Display Orientation Time
mov
call
#0x0002,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_CMD,W2
#114,W1
STRING_DISPLAY
;Send out Instruction line
;
;
cp0
bra
U2RNUM
NZ,debug_command
;Check if any commands received
;
132
goto
main_debugloop
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
debug_command:
clr
U2RNUM
;Clear receive byte count
mov
cp.b
bra
#0X0071,W0
U2RECEIVE
Z,cmdquitdebug
;'q' - Quit
;
;Quit Command Received
mov
cp.b
bra
#'s',W0
U2RECEIVE
Z,cmddispval
;'s' - display setup values for editing
;
;Display Values Command
mov
cp.b
bra
#'g',W0
U2RECEIVE
Z,cmdgpscom
;'g' - Enter GPS communication mode
;(can only exit by restarting microcontroller)
;GPS COM Mode Command
mov
cp.b
bra
#'l',W0
U2RECEIVE
Z,cmdlpbcom
;'l' - Enter Loopback Mode
;(can only exit by restarting microcontroller)
;Loopback COM Mode Command
mov
cp.b
bra
#'o',W0
U2RECEIVE
Z,cmddlcom
;'o' - Enter Datalog Mode
;(can only exit by restarting microcontroller)
;Datalog Mode Command
mov
cp.b
bra
#'m',W0
U2RECEIVE
Z,cmdosdlcom
;'m' - Enter Orientation Sensor Datalog Mode
;
;Orientation Sensor Datalog Mode Command
goto
main_debug
;Not a valid command
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmdosdlcom:
bclr
MAIN_FLAGS,#5
;Turn off character echo
call
data_enable
;enable datalogger
call
delay1s
;Allow time for the datalogger to turn on
cmdosdlcom_loop:
call
delay100ms
;Delay to lower sample rate
call
delay100ms
;to approximately 3 Hz
;Get Time
call
Timer23_read
;Time Stamp
mov
W1,MAG_TIMEH
;
mov
W0,MAG_TIMEL
;
;
call
Mag_Read_XYZ
;Read Heading Information
call
Tilt_Read
;Read Tilt Information
call
Tilt_Data_Mask
;
call
MAG_DF
;Store magnetometer CSV data
call
TILT_DF
;Store Tilt CSV data
call
TIME_DF
;Store Time CSV data
call
data_settx
;Select datalogger as TX device
;Send out Orientation Sensor Data
mov
#0x0024,W0
;'$ORSEN'
call
U2_Transmit
;
mov
#0x004F,W0
;
call
U2_Transmit
;
mov
#0x0052,W0
;
call
U2_Transmit
;
mov
#0x0053,W0
;
call
U2_Transmit
;
133
mov
call
mov
call
#0x0045,W0
U2_Transmit
#0x004E,W0
U2_Transmit
;
;
;
;
mov
call
mov
call
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
;', ' (Comma,Space)
;
;
;
mov
mov
call
#TIME_CSV+#36,W2
#0x000A,W1
STRING_DISPLAY
;Send out Time Stamp
;
;
mov
call
mov
call
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
;', ' (Comma,Space)
;
;
;
mov
mov
call
#MAG_CSV,W2
#22,W1
STRING_DISPLAY
;Send out Mag Data
;
;
mov
call
mov
call
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
;', ' (Comma,Space)
;
;
;
mov
mov
call
#TILT_CSV,W2
#14,W1
STRING_DISPLAY
;Send out Tilt Data
;
;
mov
call
mov
call
#0x000D,W0
U2_Transmit
#0x000A,W0
U2_Transmit
;CR, LF
;
;
;
;Done sending out data
cp0
U2RNUM
bra
NZ,cmdosdlcom_exit
goto
cmdosdlcom_loop
;Check if any key has been pressed
;yes, exit this loop.. but first save data
;no, repeat procedure
cmdosdlcom_exit:
clr
U2RNUM
;clear recieved byte
call
delay100ms
;
call
data_stop
;Stop Data Logger
call
delay1s
;delay
call
data_disable
;Disable Data Logger
call
bt_settx
;Select bluetooth as TX device
goto
main_debug
;Go back to debug mode
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmdgpscom:
bclr
MAIN_FLAGS,#5
;Turn off character echo
nop
;
bset
MAIN_FLAGS,#6
;Turn on GPS COM MODE
clr
U2RNUM
;
bset
IEC0,#U1RXIE
;Turn On U1 Recieve Interrupt
goto
cmdgpscom
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmdlpbcom:
bset
MAIN_FLAGS,#5
;Turn on character echo
clr
U2RNUM
;
134
goto
cmdlpbcom
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddlcom:
bclr
MAIN_FLAGS,#5
;Turn off character echo
call
data_enable
;enable datalogger
call
delay1s
;Allow datalogger time to turn on
bset
MAIN_FLAGS,#8
;Turn on datalog mode
clr
U2RNUM
;
goto
cmddlcom
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddispval:
clr
U2RNUM
;clear received bytes count
call
CLEAR_DISPLAY
;
call
VAL_DISPLAY
;Display values
mov
call
#0x0002,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DISPVAL_INS,W2
#36,W1
STRING_DISPLAY
;Send out Instruction line
;
;
bset
MAIN_FLAGS,#5
cmddispval_wait:
cp0
U2RNUM
bra
Z,cmddispval_wait
;Echo Characters
clr
U2RNUM
;Check if any commands received
;
;New byte received
;Clear receive byte count
mov
cp.b
bra
#'q',W0
U2RECEIVE
Z,cmddisp_quit
;'q' - Quit
;
;Quit Command Received, back to debug
mov
cp.b
bra
#'1',W0
U2RECEIVE
Z,cmddisp_op1
;'1' - AP_PRE1
;
;Option 1 Selected
mov
cp.b
bra
#'2',W0
U2RECEIVE
Z,cmddisp_op2
;'2' - AP_PRE2
;
;Option 2 Selected
mov
cp.b
bra
#'3',W0
U2RECEIVE
Z,cmddisp_op3
;'3' - AP_PRE3
;
;Option 3 Selected
mov
cp.b
bra
#'4',W0
U2RECEIVE
Z,cmddisp_op4
;'4' - AP_POST1
;
;Option 4 Selected
mov
cp.b
bra
#'5',W0
U2RECEIVE
Z,cmddisp_op5
;'5' - AP_POST2
;
;Option 5 Selected
mov
cp.b
bra
#'6',W0
U2RECEIVE
Z,cmddisp_op6
;'6' - AP_POST3
;
;Option 6 Selected
mov
cp.b
bra
#'7',W0
U2RECEIVE
Z,cmddisp_op7
;'7' - AP_TPNC
;
;Option 7 Selected
mov
cp.b
#'8',W0
U2RECEIVE
;'8' - AP_BIGTO
;
135
bra
Z,cmddisp_op8
;Option 8 Selected
mov
cp.b
bra
#'9',W0
U2RECEIVE
Z,cmddisp_op9
;'9' - AP_SMALLTO
;
;Option 9 Selected
mov
cp.b
bra
#'g',W0
U2RECEIVE
Z,cmddisp_opg
;'g' ;
;
goto
cmddispval
;No valid command received, wait some more
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_quit:
;
call
AP_SP
;Send out acoustic board parameters
call
delay100ms
;give slight delay for acoustic processor
goto
main_debug
;return to debug menu
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_opg:
call
AP_SP
; 'g' Save parameters to Acoustic Board
goto
cmddispval
;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op1:
mov
#3,W0
;3-Digits requested
mov
#0,W2
;Max Allowable Value (MSW)
mov
#127,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_PRE1
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op2:
mov
#3,W0
;3-Digits requested
mov
#0,W2
;Max Allowable Value (MSW)
mov
#127,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_PRE2
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op3:
mov
#3,W0
;3-Digits requested
mov
#0,W2
;Max Allowable Value (MSW)
mov
#127,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_PRE3
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op4:
mov
#3,W0
;3-Digits requested
mov
#0,W2
;Max Allowable Value (MSW)
mov
#127,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_POST1
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op5:
mov
#3,W0
;3-Digits requested
mov
#0,W2
;Max Allowable Value (MSW)
mov
#127,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_POST2
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op6:
mov
#3,W0
;3-Digits requested
136
mov
#0,W2
;Max Allowable Value (MSW)
mov
#127,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_POST3
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op7:
mov
#5,W0
;5-Digits requested
mov
#0,W2
;Max Allowable Value (MSW)
mov
#0xFFFF,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_TPNC
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op8:
mov
#10,W0
;10-Digits requested
mov
#0xFFFF,W2
;Max Allowable Value (MSW)
mov
#0xFFFF,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_BIGTOL
;store new value
mov
W1,AP_BIGTOH
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmddisp_op9:
mov
#10,W0
;10-Digits requested
mov
#0xFFFF,W2
;Max Allowable Value (MSW)
mov
#0xFFFF,W1
;Max Allowable Value (LSW)
call
UI2NUM
;Collect new user value
mov
W0,AP_SMALLTOL
;store new value
mov
W1,AP_SMALLTOH
;store new value
goto
cmddispval
;display new value
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
cmdquitdebug:
bclr
MAIN_FLAGS,#2
;clear debug mode flag
bclr
MAIN_FLAGS,#5
;Turn off character echo
bset
IEC0,#U1RXIE
;Turn On U1 Recieve Interrupt
call
MAIN_CMD_SO
;Send out Main CMD
goto
main_loop
;
;******************************************************************************
;******************************************************************************
;*********************************************************
;FIND_POSITION Find Position Routine
;
;Requests reading from acoustic processor, reads all
;orientation sensors, formats orientation and acoustic
;processor readings as CSVs in data memory. All data
;is time stamped with a count since the last 1PPS. Clears
;1PPS flag before returning.
;*********************************************************
FIND_POSITION:
call
mov
mov
call
Timer23_read
W1,AP_STIMEH
W0,AP_STIMEL
AP_GM
;Time Stamp
;
;
;Request Range and Bearing from AP Board
call
mov
mov
call
Timer23_read
W1,MAG_TIMEH
W0,MAG_TIMEL
Mag_Read_XYZ
;Time Stamp
;
;
;Read Heading Information
call
Timer23_read
;Time Stamp
137
mov
mov
call
call
call
call
W1,TILT_TIMEH
W0,TILT_TIMEL
Tilt_Read
Tilt_Data_Mask
MAG_DF
TILT_DF
;
;
;Read Tilt Information
;
;Store magnetometer CSV data
;Store Tilt CSV data
call
call
mov
mov
AP_GM_READ
Timer23_read
W1,AP_ETIMEH
W0,AP_ETIMEL
;Wait for then collect AP Measurements
;Time Stamp
;
;
call
AP_DF
;Store AP CSV data
call
TIME_DF
;Store Time CSV data
return
;
;*********************************************************
;*********************************************************
;UI2NUM User Input to Number
;
;Given the number of digits to request and a maximum
;allowable value this function will prompt the user for
;input and then convert the ASCII BCD to a binary number.
;Inputs: W0 - Number of digits to request
;
W1 - Maximum allowable value (LSW)
;
W2 - Maximum allowable value (MSW)
;Output: W0 - binary number (16-bit Word, LSW if 32-bit)
;
W1 - binary number (MSW if 32-bit)
;*********************************************************
UI2NUM:
;Make request for proper number of digits
push
W1
;Save inputs
push
W2
;
push
W0
;
clr
U2RNUM
;clear received byte counter
call
num2ascii
;Convert # of digits to ASCII
mov
#UI2NUMR+#15,W3
;Prepare output phrase
mov.b W2,[W3++]
;Place tens digit
mov.b W1,[W3++]
;Place Ones digit
mov
call
#0x0002,W1
NEWLINE_DISPLAY
;2 New lines
;
mov
mov
call
mov
call
#UI2NUMR,W2
#31,W1
STRING_DISPLAY
#0x0002,W1
NEWLINE_DISPLAY
;Send out request to user
;
;
;2 New lines
;
pop
push
W0
W0
;Restore number of digits
;save number of digits once again
bset
MAIN_FLAGS,#5
UI2NUM_wait:
cp
U2RNUM
bra
LTU,UI2NUM_wait
;Echo Characters to User
;Wait for requested number of digits
;
;not enough bytes yet, wait
bclr
MAIN_FLAGS,#5
;Stop Echoing Characters to User
mov
sub
#10,W2
W2,W0,W2
;10 = Max number of digits
;10 - number of digits => W2
;Fill UI2NUMT with 10 digit string (fill in missing digits with 0x00)
138
mov
#UI2NUMT,W3
UI2NUM_FC:
cp0
W2
bra
NZ,UI2NUM_SF
;
pop
W2
push
W2
mov
#U2RECEIVE,W4
UI2NUM_DC:
cp0
W2
bra
NZ,UI2NUM_DF
;restore number of digits
;
;
;Fill digits in array
;Check if there are still digits
;yes, fill one
;Check if there are still missing digits
;yes, fill one
clr
U2RNUM
;
bclr
SR,#15
;Clear OA bit
;Should now have a string of 10 BCD digits in UI2NUMT
mov
#UI2NUMT,W3
;
clr
A
;Clear accumulator
;Billions digit
mov
#0x3B9A,W0
;(1,000,000,000 in hex)
mov
W0,ACCBH
;
mov
#0xCA00,W0
;
mov
W0,ACCBL
;
mov.b [W3++],W0
;
UI2NUM_BDT:
;
cp0.b W0
;Check for Billions
bra
Z,UI2NUM_HMD
;no,billions, check hundred millions
add
A
;add one billion
dec.b W0,W0
;
bra
UI2NUM_BDT
;
UI2NUM_HMD:
mov
#0x05F5,W0
mov
W0,ACCBH
mov
#0xE100,W0
mov
W0,ACCBL
mov.b [W3++],W0
UI2NUM_HMT:
cp0.b W0
bra
Z,UI2NUM_TMD
add
A
dec.b W0,W0
bra
UI2NUM_HMT
;Hundred Millions Digit
;(100,000,000 in hex)
;
;
;
;
;
;Check for Billions
;no hundred millions, check ten millions
;add one hundred million
;
;
UI2NUM_TMD:
mov
#0x0098,W0
mov
W0,ACCBH
mov
#0x9680,W0
mov
W0,ACCBL
mov.b [W3++],W0
UI2NUM_TMT:
cp0.b W0
bra
Z,UI2NUM_MD
add
A
dec.b W0,W0
bra
UI2NUM_TMT
;Ten Millions Digit
;(10,000,000 in hex)
;
;
;
;
;
;Check for Ten Millions
;no ten millions, check millions
;add one ten million
;
;
UI2NUM_MD:
mov
#0x000F,W0
mov
W0,ACCBH
mov
#0x4240,W0
mov
W0,ACCBL
mov.b [W3++],W0
UI2NUM_MT:
;Millions Digit
;(1,000,000 in hex)
;
;
;
;
;
139
cp0.b
bra
add
dec.b
bra
W0
Z,UI2NUM_HTD
A
W0,W0
UI2NUM_MT
;Check for Millions
;no millions, check hundred thousands
;add one million
;
;
UI2NUM_HTD:
mov
#0x0001,W0
mov
W0,ACCBH
mov
#0x86A0,W0
mov
W0,ACCBL
mov.b [W3++],W0
UI2NUM_HTT:
cp0.b W0
bra
Z,UI2NUM_TTD
add
A
dec.b W0,W0
bra
UI2NUM_HTT
;Hundred Thousands Digit
;(100,000 in hex)
;
;
;
;
;
;Check for Hundred Thousands
;no hundred thousands, check tens of thousands
;add one hundred thousand
;
;
UI2NUM_TTD:
mov
#0x0000,W0
mov
W0,ACCBH
mov
#0x2710,W0
mov
W0,ACCBL
mov.b [W3++],W0
UI2NUM_TTT:
cp0.b W0
bra
Z,UI2NUM_TD
add
A
dec.b W0,W0
bra
UI2NUM_TTT
;Ten Thousands Digit
;(10,000 in hex)
;
;
;
;
;
;Check for ten Thousands
;no ten thousands, check thousands
;add ten thousand
;
;
UI2NUM_TD:
mov
mov
mov
mov
mov.b
UI2NUM_TT:
cp0.b
bra
add
dec.b
bra
;Thousands Digit
;(1,000 in hex)
;
;
;
;
;
;Check for Thousands
;no thousands, check hundreds
;add thousand
;
;
UI2NUM_HD:
mov
mov
mov
mov
mov.b
UI2NUM_HT:
cp0.b
bra
add
dec.b
bra
#0x0000,W0
W0,ACCBH
#0x03E8,W0
W0,ACCBL
[W3++],W0
W0
Z,UI2NUM_HD
A
W0,W0
UI2NUM_TT
#0x0000,W0
W0,ACCBH
#0x0064,W0
W0,ACCBL
[W3++],W0
W0
Z,UI2NUM_TEND
A
W0,W0
UI2NUM_HT
UI2NUM_TEND:
mov
#0x0000,W0
mov
W0,ACCBH
mov
#0x00A,W0
mov
W0,ACCBL
;Hundreds Digit
;(100 in hex)
;
;
;
;
;
;Check for hundreds
;no hundreds, check tens
;add hundred
;
;
;Tens Digit
;(10 in hex)
;
;
;
140
mov.b [W3++],W0
UI2NUM_TENT:
cp0.b W0
bra
Z,UI2NUM_OD
add
A
dec.b W0,W0
bra
UI2NUM_TENT
;
;
;Check for tens
;no tens, check ones
;add ten
;
;
UI2NUM_OD:
mov
mov
mov
mov
mov.b
UI2NUM_OT:
cp0.b
bra
add
dec.b
bra
;Ones Digit
;(1 in hex)
;
;
;
;
;
;Check for ones
;no ones, check number
;add one
;
;
#0x0000,W0
W0,ACCBH
#0x0001,W0
W0,ACCBL
[W3++],W0
W0
Z,UI2NUM_CHECK
A
W0,W0
UI2NUM_OT
UI2NUM_CHECK:
pop
W0
pop
W3
pop
W2
push
W2
push
W3
push
W0
;Check that number is under max
;num digits
;restore max allowable value (MSW)
;(LSW)
;re-save everything
;in case of error
;
;Check for ACC A overflow
bra
OA,UI2NUM_ERR
;overflow occurred... number too large
mov
mov
ACCAH,W1
ACCAL,W0
;UI Number (MSW)
;UI Number (LSW)
cp
bra
W1,W3
GEU,UI2NUM_CHK2
;UIH < MAXH?
;no, keep checking
UI2NUM_FIN:
pop
W3
pop
W3
pop
W3
return
UI2NUM_CHK2:
cpseq W1,W3
bra
UI2NUM_ERR
cp
bra
W0,W2
LEU,UI2NUM_FIN
UI2NUM_ERR:
mov
#0x0002,W1
call
NEWLINE_DISPLAY
mov
mov
call
#UI2NUMERR,W2
#30,W1
STRING_DISPLAY
;User Input is fine, number returned in W1,W0
;Restore stack
;
;
;
;UIH == MAXH?
;UI > MAX... issue an error and restart
;UIH == MAXH, Check low bytes
;UIL > MAXL?
;no, everything is good
;UI > MAX... issue an error and restart
;2 New lines
;
;Send out error message
;
;
bclr
SR,#15
;Clear OA bit
pop
pop
W0
W2
;Restore original values
;
141
pop
W1
goto
UI2NUM
;---------UI2NUM_SF:
Mov
#0x0000,W0
mov.b W0,[W3++]
dec
W2,W2
bra
UI2NUM_FC
;
;rinse and repeat
;0's left to fill
;Fill one digit
;
;decrement 0 fill counter
;check if more filling required
UI2NUM_DF:
;Digits left to fill
mov.b [W4++],W0
;Retrieve one digit
sub.b #0x30,W0
;Get rid of ASCII coding
mov.b W0,[W3++]
;place digit into string
dec
W2,W2
;decrement digit fill counter
bra
UI2NUM_DC
;check if more filling required
;*********************************************************
;*********************************************************
;ODEBUG_DISPLAY Orientation Debug Display Routine
;
;Displays sensor data in easy to read format that is
;output via the UART port.
;*********************************************************
ODEBUG_DISPLAY:
mov
#DEBUG_MAG,W2
;Send out Mag Identifier
mov
#0x001C,W1
;
call
STRING_DISPLAY
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
call
#0x0006,W1
SPACE_DISPLAY
;Spaces
;
mov
mov
call
#MAG_CSV,W2
#0x0016,W1
STRING_DISPLAY
;Send out Mag Data
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_TILT,W2
#0x001C,W1
STRING_DISPLAY
;Send out Tilt Identifier
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
call
#0x0006,W1
SPACE_DISPLAY
;Spaces
;
mov
mov
call
#TILT_CSV,W2
#0x000E,W1
STRING_DISPLAY
;Send out Tilt Data
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
return
;
;*********************************************************
;*********************************************************
;ADEBUG_DISPLAY Acoustic Debug Display Routine
;
;Displays acoustic data in easy to read format that is
142
;output via the UART port.
;*********************************************************
ADEBUG_DISPLAY:
mov
#DEBUG_A1,W2
;Send out Range Identifier
mov
#0x000C,W1
;
call
STRING_DISPLAY
;
mov
mov
call
#AP_CSV,W2
#0x000A,W1
STRING_DISPLAY
;Send out Range Information
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_A2,W2
#0x000C,W1
STRING_DISPLAY
;Send out Order Identifier
;
;
mov
mov
call
#AP_CSV+#0xC,W2
#0x0001,W1
STRING_DISPLAY
;Send out Order Information
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_A3,W2
#0x000C,W1
STRING_DISPLAY
;Send out Tran1 Identifier
;
;
mov
mov
call
#AP_CSV+#0xF,W2
#0x0005,W1
STRING_DISPLAY
;Send out Tran1 Information
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_A4,W2
#0x000C,W1
STRING_DISPLAY
;Send out Tran2 Identifier
;
;
mov
mov
call
#AP_CSV+#0x16,W2
#0x0005,W1
STRING_DISPLAY
;Send out Tran2 Information
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_A5,W2
#0x000C,W1
STRING_DISPLAY
;Send out Tran3 Identifier
;
;
mov
mov
call
#AP_CSV+#0x1D,W2
#0x0005,W1
STRING_DISPLAY
;Send out Tran3 Information
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_A6,W2
#0x000C,W1
STRING_DISPLAY
;Send out Stime Identifier
;
;
mov
#TIME_CSV,W2
;Send out Stime Information
143
mov
call
#0x000A,W1
STRING_DISPLAY
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_A7,W2
#0x000C,W1
STRING_DISPLAY
;Send out Etime Identifier
;
;
mov
mov
call
#TIME_CSV+#0x0C,W2
#0x000A,W1
STRING_DISPLAY
;Send out Etime Information
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
return
;
;*********************************************************
;*********************************************************
;OT_DISPLAY Orientation Time Display Routine
;
;Displays sensor time data in easy to read format that is
;output via the UART port.
;*********************************************************
OT_DISPLAY:
mov
#DEBUG_MAGT,W2
;Send out Mag Time Identifier
mov
#0x000C,W1
;
call
STRING_DISPLAY
;
mov
mov
call
#TIME_CSV+#36,W2
#0x000A,W1
STRING_DISPLAY
;Send out Mag Time Data
;
;
mov
call
#0x0001,W1
NEWLINE_DISPLAY
;New line
;
mov
mov
call
#DEBUG_TILTT,W2
#0x000C,W1
STRING_DISPLAY
;Send out Tilt Time Identifier
;
;
mov
mov
call
#TIME_CSV+#24,W2
#0x000A,W1
STRING_DISPLAY
;Send out Tilt Time Data
;
;
return
;
;*********************************************************
;*********************************************************
;VAL_DISPLAY Setup Values Display Routine
;
;Displays setup values in easy to read format that is
;output via the UART port.
;*********************************************************
VAL_DISPLAY:
call
VAL_DF
;Update display strings with current values
mov
mov
call
mov
call
#VALDISP1,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;Send out first line
;
;
;New line
;
mov
#VALDISP2,W2
;Send out next line
144
mov
call
mov
call
mov
mov
call
mov
call
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
#VALDISP3,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;
;
;New line
;
;Send out next line
;
;
;New line
;
mov
mov
call
mov
call
#VALDISP4,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;Send out next line
;
;
;New line
;
mov
mov
call
mov
call
#VALDISP5,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;Send out next line
;
;
;New line
;
mov
mov
call
mov
call
#VALDISP6,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;Send out next line
;
;
;New line
;
mov
mov
call
mov
call
#VALDISP7,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;Send out next line
;
;
;New line
;
mov
mov
call
mov
call
#VALDISP8,W2
#28,W1
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
;Send out next line
;
;
;New line
;
mov
mov
call
mov
call
#VALDISP9,W2
#28,W1
STRING_DISPLAY
#0x0002,W1
NEWLINE_DISPLAY
;Send out last line
;
;
;New line
;
return
;
;*********************************************************
;*********************************************************
;VAL_DF Setup Values Data Format
;
;Updates setup value display strings with the current
;values of the setup parameters.
;*********************************************************
VAL_DF:
mov
#VALDISP1+#12,W13
;load W13 starting memory location
mov
AP_PRE1,W0
;Retrieve current value
call
num2ascii
;Convert Number to ASCII Character Array
mov.b
W3,[W13++]
;Insert current value into display string
mov.b
W2,[W13++]
;
mov.b
W1,[W13++]
;
mov
#VALDISP2+#12,W13
;load W13 starting memory location
145
mov
call
mov.b
mov.b
mov.b
AP_PRE2,W0
num2ascii
W3,[W13++]
W2,[W13++]
W1,[W13++]
;Retrieve current value
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
mov
mov
call
mov.b
mov.b
mov.b
#VALDISP3+#12,W13
AP_PRE3,W0
num2ascii
W3,[W13++]
W2,[W13++]
W1,[W13++]
;load W13 starting memory location
;Retrieve current value
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
mov
mov
call
mov.b
mov.b
mov.b
#VALDISP4+#12,W13
AP_POST1,W0
num2ascii
W3,[W13++]
W2,[W13++]
W1,[W13++]
;load W13 starting memory location
;Retrieve current value
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
mov
mov
call
mov.b
mov.b
mov.b
#VALDISP5+#12,W13
AP_POST2,W0
num2ascii
W3,[W13++]
W2,[W13++]
W1,[W13++]
;load W13 starting memory location
;Retrieve current value
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
mov
mov
call
mov.b
mov.b
mov.b
#VALDISP6+#12,W13
AP_POST3,W0
num2ascii
W3,[W13++]
W2,[W13++]
W1,[W13++]
;load W13 starting memory location
;Retrieve current value
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
mov
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
#VALDISP7+#12,W13
AP_TPNC,W0
num2ascii
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
;load W13 starting memory location
;Retrieve current reading
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
;
;
mov
mov
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
#VALDISP8+#12,W13
AP_BIGTOH,W1
AP_BIGTOL,W0
bnum2ascii
W10,[W13++]
W9,[W13++]
W8,[W13++]
W7,[W13++]
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
;load W13 starting memory location
;Retrieve current reading
;
;Convert Number to ASCII Character Array
;Insert current value into display string
;
;
;
;
;
;
;
;
;
mov
mov
mov
call
mov.b
#VALDISP9+#12,W13
AP_SMALLTOH,W1
AP_SMALLTOL,W0
bnum2ascii
W10,[W13++]
;load W13 starting memory location
;Retrieve current reading
;
;Convert Number to ASCII Character Array
;Insert current value into display string
146
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
W9,[W13++]
W8,[W13++]
W7,[W13++]
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
;
;
;
;
;
;
;
;
;
return
;
;*********************************************************
;*********************************************************
;STRING_DISPLAY String Display Routine
;
;Sends a string out the UART. The string starting
;location is passed in W2, the number of charactors to
;send out is passed in W1.
;*********************************************************
STRING_DISPLAY:
cp0
W1
;Check if there are still
bra
NZ,SD_SENDOUT
;characters left to send
return
;no
SD_SENDOUT:
;Characters left to send out
mov.b
[W2++],W0
;Send character out UART2
call
U2_Transmit
;
dec
W1,W1
;
bra
STRING_DISPLAY
;
;*********************************************************
;*********************************************************
;SPACE_DISPLAY Space Display Routine
;
;Sends a specified number of space characters out through
;UART2. The number of spaces is passed through W1.
;*********************************************************
SPACE_DISPLAY:
cp0
W1
;Check if there are still
bra
NZ,SPD_SENDOUT
;spaces left to send
return
;no
SPD_SENDOUT:
;Spaces left to send out
mov
#0x0020,W0
;Send space out UART2
call
U2_Transmit
;
dec
W1,W1
;
bra
SPACE_DISPLAY
;
;*********************************************************
;*********************************************************
;CLEAR_DISPLAY Clear Display Routine
;
;Sends out the Clear display ANSI command <ESC>[2J
;*********************************************************
CLEAR_DISPLAY:
mov
#0x001B,W0
;Send ESC out UART2
call
U2_Transmit
;
mov
#0x005B,W0
;Send [ out UART2
call
U2_Transmit
;
mov
#0x0032,W0
;Send 2 out UART2
call
U2_Transmit
;
mov
#0x004A,W0
;Send J out UART2
call
U2_Transmit
;
return
;no
;*********************************************************
;*********************************************************
;NEWLINE_DISPLAY New Line Display Routine
147
;
;Sends a specified number of new Line characters out through
;UART2. The number of new lines is passed through W1.
;*********************************************************
NEWLINE_DISPLAY:
cp0
W1
;Check if there are still
bra
NZ,NL_SENDOUT
;new lines left to send
return
;no
NL_SENDOUT:
;new lines left to send out
mov
#0x000D,W0
;CR
call
U2_Transmit
;
mov
#0x000A,W0
;LF
call
U2_Transmit
;
dec
W1,W1
;
bra
NEWLINE_DISPLAY
;
;*********************************************************
;*********************************************************
;HOME_DISPLAY Put Cursor to Home Position Routine
;
;Sends out the Home ANSI command ESC[#;#H
;*********************************************************
HOME_DISPLAY:
mov
#0x001B,W0
;Send ESC out UART2
call
U2_Transmit
;
mov
#0x005B,W0
;Send [ out UART2
call
U2_Transmit
;
mov
#0x0030,W0
;Send 0 out UART2
call
U2_Transmit
;
mov
#0x003B,W0
;Send ; out UART2
call
U2_Transmit
;
mov
#0x0030,W0
;Send 0 out UART2
call
U2_Transmit
;
mov
#0x0048,W0
;Send H out UART2
call
U2_Transmit
;
return
;no
;*********************************************************
;********Data Format Routines*********************************
;*********************************************************
;MAG_DF Magnetometer Data Format
;
;Converts magnetometer data into an ASCII Character Array
;and stores the readings as CSVs in data memory
;*********************************************************
MAG_DF:
mov
#MAG_CSV,W13
;load W13 starting memory location
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
XMAG,W0
snum2ascii
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;Retrieve X-axis reading
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
call
mov.b
YMAG,W0
snum2ascii
W6,[W13++]
;Retrieve Y-axis reading
;Convert Number to ASCII Character Array
;
148
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;
;
;
;
;
;', ' (Comma,Space)
;
mov
ZMAG,W0
;Retrieve Z-axis reading
call
snum2ascii
;Convert Number to ASCII Character Array
mov.b
W6,[W13++]
;
mov.b
W5,[W13++]
;
mov.b
W4,[W13++]
;
mov.b
W3,[W13++]
;
mov.b
W2,[W13++]
;
mov.b
W1,[W13++]
;
return
;
;*********************************************************
;*********************************************************
;TILT_DF Tilt Data Format
;
;Converts tilt data into an ASCII Character Array and
;stores the readings as CSVs in data memory
;*********************************************************
TILT_DF:
mov
#TILT_CSV,W13
;load W13 starting memory location
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
XTilt,W0
snum2ascii
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;Retrieve X-tilt reading
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
YTilt,W0
snum2ascii
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;Retrieve Y-Tilt reading
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
XAccel,W0
snum2ascii
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;Retrieve X-Accel reading
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
call
mov.b
YAccel,W0
snum2ascii
W6,[W13++]
;Retrieve Y-Accel reading
;Convert Number to ASCII Character Array
;
149
mov.b
W5,[W13++]
;
mov.b
W4,[W13++]
;
mov.b
W3,[W13++]
;
mov.b
W2,[W13++]
;
mov.b
W1,[W13++]
;
return
;
;*********************************************************
;*********************************************************
;AP_DF Acoustic Processor Data Format
;
;Converts AP data into an ASCII Character Array and
;stores the readings as CSVs in data memory
;*********************************************************
AP_DF:
mov
#AP_CSV,W13
;load W13 starting memory location
mov
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
AP_RANGEH,W1
AP_RANGEL,W0
bnum2ascii
W10,[W13++]
W9,[W13++]
W8,[W13++]
W7,[W13++]
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;Retrieve Range reading
;
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
call
mov.b
mov
mov.b
mov
mov.b
AP_TRANSORDER,W0
num2ascii
W1,[W13++]
#0x002C,W0
W0,[W13++]
#0x0020,W0
W0,[W13++]
;Retrieve TRANSORDER reading
;Convert Number to ASCII Character Array
;(0-7 are only possible values)
;
;', ' (Comma,Space)
;
;
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov.b
mov
mov.b
AP_TRANS1,W0
num2ascii
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x002C,W0
W0,[W13++]
#0x0020,W0
W0,[W13++]
;Retrieve Trans1 reading
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;', ' (Comma,Space)
;
;
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov.b
mov
mov.b
AP_TRANS2,W0
num2ascii
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x002C,W0
W0,[W13++]
#0x0020,W0
W0,[W13++]
;Retrieve Trans2 reading
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;', ' (Comma,Space)
;
;
150
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
AP_TRANS3,W0
num2ascii
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
;Retrieve Trans3 reading
;Convert Number to ASCII Character Array
;
;
;
;
;
return
;
;*********************************************************
;*********************************************************
;TIME_DF TIME Data Format
;
;Converts TIME data into an ASCII Character Array and
;stores the readings as CSVs in data memory
;*********************************************************
TIME_DF:
mov
#TIME_CSV,W13
;load W13 starting memory location
mov
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
AP_STIMEH,W1
AP_STIMEL,W0
bnum2ascii
W10,[W13++]
W9,[W13++]
W8,[W13++]
W7,[W13++]
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;
;
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov
mov
AP_ETIMEH,W1
AP_ETIMEL,W0
bnum2ascii
W10,[W13++]
W9,[W13++]
W8,[W13++]
W7,[W13++]
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;
;
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;
;
;
;
;', ' (Comma,Space)
;
mov
mov
call
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
mov.b
TILT_TIMEH,W1
TILT_TIMEL,W0
bnum2ascii
W10,[W13++]
W9,[W13++]
W8,[W13++]
W7,[W13++]
W6,[W13++]
W5,[W13++]
W4,[W13++]
W3,[W13++]
;
;
;Convert Number to ASCII Character Array
;
;
;
;
;
;
;
;
151
mov.b
mov.b
mov
mov
W2,[W13++]
W1,[W13++]
#0x202C,W0
W0,[W13++]
;
;
;', ' (Comma,Space)
;
mov
MAG_TIMEH,W1
;
mov
MAG_TIMEL,W0
;
call
bnum2ascii
;Convert Number to ASCII Character Array
mov.b
W10,[W13++]
;
mov.b
W9,[W13++]
;
mov.b
W8,[W13++]
;
mov.b
W7,[W13++]
;
mov.b
W6,[W13++]
;
mov.b
W5,[W13++]
;
mov.b
W4,[W13++]
;
mov.b
W3,[W13++]
;
mov.b
W2,[W13++]
;
mov.b
W1,[W13++]
;
return
;
;*********************************************************
;*********************************************************
;sensor_data_write
SENSOR DATA WRITE
;
;Sends out all the sensor data in a comma delimited string
;out the UART2 port
;*********************************************************
sensor_data_write:
mov
#0x0024,W0
;'$UWGPS'
call
U2_Transmit
;
mov
#0x0055,W0
;
call
U2_Transmit
;
mov
#0x0057,W0
;
call
U2_Transmit
;
mov
#0x0047,W0
;
call
U2_Transmit
;
mov
#0x0050,W0
;
call
U2_Transmit
;
mov
#0x0053,W0
;
call
U2_Transmit
;
mov
call
mov
call
mov
mov
call
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
#MAG_CSV,W2
#22,W1
STRING_DISPLAY
;', ' (Comma,Space)
;
;
;
;Send out Mag Data
;
;
mov
call
mov
call
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
;', ' (Comma,Space)
;
;
;
mov
mov
call
#TILT_CSV,W2
#14,W1
STRING_DISPLAY
;Send out Tilt Data
;
;
mov
call
mov
call
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
;', ' (Comma,Space)
;
;
;
152
mov
mov
call
mov
call
mov
call
mov
mov
call
#AP_CSV,W2
#34,W1
STRING_DISPLAY
#0x002C,W0
U2_Transmit
#0x0020,W0
U2_Transmit
#TIME_CSV,W2
#46,W1
STRING_DISPLAY
;Send out AP Data
;
;
;', ' (Comma,Space)
;
;
;
;Send out Time Data
;
;
mov
#0x002C,W0
;', ' (Comma,Space)
call
U2_Transmit
;
mov
#0x0020,W0
;
call
U2_Transmit
;
return
;
;*********************************************************
;*************************************************************
;********SPI Routines*****************************************
;*********************************************************
;SPI_INIT SPI Initialization
;*********************************************************
SPI_INIT:
clr
SPIRNUM
;Clear Counters
clr
SPITNUM
;
clr
SPIFLAGS
;Clear Flags
bset
AP_TRIS,#AP_SS
;Use SS pin to find out if
;data is ready
bclr
PMD1,#SPI2MD
;Make sure module isn't disabled
clr
SPI2BUF
;Clear SPIxBUF register
;if using interrupts:
;
bclr
IFS2,#SPI2IF
;
nop
bclr
IFS2,#SPI2EIF
;
nop
;
bset
IEC2,#SPI2IE
;
nop
;
bset
IEC2,#SPI2EIE
;
bset
IPC8,#9
;Set Interrupt Priority 3
nop
;
bclr
IPC8,#10
;
nop
;
bset
IPC8,#8
;
;
mov
#0x043F,W0
;Setup 1st SPI Control Register 1:1 Clock
mov
#0x043D,W0
;Setup 1st SPI Control Register 1:16 clock
mov
W0,SPI2CON1
;Master Mode,
mov
#0x0000,W0
;Setup 2nd SPI Control Register
mov
W0,SPI2CON2
;No framed SPI support
bclr
SPI2STAT,#SPIROV
;Clear SPIROV bit
nop
;
bset
SPI2STAT,#SPIEN
;Enable SPI Operation
return
;
;*********************************************************
;*********************************************************
;__SPI2Interrupt SPI Interrupt Routine
;*********************************************************
__SPI2Interrupt:
push
W0
;
push
W1
;
push
W2
;
153
bclr
bclr
IFS2,#SPI2IF
SPIFLAGS,#0
;Clear Interrupt Flag
;Clear SPI Interrupt Occurred? Flag
cp0
bra
SPITNUM
NZ,SPIINTTRANSMIT
;Check if any data to be transmitted
;
rlnc
mov
add
SPIRNUM,WREG
#SPIRECEIVE,W1
W0,W1,W2
;Load Address Location
;
;
mov
mov
SPI2BUF,W0
W0,[W2++]
;Retrieve word
;Save received word
inc
SPIRNUM
SPIINTRETURN:
pop
W2
pop
W1
pop
W0
retfie
;Increment Received Word Count
;
;
;
;
SPIINTTRANSMIT:
dec
SPITNUM
;decrement Transmit Counter
mov
SPI2BUF,W0
;Read data out to prevent overflow
goto
SPIINTRETURN
;
;*********************************************************
;*********************************************************
;__SPI1ErrInterrupt SPI ERROR Interrupt
;
;If an SPI overflow occurs, overflow flag must be cleared
;to re-enable reception
;*********************************************************
__SPI2ErrInterrupt:
bclr
IFS2,#SPI2EIF
;Clear interrupt flag
bclr
SPI2STAT,#SPIROV
;Clear SPIROV bit
retfie
;
;*********************************************************
;*********************************************************
;SPI_WRITE SPI Word Write
;
;Data to be written is to be stored at location SPITRANSMIT
;Number of words to be written stored in SPITNUM
;*********************************************************
SPI_WRITE:
mov
#SPITRANSMIT,W2
;Load start address
SPI_REWRITE:
cp0
SPITNUM
;Check if any data to be transmitted
bra
NZ,SPI_WRITEWORD
;
return
;no data to send
SPI_WRITEWORD:
bset
SPIFLAGS,#0
;Set interrupt check flag
mov
[W2++],W0
;
mov
W0,SPI2BUF
;Send Received word out
nop
;
SPI_WRITE_WAIT:
btsc
SPIFLAGS,#0
;Transfer occur yet?
goto
SPI_WRITE_WAIT
;no
goto
SPI_REWRITE
;yes
;*********************************************************
;*********************************************************
;SPI_READ SPI Word READ
;
;Waits until slave has data to read, it then reads all of
;it
154
;*********************************************************
SPI_READ:
nop
btss
AP_PORT,#AP_SS
;Check if any data needs to be read
goto
SPI_READ
;no data to read yet, wait
SPI_READWORD:
bset
SPIFLAGS,#0
;Set interrupt check flag
mov
0x0000,W0
;Send blank word
mov
W0,SPI2BUF
;in order to initiate transfer
nop
;
SPI_READ_WAIT:
btsc
SPIFLAGS,#0
;Transfer occur yet?
goto
SPI_READ_WAIT
;no
nop
;Give slave a little
nop
;time to clear data
nop
;ready flag
nop
;
nop
;
nop
;
nop
;
btss
AP_PORT,#AP_SS
;Check if any more data needs to be read
return
;no, finished
goto
SPI_READWORD
;yes
;*********************************************************
;*********************************************************
;AP_INIT Acoustic Processor Initialization
;
;Setup default values for AP components
;*********************************************************
AP_INIT:
mov
#45,W0
;Transmit Number of Cycles
mov
W0,AP_TPNC
;
mov
#0x007F,W0
;Pre-Amp 1 Gain
mov
W0,AP_PRE1
;
mov
#0x007F,W0
;Pre-Amp 2 Gain
mov
W0,AP_PRE2
;
mov
#0x007F,W0
;Pre-Amp 3 Gain
mov
W0,AP_PRE3
;
mov
#0x007F,W0
;Post-Amp 1 Gain
mov
W0,AP_POST1
;
mov
#0x007F,W0
;Post-Amp 2 Gain
mov
W0,AP_POST2
;
mov
#0x007F,W0
;Post-Amp 3 Gain
mov
W0,AP_POST3
;
mov
#0x0131,W0
;Big Timeout High Word
mov
W0,AP_BIGTOH
;
mov
#0x2D00,W0
;Big Timeout Low Word
mov
W0,AP_BIGTOL
;
mov
#0x0000,W0
;Small Timeout High Word
mov
W0,AP_SMALLTOH
;
mov
#0x4E20,W0
;Small Timeout Low Word
mov
W0,AP_SMALLTOL
;
clr
AP_COMMAND
;Error occurred,
clr
AP_RANGEH
;clear all data
clr
AP_RANGEL
;
clr
AP_TRANS1
;
clr
AP_TRANS2
;
clr
AP_TRANS3
;
clr
AP_TRANSORDER
;
return
;done
;*********************************************************
;*********************************************************
155
;AP_GM Acoustic Processor Get Measurement
;
;Request that the AP start getting a range and bearing
;measurement. It should then signal the AP_SS line when
;data is ready.
;*********************************************************
AP_GM:
mov
#0X474D,W0
;'GM' - Command
mov
W0,SPITRANSMIT
;
mov
#0x0001,W0
;Set Number of bytes
mov
W0,SPITNUM
;to send out
call
SPI_WRITE
;Send Command
return
;done
;*********************************************************
;*********************************************************
;AP_GM_READ Acoustic Processor Get Measurement Read
;
;Wait for AP data to be ready, then retrieve it and parse
;it into data memory
;*********************************************************
AP_GM_READ:
call
SPI_READ
;Wait, then read in AP data
mov
#0x0007,W0
;Check that at least 7 words
cp
SPIRNUM
;were received
bra
LTU,APGMREAD_ERROR
;if not, error
;enough data received, parse data out
mov
#SPIRECEIVE,W2
;Load start address
mov
[W2++],W0
;
mov
W0,AP_COMMAND
;
mov
[W2++],W0
;
mov
W0,AP_RANGEH
;
mov
[W2++],W0
;
mov
W0,AP_RANGEL
;
mov
[W2++],W0
;
mov
W0,AP_TRANSORDER
;
mov
[W2++],W0
;
mov
W0,AP_TRANS1
;
mov
[W2++],W0
;
mov
W0,AP_TRANS2
;
mov
[W2++],W0
;
mov
W0,AP_TRANS3
;
clr
SPIRNUM
;Clear receive buffer count
return
;done
APGMREAD_ERROR:
clr
AP_COMMAND
;Error occurred,
clr
AP_RANGEH
;clear all data
clr
AP_RANGEL
;
clr
AP_TRANS1
;
clr
AP_TRANS2
;
clr
AP_TRANS3
;
clr
AP_TRANSORDER
;
return
;
;*********************************************************
;*********************************************************
;AP_GB Acoustic Processor Get Bearing
;
;Request that the AP start getting a bearing
;measurement. It should then signal the AP_SS line when
;data is ready.
;*********************************************************
AP_GB:
mov
#0X4742,W0
;'GB' - Command
mov
W0,SPITRANSMIT
;
156
mov
#0x0001,W0
;Set Number of bytes
mov
W0,SPITNUM
;to send out
call
SPI_WRITE
;Send Command
return
;done
;*********************************************************
;*********************************************************
;AP_GB_READ Acoustic Processor Get Bearing Read
;
;Wait for AP data to be ready, then retrieve it and parse
;it into data memory
;*********************************************************
AP_GB_READ:
call
SPI_READ
;Wait, then read in AP data
mov
#0x0005,W0
;Check that at least 5 words
cp
SPIRNUM
;were received
bra
LTU,APGBREAD_ERROR
;if not, error
;enough data received, parse data out
mov
#SPIRECEIVE,W2
;Load start address
mov
[W2++],W0
;
mov
W0,AP_COMMAND
;
mov
[W2++],W0
;
mov
W0,AP_TRANSORDER
;
mov
[W2++],W0
;
mov
W0,AP_TRANS1
;
mov
[W2++],W0
;
mov
W0,AP_TRANS2
;
mov
[W2++],W0
;
mov
W0,AP_TRANS3
;
clr
AP_RANGEH
;clear range (not used)
clr
AP_RANGEL
;
clr
SPIRNUM
;Clear receive buffer count
return
;done
APGBREAD_ERROR:
clr
AP_COMMAND
;Error occurred,
clr
AP_RANGEH
;clear all data
clr
AP_RANGEL
;
clr
AP_TRANS1
;
clr
AP_TRANS2
;
clr
AP_TRANS3
;
clr
AP_TRANSORDER
;
return
;
;*********************************************************
;*********************************************************
;AP_TP Acoustic Processor Transmit Pulse
;
;Request that the AP transmit a pulse of length determined
;by AP_TPNC.
;*********************************************************
AP_TP:
mov
#SPITRANSMIT,W2
;Set Message Address
mov
#0X5450,W0
;'TP' - Command
mov
W0,[W2++]
;
mov
AP_TPNC,W0
;Number of Transmit Cycles
mov
W0,[W2++]
;
mov
#0x0002,W0
;Set Number of bytes
mov
W0,SPITNUM
;to send out
call
SPI_WRITE
;Send Command
return
;done
;*********************************************************
;*********************************************************
;AP_SP Acoustic Processor Set Parameters
;
;Send out new parameters to acoustic board
157
;*********************************************************
AP_SP:
mov
#SPITRANSMIT,W2
;Set Message Address
mov
#0X5350,W0
;'SP' - Command
mov
W0,[W2++]
;
mov
AP_TPNC,W0
;Number of Transmit Cycles
mov
W0,[W2++]
;
mov
AP_PRE1,W0
;Pre-Amp 1 Gain
mov
W0,[W2++]
;
mov
AP_PRE2,W0
;Pre-Amp 2 Gain
mov
W0,[W2++]
;
mov
AP_PRE3,W0
;Pre-Amp 3 Gain
mov
W0,[W2++]
;
mov
AP_POST1,W0
;Post-Amp 1 Gain
mov
W0,[W2++]
;
mov
AP_POST2,W0
;Post-Amp 2 Gain
mov
W0,[W2++]
;
mov
AP_POST3,W0
;Post-Amp 3 Gain
mov
W0,[W2++]
;
mov
AP_BIGTOH,W0
;Big Timeout High Word
mov
W0,[W2++]
;
mov
AP_BIGTOL,W0
;Big Timeout Low Word
mov
W0,[W2++]
;
mov
AP_SMALLTOH,W0
;Small Timeout High Word
mov
W0,[W2++]
;
mov
AP_SMALLTOL,W0
;Small Timeout Low Word
mov
W0,[W2++]
;
mov
#0x000C,W0
;Set Number of bytes
mov
W0,SPITNUM
;to send out
call
SPI_WRITE
;Send Command
return
;done
;*********************************************************
;*************************************************************
;********Timer Routines***************************************
;*********************************************************
;Timer23_init - Timers 2/3 Init (use as a 32-bit timer)
;
;Setup Timer 2/3 to operated as a single 32-bit timer
;*********************************************************
Timer23_init:
bclr
PMD1,#12
;Enable Timer 2/3 Modules
bclr
PMD1,#13
;
clr
T2CON
;
clr
T3CON
;
mov
#0xFFFF,W0
;Allow for Maximum Count
mov
W0,PR3
;
mov
W0,PR2
;
;mov
#0x0038,W0
;Setup 32-bit timer (1:256 prescaler)
mov
#0x0008,W0
;Setup 32-bit timer (1:1 prescaler)
mov
W0,T2CON
;(Timer 2/3)
;Not setting up any interrupts
nop
bset
T2CON,#15
;start timer
return
;
;*********************************************************
;*********************************************************
;Timer23_clear - Timers 2/3 Clear
;
;Clear 32-bit timer (Start at 0x00000000)
;*********************************************************
Timer23_clear:
;
158
mov
#0x0000,W0
;Write 0x0000 to MSW
mov
W0,TMR3HLD
;(Timer 2/3)
mov
W0,TMR2
;(Timer 2/3)
return
;
;*********************************************************
;*********************************************************
;Timer23_read - Timers 2/3 Read
;
;Read the 32-bit timer
;*********************************************************
Timer23_read:
mov
TMR2,W0
;Transfer the LSW into W0
mov
TMR3HLD, W1
;Transfer the MSW from the
;holding register to W1
return
;
;*********************************************************
;*********************************************************
;OnePPS_init - 1PPS Init (INT4)
;
;Setup 1PPS pin as input and interrupt used to clear timer
;*********************************************************
OnePPS_init:
bset
INT_TRIS,#GPS_INT
;Set 1PPS pin as an input
;Setup INT4 interrupt
bclr
INTCON2,#INT4EP
;Interrupt on positive edge
bset
IPC13,#9
;Set interrupt priority
;to level 6 (default level 4)
bclr
INTCON1,#NSTDIS
;Enable nested interrupts
bclr
IFS3,#INT4IF
;Clear interrupt flag
bset
IEC3,#INT4IE
;Enable INT4 interrupt
return
;
;*********************************************************
;*********************************************************
;__INT4Interrupt - INT4 Interrupt Routine (1PPS)
;
;Clears Timer2/3 when 1PPS interrupt is received
;*********************************************************
__INT4Interrupt:
;Save context
push
W0
;Save W0
push
W1
;
push
W2
;
;Clear Timer 2/3 value
mov
#0x0000,W0
;Write 0x0000 to MSW
mov
W0,TMR3HLD
;(Timer 2/3)
mov
W0,TMR2
;(Timer 2/3)
bset
MAIN_FLAGS,#0
;Set 1PPS Flag
btss
MAIN_FLAGS,#1
;Send out Debug "[PPS]"?
goto
int4_exit
;no
mov
#GPSPPS,W2
;yes, Send out PPS message
mov
#5,W1
;"[PPS]"
call
STRING_DISPLAY
;
mov
#0x0001,W1
;New line
call
NEWLINE_DISPLAY
;
int4_exit:
;Restore context
pop
W2
;
pop
W1
;
pop
W0
;Restore W0
bclr
IFS3,#INT4IF
;Clear interrupt flag
retfie
;return
;*********************************************************
159
;*************************************************************
;********Tilt Sensor Routines*********************************
;*********************************************************
;tilt_init - Tilt Init Routine
;
;Initialize tilt sensor pins
;*********************************************************
Tilt_init:
;SCLK
bclr
XL_TRIS,#XL_SCLK
;Set inputs and outputs
bset
XL_PORT,#XL_SCLK
;
;DIN
bclr
XL_TRIS,#XL_DIN
;
bclr
XL_PORT,#XL_DIN
;
;DOUT
bset
XL_TRIS,#XL_DOUT
;
nop
;CS(Bar)
bclr
XL_TRIS,#XL_CS
;
bset
XL_PORT,#XL_CS
;
;AUX DIO0
bset
XL_TRIS,#XL_DIO0
;
return
;
;*********************************************************
;****Tilt Sensor Setup**************************
Tilt_Setup:
mov
#0xB804,W0
;Set AVG_CNT filter to 16 taps
call
Tilt_RW
;
mov
#0xB684,W0
;Set Sample Period to ~15ms
call
Tilt_RW
;equates to about 0.25sec measurement
;with 16 taps window average
mov
#0xB406,W0
;Set DIO0 line to be active high Data
call
Tilt_RW
;Ready Line
return
;***********************************************
;****Tilt Sensor Zero***************************
Tilt_Zero:
mov
#0xBE01,W0
;Tell Sensor to zero its offsets
call
Tilt_RW
;
return
;***********************************************
;****Tilt Data Mask*****************************
Tilt_Data_Mask:
;Tilt Values should only be 12-bits
mov
#0x0FFF,W0
;Mask off 12 bit value for Tilt
and
XTilt
;
and
YTilt
;
mov
#0xF000,W0
;Extend Sign bit if present
btsc
XTilt,#11
;
ior
XTilt
;
btsc
YTilt,#11
;
ior
YTilt
;
;Acceleration Values should be 14-bits
mov
#0x3FFF,W0
;Mask off 14 bit value
and
XAccel
;
and
YAccel
;
mov
#0xC000,W0
;Extend Sign bit if present
btsc
XAccel,#13
;
ior
XAccel
;
btsc
YAccel,#13
;
ior
YAccel
;
160
return
;
;***********************************************
;****Tilt Sensor Read Data**********************
Tilt_Read:
mov
#0x0D00,W0
;Send out read command
call
Tilt_RW
;
mov
#0x0F00,W0
;Send out read command
call
Tilt_RW
;
mov
W1,XTilt
;Retrieve X-Tilt
mov
#0x0500,W0
;Send out read command
call
Tilt_RW
;
mov
W1,YTilt
;Retrieve Y-Tilt
mov
#0x0700,W0
;Send out read command
call
Tilt_RW
;
mov
W1,XAccel
;Retrieve X-Accel
mov
#0x0B00,W0
;Send out read command
collected)
call
Tilt_RW
;
mov
W1,YAccel
;Retrieve Y-Accel
return
;
;***********************************************
;****Tilt Read Write****************************
;W0 includes Command Word (chooses register to read)
Tilt_RW:
bclr
XL_PORT,#XL_CS
;CSbar low
;W0 should have COMMAND BYTE
call
Tilt_Shift
;Shift out Command Word
;Receive Data Word in W1
bset
XL_PORT,#XL_CS
;CSbar high
call
delay71us
;delay to satisfy timing
return
;Return with Data in W1
;***********************************************
;****Tilt Clock Pulse***************************
Tilt_Clock_Pulse:
bclr
XL_PORT,#XL_SCLK
;Clock Pin Low
nop
;
nop
;
bset
XL_PORT,#XL_SCLK
;Clock Pin High
return
;
;***********************************************
;****Tilt Shift*********************************
Tilt_Shift:
;Clear output variable W1
clr
W1
;
;Begin Shifting Data
bclr
XL_PORT,#XL_DIN
;Assume Bit Clear
btsc
W0,#15
;Send out MSB
bset
XL_PORT,#XL_DIN
;Bit Set
call
Tilt_Clock_Pulse
;Clock bit through
btsc
XL_PORT,#XL_DOUT
;Read in MSB
bset
W1,#15
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#14
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#14
;Assume Bit Clear
;Send out bit #14
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
XL_PORT,#XL_DIN
W0,#13
;Assume Bit Clear
;Send out bit #13
for X-Tilt
for Y-Tilt
for X-Accel
for Y-Accel
for Temp (never
161
bset
call
btsc
bset
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#13
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#12
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#12
;Assume Bit Clear
;Send out bit #12
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#11
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#11
;Assume Bit Clear
;Send out bit #11
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#10
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#10
;Assume Bit Clear
;Send out bit #10
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#9
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#9
;Assume Bit Clear
;Send out bit #9
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#8
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#8
;Assume Bit Clear
;Send out bit #8
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#7
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#7
;Assume Bit Clear
;Send out bit #7
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#6
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#6
;Assume Bit Clear
;Send out bit #6
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#5
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#5
;Assume Bit Clear
;Send out bit #5
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
XL_PORT,#XL_DIN
W0,#4
;Assume Bit Clear
;Send out bit #4
162
bset
call
btsc
bset
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#4
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#3
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#3
;Assume Bit Clear
;Send out bit #3
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#2
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#2
;Assume Bit Clear
;Send out bit #2
;Bit Set
;Clock bit through
;Read in bit
;
bclr
btsc
bset
call
btsc
bset
XL_PORT,#XL_DIN
W0,#1
XL_PORT,#XL_DIN
Tilt_Clock_Pulse
XL_PORT,#XL_DOUT
W1,#1
;Assume Bit Clear
;Send out bit #1
;Bit Set
;Clock bit through
;Read in bit
;
bclr
XL_PORT,#XL_DIN
;Assume Bit Clear
btsc
W0,#0
;Send out bit #0
bset
XL_PORT,#XL_DIN
;Bit Set
call
Tilt_Clock_Pulse
;Clock bit through
btsc
XL_PORT,#XL_DOUT
;Read in bit
bset
W1,#0
;
return
;
;***********************************************
;*************************************************************
;********GPS Routines*****************************************
;*********************************************************
;gps_init - GPS Init Routine
;
;Initialize GPS pins
;*********************************************************
gps_init:
bclr
GPS_TRIS,#GPS_ENABLE
;
bclr
GPS_PORT,#GPS_ENABLE
;
clr
GPSFLAGS
;
clr
GPSBUF1CNT
;
clr
GPSBUF2CNT
;
return
;
;*********************************************************
;*********************************************************
;gps_enable - GPS Enable Routine
;
;Provides power to GPS module
;*********************************************************
gps_enable:
bset
GPS_PORT,#GPS_ENABLE
;
return
;
;*********************************************************
;*********************************************************
;gps_disable - GPS Disable Routine
163
;
;Removes power from GPS module
;*********************************************************
gps_disable:
bclr
GPS_PORT,#GPS_ENABLE
;
return
;
;*********************************************************
;*********************************************************
;gps_rxdata - GPS Recieve Data Byte Routine
;
;This routine looks for GPS messages and stores them to
;a buffer in RAM such that they can be stored.
;*********************************************************
gps_rxdata:
btsc
GPSFLAGS,#0
;Check if Message is in progress
goto
gps_rxdata_mip
;message in progress, deal with it
;no
message in progress
mov
#0x0024,W0
cp.b
U1RECEIVE
bra
Z,gps_rxdata_nm
return
;Check for start of new message ('$')
;
;Start of new message, write data
;not a new message, done
gps_rxdata_mip:
btsc
GPSFLAGS,#6
goto
gps_rxdata_mipw1
goto
gps_rxdata_mipw2
;Check which buffer is being used
;Buffer 1
;Buffer 2
gps_rxdata_nm:
btss
GPSFLAGS,#2
goto
gps_rxdata_w1
btss
GPSFLAGS,#4
goto
gps_rxdata_w2
bset
GPSFLAGS,#1
mov
#GPSBUFFULL,W2
mov
#22,W1
call
STRING_DISPLAY
mov
#0x0001,W1
call
NEWLINE_DISPLAY
return
gps_rxdata_w1:
bset
GPSFLAGS,#0
bset
GPSFLAGS,#6
clr
GPSBUF1CNT
gps_rxdata_mipw1:
mov
#GPSMSG1,W0
add
GPSBUF1CNT,WREG
mov
U1RECEIVE,W1
mov.b W1,[W0]
inc
GPSBUF1CNT
;Check for end of message
mov
#0x000A,W0
cp.b
U1RECEIVE
bra
Z,gps_rxdata_eom1
return
gps_rxdata_eom1:
bclr
GPSFLAGS,#0
bset
GPSFLAGS,#2
bset
GPSFLAGS,#3
return
gps_rxdata_w2:
;Check if buffer 1 is available
;yes it is, choose buffer 1
;Check if buffer 2 is available
;yes it is, choose buffer 2
;both buffers full, set error flag
;Send out error message
;"ERROR: GPS BUFFER FULL"
;
;New line
;
;return
;Set message in progress flag
;Set use buffer 1 flag
;clear byte counter
;load buffer address
;add byte count to buffer address
;
;place received character in next buffer location
;increment counter
;Check for end of message ('LF')
;
;End of message.. perform additional tasks
;not the end of the message, done
;Clear MIP Flag
;Set Buffer 1 Full Flag
;Set Buffer 1 Requires Writing Flag
;done
164
bset
GPSFLAGS,#0
;Set message in progress flag
bclr
GPSFLAGS,#6
;indicate use of buffer 2
clr
GPSBUF2CNT
;clear byte counter
gps_rxdata_mipw2:
mov
#GPSMSG2,W0
;load buffer address
add
GPSBUF2CNT,WREG
;add byte count to buffer address
mov
U1RECEIVE,W1
;
mov.b W1,[W0]
;place received character in next buffer location
inc
GPSBUF2CNT
;increment counter
;Check for end of message
mov
#0x000A,W0
;Check for end of message ('LF')
cp.b
U1RECEIVE
;
bra
Z,gps_rxdata_eom2
;End of message.. perform additional tasks
return
;not the end of the message, done
gps_rxdata_eom2:
bclr
GPSFLAGS,#0
;Clear MIP Flag
bset
GPSFLAGS,#4
;Set Buffer 2 Full Flag
bset
GPSFLAGS,#5
;Set Buffer 2 Requires Writing Flag
return
;done
;*********************************************************
;*********************************************************
;gps_rxdata_err
GPS Receive Data Error
;
;This routine is called when a GPS Data Receive Error
;occurs. It clears any pending message in progress so as
;to prevent incomplete data messages from being recorded.
;*********************************************************
gps_rxdata_err:
bclr
GPSFLAGS,#0
;Clear any message in progress
return
;
;*********************************************************
;*********************************************************
;gps_data_write
GPS Data Write
;
;This routine will check for any messeges in the two GPS
;RX Data Buffers. If there is a full message waiting to
;be written, it will write it.
;*********************************************************
gps_data_write:
bclr
GPSFLAGS,#7
;Clear GPS Data Written Flag
nop
btsc
GPSFLAGS,#3
;Check if buffer 1 requires writing
goto
gps_data_write_1
;yes it does, must check first for 2 messages
btsc
call
GPSFLAGS,#5
gps_data_write2
btss
GPSFLAGS,#7
call
gps_empty_write
return
gps_data_write_1:
btsc
GPSFLAGS,#5
goto
gps_data_write_2
no message
call
gps_data_write1
return
gps_data_write_2:
bclr
GPSFLAGS,#2
nop
bclr
GPSFLAGS,#3
nop
;Check if buffer 2 requires writing
;yes it does, write it
;Check if some GPS data written?
;no, write empty record
;All buffers written if needed, done
;Check if buffer 2 contains a message
;yes, two messages in buffer.. clear buffer, write
;no, write buffer 1 record
;clear "buffer full" flag
;clear "buffer requires writing" flag
165
bclr
GPSFLAGS,#4
;clear "buffer full" flag
nop
bclr
GPSFLAGS,#5
;clear "buffer requires writing" flag
call
gps_empty_write
;write empty record
return
;
;*********************************************************
;*********************************************************
;gps_data_write1
GPS WRITE BUFFER 1 DATA
;
;Writes out data in GPS RX Data 1 Buffer to SD Card.
;Assumes GPSBUF1CNT number of bytes in message beginning
;at GPS1BUFF memory location.
;*********************************************************
gps_data_write1:
bset
GPSFLAGS,#7
;Indicate GPS Data Written
mov
#GPSMSG1,W2
;load buffer address
mov
GPSBUF1CNT,W1
;load number of characters
dec
W1,W1
;rid of <CR> <LF>
dec
W1,W1
;
gps_data_write1l:
cp0
W1
;Check if there are still
bra
NZ,gps_data_write1so ;characters left to send
bra
gps_data_write1end
;no
gps_data_write1so:
;Characters left to send out
mov.b
[W2++],W0
;Send character out UART2 to datalogger
call
U2_Transmit
;
dec
W1,W1
;
bra
gps_data_write1l
;
gps_data_write1end:
bclr
GPSFLAGS,#2
;clear "buffer full" flag
nop
bclr
GPSFLAGS,#3
;clear "buffer requires writing" flag
return
;
;*********************************************************
;*********************************************************
;gps_data_write2
GPS WRITE BUFFER 2 DATA
;
;Writes out data in GPS RX Data 2 Buffer to SD Card.
;Assumes GPSBUF2CNT number of bytes in message beginning
;at GPS2BUFF memory location.
;*********************************************************
gps_data_write2:
bset
GPSFLAGS,#7
;Indicate GPS Data Written
mov
#GPSMSG2,W2
;load buffer address
mov
GPSBUF2CNT,W1
;load number of characters
dec
W1,W1
;rid of <CR> <LF>
dec
W1,W1
;
gps_data_write2l:
cp0
W1
;Check if there are still
bra
NZ,gps_data_write2so ;characters left to send
bra
gps_data_write2end
;no
gps_data_write2so:
;Characters left to send out
mov.b
[W2++],W0
;Send character out UART2 to datalogger
call
U2_Transmit
;
dec
W1,W1
;
bra
gps_data_write2l
;
gps_data_write2end:
bclr
GPSFLAGS,#4
;clear "buffer full" flag
nop
bclr
GPSFLAGS,#5
;clear "buffer requires writing" flag
return
;
166
;*********************************************************
;*********************************************************
;gps_empty_write
GPS EMPTY RECORD WRITE
;
;Writes an empty gps record to UART2
;*********************************************************
gps_empty_write:
mov
#14,W1
;load number comma, spaces
gps_empty_writel:
cp0
W1
;Check if there are still
bra
NZ,gps_empty_writeso ;characters left to send
bra
gps_empty_writeend
;no
gps_empty_writeso:
;Characters left to send out
mov
#0x002C,W0
;', ' (Comma,Space)
call
U2_Transmit
;
mov
#0x0020,W0
;
call
U2_Transmit
;
dec
W1,W1
;
bra
gps_empty_writel
;
gps_empty_writeend:
return
;
;*********************************************************
;*************************************************************
;********UART 1 ROUTINES**************************************
;****UART 1 Initialization***********************
UART1_Init:
;Make sure module isn't disabled
bclr
PMD1,#U1MD
;Enable UART1
;Setup UART Regisiters
mov
#BAUD_NUM1,W0
;Setup Bard Rate
mov
W0,U1BRG
;
mov
#U1STA_C,W0
;Setup interrupts, etc
mov
W0,U1STA
;
mov
#U1MODE_C,W0
;Setup Baud rate High Reg, etc
mov
W0,U1MODE
;
bset
U1STA,#UTXEN
;set transmit enabled
;Setup UART Interrupts
bset
INTCON1,#NSTDIS
;Disable Interrupt Nesting
bclr
IFS0,#U1RXIF
;Clear Recieve Interrupt Flag
bset
IEC0,#U1RXIE
;Enable Recieve Interrupt
return
;done
;************************************************
;*****UART 1 Transmit Routine********************
U1_Transmit:
bset
U1STA,#UTXEN
;set transmit enabled
mov
W0,U1TXREG
;send out W0
U1_TransWait:
nop
btsc
U1STA,#UTXBF
;Check if transmit buffer is full
goto
U1_TransWait
;buffer full, wait
return
;buffer has room for more, return
;************************************************
;*****UART 1 Disable Transmit Routine************
U1_TransmitDisable:
btss
U1STA,#TRMT
;Check that last byte has been sent out
goto
U1_TransmitDisable
;no, wait
bclr
U1STA,#UTXEN
;last byte out, disable transmit
return
;
;************************************************
167
;*****UART 1 Recieve Interrupt Routine***********
__U1RXInterrupt:
;Save context
push
W0
;Save W0
push
W1
;Save W1
push
W2
;
btsc
goto
U1STA,#OERR
U1OverrunError
;Check for overrun error
;overrun error occurred
btsc
goto
U1STA,#FERR
U1FramingError
;Check for framing error
;framing error detected
U1ReadByte:
;Byte(s) are ready to be read
mov
U1RXREG,W0
;
mov
W0,U1RECEIVE
;
;Call function to deal with recieved byte
btsc
MAIN_FLAGS,#6
;Check for GPS COM MODE
call
bt_transmit
;GPS COM MODE (send GPS data to computer)
btsc
MAIN_FLAGS,#7
;Check for GPS Data Storage Mode
call
gps_rxdata
;GPS DATA STORAGE MODE
;Byte has been dealt with
btsc
U1STA,#URXDA
;Check if another byte is available
goto
U1ReadByte
;yes, read it
;Restore context
pop
W2
pop
W1
pop
W0
bclr
IFS0,#U1RXIF
retfie
U1OverrunError:
mov
#U1ORERR,W2
mov
#11,W1
call
STRING_DISPLAY
mov
#0x0001,W1
call
NEWLINE_DISPLAY
call
gps_rxdata_err
;Restore context
pop
W2
pop
W1
pop
W0
bclr
U1STA,#OERR
bclr
IFS0,#U1RXIF
retfie
U1FramingError:
mov
#U1FRERR,W2
mov
#11,W1
call
STRING_DISPLAY
mov
#0x0001,W1
call
NEWLINE_DISPLAY
call
gps_rxdata_err
;Restore context
pop
W2
pop
W1
pop
W0
bclr
U1STA,#FERR
;
;Restore W1
;Restore W0
;Clear Recieve Interrupt Flag
;return
;Send out error message
;
;
;New line
;
;Clear MIP
;
;Restore W1
;Restore W0
;Clear error
;Clear Recieve Interrupt Flag
;
;Send out error message
;
;
;New line
;
;Clear MIP
;
;Restore W1
;Restore W0
;Clear error
168
bclr
IFS0,#U1RXIF
;Clear Recieve Interrupt Flag
retfie
;
;************************************************
;*************************************************************
;********Datalogger Routines**********************************
;*********************************************************
;data_init - Datalogger Init Routine
;
;Initializes all the datalogger pins
;*********************************************************
data_init:
bclr
DL_TRIS,#DL_ENABLE
;Datalogger disabled
bclr
DL_PORT,#DL_ENABLE
;
bclr
DL_TRIS,#DL_STOP
;Stop pin
bclr
DL_PORT,#DL_STOP
;
bclr
DL_TRIS,#DL_RST
;Reset pin
bclr
DL_PORT,#DL_RST
;
return
;
;*********************************************************
;*********************************************************
;data_enable - Datalogger Enable Routine
;
;Provides power to Datalogger module
;*********************************************************
data_enable:
bclr
DL_TRIS,#DL_ENABLE
;
bset
DL_PORT,#DL_ENABLE
;
return
;
;*********************************************************
;*********************************************************
;data_disable - Datalogger Disable Routine
;
;Removes power to Datalogger module
;*********************************************************
data_disable:
bclr
DL_TRIS,#DL_ENABLE
;
bclr
DL_PORT,#DL_ENABLE
;
return
;
;*********************************************************
;*********************************************************
;data_stop - Datalogger Stop Routine
;
;Stops logging data
;*********************************************************
data_stop:
bset
DL_PORT,#DL_STOP
;
call
delay100ms
;
bclr
DL_PORT,#DL_STOP
;
return
;
;*********************************************************
;*********************************************************
;data_reset - Datalogger Reset Routine
;
;Resets the datalogger to reenable data logging
;*********************************************************
data_reset:
bset
DL_PORT,#DL_RST
;
call
delay100ms
;
169
bclr
DL_PORT,#DL_RST
;
return
;
;*********************************************************
;*********************************************************
;data_trans - Datalogger Simple Transmit Routine
;
;Selects data logger, then sends byte in W0 out
;*********************************************************
data_trans:
bclr
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
;
bclr
U2SS_PORT,#TXSS1
;the Datalogger RX
call
U2_Transmit
;
return
;
;*********************************************************
;*********************************************************
;data_settx - Set Datalogger as TX device
;
;Selects data logger as TX device
;*********************************************************
data_settx:
bclr
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
;
bclr
U2SS_PORT,#TXSS1
;the Datalogger RX
return
;
;*********************************************************
;*********************************************************
;data_transmit - Datalogger Transmit Routine
;
;Sends value in W0 out to the datalogger
;*********************************************************
data_transmit:
push
SR
;Save Current status register
push
W1
;
mov
#0x00E0,W1
;Disable Interrupts
mov
W1,SR
;
btss
U2FLAGS,#1
;Last byte sent out to datalogger?
goto
data_transmit_set
;no, reset multiplexor
call
U2_Transmit
;yes, send byte out
bset
U2FLAGS,#1
;Indicate byte sent to datalogger
pop
W1
;
pop
SR
;Restore SR, enable interrupts
return
;
data_transmit_set:
btss
U2STA,#TRMT
;Byte still being transmitted?
goto
data_transmit_set
;yes, wait for completion
;all previous bytes transmitted
bset
U2FLAGS,#1
;Indicate byte sent to datalogger
bclr
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
bclr
U2SS_PORT,#TXSS1
;the Datalogger RX
nop
;
call
U2_Transmit
;
pop
W1
;
pop
SR
;Restore SR, enable interrupts
return
;
;*********************************************************
;*************************************************************
170
;********Conversion Routines**********************************
;*********************************************************
;SNUM2ASCII - Signed Number to ASCII
;Converts Signed 16-bit value in W0 to 5 ASCII characters plus
;a sign character.
;The ASCII values are stored in
;W6 - Sign (' ' or '-')
;W5 - ten thousands
;W4 - thousands
;W3 - hundreds
;W2 - tens
;W1 - ones
;*********************************************************
snum2ascii:
btss
W0,#15
;Check the sign bit
goto
snum2asciipos
;Positive Number
;negative number, must reverse the
;2's complement procedure before calling num2bcd
mov
#0xFFFF,W6
;Invert all the bits
xor
W0,W6,W0
;
inc
W0,W0
;add 1
call
num2bcd
;
mov
#0x002D,W6
;Set Sign character
add
#0x30,W1
;Convert BCD to ASCII
add
#0x30,W2
;
add
#0x30,W3
;
add
#0x30,W4
;
add
#0x30,W5
;
return
;
snum2asciipos:
mov
#0x0020,W6
;Set Sign character
call
num2bcd
;Convert digits
add
#0x30,W1
;Convert BCD to ASCII
add
#0x30,W2
;
add
#0x30,W3
;
add
#0x30,W4
;
add
#0x30,W5
;
return
;
;***********************************************
;*********************************************************
;NUM2ASCII - Number to ASCII
;Converts 16-bit value in W0 to 5 ASCII characters
;The BCD values are stored in
;W5 - ten thousands
;W4 - thousands
;W3 - hundreds
;W2 - tens
;W1 - ones
;*********************************************************
num2ascii:
call
num2bcd
;convert to BCD
add
#0x30,W1
;Convert BCD to ASCII
add
#0x30,W2
;
add
#0x30,W3
;
add
#0x30,W4
;
add
#0x30,W5
;
return
;
;*********************************************************
;*********************************************************
;NUM2BCD - Number to BCD
;Converts 16-bit value in W0 to 5 BCD digits
;The BCD values are stored in
171
;W5 - ten thousands
;W4 - thousands
;W3 - hundreds
;W2 - tens
;W1 - ones
;*********************************************************
num2bcd:
clr
W5
;Clear the variables to be stored
clr
W4
;
clr
W3
;
clr
W2
;
clr
W1
;
tenthoutest:
mov
#0x2710,W1
cp
W0,W1
bra
ltu,thoutest
sub
inc.b
bra
thoutest:
mov
cp
bra
sub
inc.b
bra
hundtest:
mov
cp
bra
sub
inc.b
bra
tentest:
mov
cp
bra
sub
inc.b
bra
W0,W1,W0
W5,W5
tenthoutest
#0x03E8,W1
W0,W1
ltu,hundtest
W0,W1,W0
W4,W4
thoutest
#0x0064,W1
W0,W1
ltu,tentest
W0,W1,W0
W3,W3
hundtest
#0x000A,W1
W0,W1
ltu,onetest
W0,W1,W0
W2,W2
tentest
;(10,000 in hex)
;
;Check if W0 >= 10,000
;it is
;Subtract 10,000 from Incoming number
;Increment BCD digit
;repeat
;(1,000 in hex)
;
;Check if W0 >= 1,000
;it is
;Subtract 1,000 from Incoming number
;Increment BCD digit
;repeat
;(100 in hex)
;
;Check if W0 >= 100
;it is
;Subtract 100 from Incoming number
;Increment BCD digit
;repeat
;(10 in hex)
;
;Check if W0 >= 10
;it is
;Subtract 10 from Incoming number
;Increment BCD digit
;repeat
onetest:
;Rest are ones currently stored in W0
mov
W0,W1
;
return
;***********************************************
;done
;*********************************************************
;BNUM2ASCII - Big Number to ASCII Character Array
;Converts 32-bit value in W1,W0 to 10 ASCII digits
;The ASCII values are stored in
;W10 - billions
;W9 - hundred millions
;W8 - ten millions
;W7 - millions
172
;W6 - hundred thousands
;W5 - ten thousands
;W4 - thousands
;W3 - hundreds
;W2 - tens
;W1 - ones
;*********************************************************
bnum2ascii:
clr
W10
;Clear the variables to be stored
clr
W9
;
clr
W8
;
clr
W7
;
clr
W6
;
clr
W5
;
clr
W4
;
clr
W3
;
clr
W2
;
clr
ACCAU
mov
W1,ACCAH
mov
W0,ACCAL
bbilliontest:
mov
#0x3B9A,W0
mov
W0,ACCBH
mov
#0xCA00,W0
mov
W0,ACCBL
;Check if ACCA is > than ACCB
mov
ACCAH,W0
mov
ACCBH,W1
cp
W0,W1
bra
ltu,bhundmilltest
bra
gtu,bbilliongt
mov
ACCAL,W0
mov
ACCBL,W1
cp
W0,W1
bra
ltu,bhundmilltest
bbilliongt:
SUB
A
inc.b
W10,W10
bra
bbilliontest
bhundmilltest:
mov
#0x05F5,W0
mov
W0,ACCBH
mov
#0xE100,W0
mov
W0,ACCBL
;Check if ACCA is > than ACCB
mov
ACCAH,W0
mov
ACCBH,W1
cp
W0,W1
bra
ltu,btenmilltest
bra
gtu,bhundmillgt
mov
ACCAL,W0
mov
ACCBL,W1
cp
W0,W1
bra
ltu,btenmilltest
bhundmillgt:
SUB
A
inc.b
W9,W9
bra
bhundmilltest
;Move 32-bit value to Accumulator
;
;
;(1,000,000,000 in hex)
;
;
;
;;First Check if ACCAH >= ACCBH
;
;
;no, no billions
;definetly some billions
;ACCAH = ACCBH
;Check LSBs
;
;no billions
;Subtract a billion from incoming number
;increment BCD digit
;repeat
;(100,000,000 in hex)
;
;
;
;;First Check if ACCAH >= ACCBH
;
;
;no, no hundred millions
;definetly some hundred millions
;ACCAH = ACCBH
;Check LSBs
;
;no hundred millions
;Subtract a hundred million from incoming number
;increment BCD digit
;repeat
173
btenmilltest:
mov
#0x0098,W0
mov
W0,ACCBH
mov
#0x9680,W0
mov
W0,ACCBL
;Check if ACCA is > than ACCB
mov
ACCAH,W0
mov
ACCBH,W1
cp
W0,W1
bra
ltu,bmilltest
bra
gtu,btenmillgt
mov
ACCAL,W0
mov
ACCBL,W1
cp
W0,W1
bra
ltu,bmilltest
btenmillgt:
SUB
A
inc.b
W8,W8
bra
btenmilltest
bmilltest:
mov
mov
mov
mov
#0x000F,W0
W0,ACCBH
#0x4240,W0
W0,ACCBL
;Check if ACCA is > than ACCB
mov
ACCAH,W0
mov
ACCBH,W1
cp
W0,W1
bra
ltu,bhundthoutest
bra
gtu,bmillgt
mov
ACCAL,W0
mov
ACCBL,W1
cp
W0,W1
bra
ltu,bhundthoutest
bmillgt:
SUB
A
inc.b
W7,W7
bra
bmilltest
bhundthoutest:
mov
#0x0001,W0
mov
W0,ACCBH
mov
#0x86A0,W0
mov
W0,ACCBL
;Check if ACCA is > than ACCB
mov
ACCAH,W0
mov
ACCBH,W1
cp
W0,W1
bra
ltu,btenthoutest
bra
gtu,bhundthougt
mov
ACCAL,W0
mov
ACCBL,W1
cp
W0,W1
bra
ltu,btenthoutest
bhundthougt:
SUB
A
inc.b
W6,W6
bra
bhundthoutest
;(10,000,000 in hex)
;
;
;
;;First Check if ACCAH >= ACCBH
;
;
;no, no ten millions
;definetly some ten millions
;ACCAH = ACCBH
;Check LSBs
;
;no tens of millions
;Subtract ten million from incoming number
;increment BCD digit
;repeat
;(1,000,000 in hex)
;
;
;
;;First Check if ACCAH >= ACCBH
;
;
;no, no millions
;definetly some millions
;ACCAH = ACCBH
;Check LSBs
;
;no millions
;Subtract a million from incoming number
;increment BCD digit
;repeat
;(100,000 in hex)
;
;
;
;;First Check if ACCAH >= ACCBH
;
;
;no, no hundred thousands
;definetly some hundred thousands
;ACCAH = ACCBH
;Check LSBs
;
;no hundred thousands
;Subtract a million from incoming number
;increment BCD digit
;repeat
174
btenthoutest:
mov
#0x0000,W0
mov
W0,ACCBH
mov
#0x2710,W0
mov
W0,ACCBL
;Check if ACCA is > than ACCB
mov
ACCAH,W0
mov
ACCBH,W1
cp
W0,W1
bra
ltu,bthoutestr
bra
gtu,btenthougt
mov
ACCAL,W0
mov
ACCBL,W1
cp
W0,W1
bra
ltu,bthoutestr
btenthougt:
SUB
A
inc.b
W5,W5
bra
btenthoutest
bthoutestr:
mov
ACCAL,W0
bthoutest:
mov
#0x03E8,W1
cp
W0,W1
bra
ltu,bhundtest
sub
inc.b
bra
W0,W1,W0
W4,W4
bthoutest
bhundtest:
mov
#0x0064,W1
cp
W0,W1
bra
ltu,btentest
sub
inc.b
bra
btentest:
mov
cp
bra
sub
inc.b
bra
W0,W1,W0
W3,W3
bhundtest
#0x000A,W1
W0,W1
ltu,bonetest
W0,W1,W0
W2,W2
btentest
;(10,000 in hex)
;
;
;
;;First Check if ACCAH >= ACCBH
;
;
;no, no ten thousands
;definetly some ten thousands
;ACCAH = ACCBH
;Check LSBs
;
;no tens of thousands
;Subtract a million from incoming number
;increment BCD digit
;repeat
;No need for a 32-bit accumulator anymore
;(1,000 in hex)
;
;Check if W0 >= 1,000
;it is
;Subtract 1,000 from Incoming number
;Increment BCD digit
;repeat
;(100 in hex)
;
;Check if W0 >= 100
;it is
;Subtract 100 from Incoming number
;Increment BCD digit
;repeat
;(10 in hex)
;
;Check if W0 >= 10
;it is
;Subtract 10 from Incoming number
;Increment BCD digit
;repeat
bonetest:
;Rest are ones currently stored in W0
mov
W0,W1
;
add
add
add
add
add
add
add
add
add
#0x30,W1
#0x30,W2
#0x30,W3
#0x30,W4
#0x30,W5
#0x30,W6
#0x30,W7
#0x30,W8
#0x30,W9
;Convert BCD to ASCII
;
;
;
;
;
;
;
;
175
add
#0x30,W10
;
return
;done
;***********************************************
;*************************************************************
;********Magnetometer ROUTINES********************************
;****Magnetometer Init**************************
Magnetometer_Init:
;Setup Pins as required
;SSbar
bclr
MSSTRIS,#MSSPIN
;SSbar an output
bset
MSSPORT,#MSSPIN
;SSbar high
;Reset
bclr
MRESTRIS,#MRESPIN
;Reset an output
bclr
MRESPORT,#MRESPIN
;Reset low
;SCLK
bclr
MSCLKTRIS,#MSCLKPIN ;SCLK an output
bclr
MSCLKPORT,#MSCLKPIN ;SCLK low
;MOSI
bclr
MMOSITRIS,#MMOSIPIN ;MOSI an output
bclr
MMOSIPORT,#MMOSIPIN ;MOSI low
;MISO
bset
MMISOTRIS,#MMISOPIN ;MISO an input
nop
;
;DRDY
bset
MDRDYTRIS,#MDRDYPIN ;DRDY an input
clr
XMAG
;Clear storage variables
clr
YMAG
;
clr
ZMAG
;
return
;done with init
;***********************************************
;*********************************************************
;Mag_Read_XYZ - Read all Magnetometer axes
;
;Reads all axes of magnetometer data and stores the results
;in XMAG,YMAG, and ZMAG respectively.
;*********************************************************
Mag_Read_XYZ:
mov
#0x0061,W0
;X-Axis
call
Magnetometer_Read
;Get Reading
mov
W0,XMAG
;Store Reading
mov
call
mov
#0x0062,W0
Magnetometer_Read
W0,YMAG
;Y-Axis
;Get Reading
;Store Reading
mov
call
mov
#0x0063,W0
Magnetometer_Read
W0,ZMAG
;Z-Axis
;Get Reading
;Store Reading
return
;*********************************************************
;****Magnetometer Read**************************
;W0 includes COMMAND BYTE (choose axis)
Magnetometer_Read:
bclr
MSSPORT,#MSSPIN
;SSbar low
nop
;
nop
;
bset
MRESPORT,#MRESPIN
;Reset line high
nop
;
nop
;
nop
;
bclr
MRESPORT,#MRESPIN
;Reset line low
176
nop
;
nop
;
;W0 should have COMMAND BYTE
call
Mag_Shift_Out
;Shift out Command Byte
Mag_wait:
btss
MDRDYPORT,#MDRDYPIN ;Wait till Data is Ready
goto
Mag_wait
;
;Data Ready, Shift in 16 bits
clr
W0
;All bits clear to start
call
Mag_Shift_In
;shift in highest 8 bits
swap
W0
;swap low for high byte
call
Mag_Shift_In
;shift in lowest 8 bits
bset
MSSPORT,#MSSPIN
;SSbar high
return
;***********************************************
;****Magnetometer Clock Pulse*******************
Mag_Clock_Pulse:
bset
MSCLKPORT,#MSCLKPIN ;Clock Pin High
nop
;
nop
;
nop
;
nop
;
bclr
MSCLKPORT,#MSCLKPIN ;Clock Pin Low
return
;
;***********************************************
;****Magnetometer Shift In**********************
Mag_Shift_In:
;Low byte of W0 assumed clear
btsc
MMISOPORT,#MMISOPIN ;Read in MSB
bset
W0,#7
;
call
Mag_Clock_Pulse
;Clock bit through
nop
btsc
bset
call
;
MMISOPORT,#MMISOPIN ;Read in #6
W0,#6
;
Mag_Clock_Pulse
;Clock bit through
nop
btsc
bset
call
;
MMISOPORT,#MMISOPIN ;Read in #5
W0,#5
;
Mag_Clock_Pulse
;Clock bit through
nop
btsc
bset
call
;
MMISOPORT,#MMISOPIN ;Read in #4
W0,#4
;
Mag_Clock_Pulse
;Clock bit through
nop
btsc
bset
call
;
MMISOPORT,#MMISOPIN ;Read in #3
W0,#3
;
Mag_Clock_Pulse
;Clock bit through
nop
btsc
bset
call
;
MMISOPORT,#MMISOPIN ;Read in #2
W0,#2
;
Mag_Clock_Pulse
;Clock bit through
nop
btsc
;
MMISOPORT,#MMISOPIN ;Read in #1
177
bset
call
W0,#1
Mag_Clock_Pulse
;
;Clock bit through
nop
;
btsc
MMISOPORT,#MMISOPIN ;Read in #0
bset
W0,#0
;
call
Mag_Clock_Pulse
;Clock bit through
return
;
;***********************************************
;****Magnetometer Shift Out*********************
Mag_Shift_Out:
bclr
MMOSIPORT,#MMOSIPIN ;Assume Bit Clear
btsc
W0,#7
;Send out MSB (#7)
bset
MMOSIPORT,#MMOSIPIN ;Bit Set
call
Mag_Clock_Pulse
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#6
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#6)
;Bit Set
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#5
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#5)
;Bit Set
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#4
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#4)
;Bit Set
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#3
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#3)
;Bit Set
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#2
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#2)
;Bit Set
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#1
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#1)
;Bit Set
;Clock bit through
bclr
btsc
bset
call
MMOSIPORT,#MMOSIPIN
W0,#0
MMOSIPORT,#MMOSIPIN
Mag_Clock_Pulse
;Assume Bit Clear
;Send out MSB (#0)
;Bit Set
;Clock bit through
return
;
;***********************************************
;*************************************************************
;********UART 2 ROUTINES**************************************
;****UART 2 Initialization***********************
UART2_Init:
;Make sure module isn't disabled
bclr
PMD1,#U2MD
;Enable UART2
;Setup the port pins
178
;Setup UART Regisiters
mov
#BAUD_NUM2,W0
mov
W0,U2BRG
mov
#U2STA_C,W0
mov
W0,U2STA
mov
#U2MODE_C,W0
mov
W0,U2MODE
bset
U2STA,#UTXEN
;Setup Baud Rate
;
;Setup interrupts, etc
;
;Setup Baud rate High Reg, etc
;
;set transmit enabled
;Setup UART Interrupts
bset
INTCON1,#NSTDIS
;Disable Interrupt Nesting
bclr
IFS1,#U2RXIF
;Clear Recieve Interrupt Flag
bset
IEC1,#U2RXIE
;Enable Recieve Interrupt
return
;done
;************************************************
;*****Computer Transmit Routine******************
computer_transmit:
push
SR
;Save Current status register
push
W1
;
mov
#0x00E0,W1
;Disable Interrupts
mov
W1,SR
;
btsc
U2FLAGS,#1
;Last byte sent out to computer?
goto
computer_transmit_set ;no, reset multiplexor
call
bt_transmit_check
;yes, send byte out
call
U2_Transmit
;still have to select bt or rs-232 though
bclr
U2FLAGS,#1
;Indicate byte sent to computer
pop
W1
;
pop
SR
;Restore SR, enable interrupts
return
;
computer_transmit_set:
btss
U2STA,#TRMT
;Byte still being transmitted?
goto
computer_transmit_set ;yes, wait for completion
;all previous bytes transmitted
bclr
U2FLAGS,#1
;Indicate byte sent to computer
call
bt_transmit_check
;Select Bluetooth or RS-232
call
U2_Transmit
;
pop
W1
;
pop
SR
;Restore SR, enable interrupts
return
;
;************************************************
;*****UART 2 Transmit Routine********************
U2_Transmit:
bset
U2STA,#UTXEN
;set transmit enabled
mov
W0,U2TXREG
;send out W0
U2_TransWait:
nop
btsc
U2STA,#UTXBF
;Check if transmit buffer is full
goto
U2_TransWait
;buffer full, wait
return
;buffer has room for more, return
;************************************************
;*****UART 2 Disable Transmit Routine************
U2_TransmitDisable:
btss
U2STA,#TRMT
;Check that last byte has been sent out
goto
U2_TransmitDisable
;no, wait
bclr
U2STA,#UTXEN
;last byte out, disable transmit
return
;
;************************************************
;*****UART 2 Recieve Interrupt Routine***********
__U2RXInterrupt:
;Save context
push
W0
;Save W0
179
push
push
W1
W2
;
;
btsc
goto
U2STA,#OERR
U2OverrunError
;Check for overrun error
;overrun error occurred
btsc
goto
U2STA,#FERR
U2FramingError
;Check for framing error
;framing error detected
clr
W0
U2ReadByte:
;Byte(s) are ready to be read
bclr
U2FLAGS,#0
mov
mov
add
U2RNUM,WREG
#U2RECEIVE,W1
W0,W1,W2
;Clear UART2 Interrupt Occurred? Flag
;Load Address Location
;
;
mov
mov.b
U2RXREG, W0
W0,[W2++]
;
;Save received word
btsc
call
btsc
call
btsc
MAIN_FLAGS,#6
U1_Transmit
MAIN_FLAGS,#5
bt_transmit
MAIN_FLAGS,#8
call
data_trans
;GPS COM MODE?
;yes, send character out to GPS unit
;Echo Character?
;yes
;Datalog Mode?
;(Logs all incoming characters to data logger)
;yes
inc
mov
cp
bra
U2RNUM
#30,W0
U2RNUM
GEU,u2rxint_clr
;Increment Received Word Count
;Check if byte count too high
;
;byte count >= 30, clear buffer
;Byte has been dealt with
btsc
U2STA,#URXDA
goto
U2ReadByte
;Check if another byte is available
;yes, read it
;Restore context
pop
W2
pop
W1
pop
W0
bclr
IFS1,#U2RXIF
retfie
;
;
;Restore W0
;Clear Recieve Interrupt Flag
;return
U2OverrunError:
mov
#U2ORERR,W2
mov
#11,W1
call
STRING_DISPLAY
mov
#0x0001,W1
call
NEWLINE_DISPLAY
pop
pop
pop
bclr
bclr
retfie
W2
W1
W0
U2STA,#OERR
IFS1,#U2RXIF
U2FramingError:
mov
#U2FRERR,W2
mov
#11,W1
;Send out error message
;
;
;New line
;
;
;
;Restore W0
;Clear error
;Clear Recieve Interrupt Flag
;
;Send out error message
;
180
call
mov
call
pop
pop
pop
bclr
bclr
retfie
STRING_DISPLAY
#0x0001,W1
NEWLINE_DISPLAY
W2
W1
W0
U2STA,#FERR
IFS1,#U2RXIF
;
;New line
;
;
;
;Restore W0
;Clear error
;Clear Recieve Interrupt Flag
;
u2rxint_clr:
clr
U2RNUM
;too many bytes, clear buffer
pop
W2
;
pop
W1
;
pop
W0
;Restore W0
bclr
IFS1,#U2RXIF
;Clear Recieve Interrupt Flag
retfie
;
;************************************************
;*************************************************************
;*********************************************************
;com_transmit - Computer Transmit Routine
;
;Sends value in W0 out to the Computer
;*********************************************************
com_transmit:
bclr
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
bset
U2SS_PORT,#TXSS1
;the Computer RX
call
U2_Transmit
;
return
;
;*********************************************************
;*********************************************************
;bt_transmit - Bluetooth Transmit Routine
;
;Sends value in W0 out via Bluetooth
;*********************************************************
bt_transmit:
bset
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
bclr
U2SS_PORT,#TXSS1
;the BT RX
call
U2_Transmit
;
return
;
;*********************************************************
;*********************************************************
;bt_settx - Set Bluetooth to TX Device
;
;Selects bluetooth as TX device
;*********************************************************
bt_settx:
bset
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
bclr
U2SS_PORT,#TXSS1
;the BT RX
return
;
;*********************************************************
;*********************************************************
;bt_receive - Bluetooth Receive Mode
;
;Forces UART 2 to recieve from the bluetooth module
;regardless of whether there is an active connection.
;*********************************************************
bt_receive:
bset
U2SS_PORT,#RXSS0
;Connect UART2 RX to the
;
181
nop
bclr
U2SS_PORT,#RXSS1
;Bluetooth TX
return
;
;*********************************************************
;*********************************************************
;bt_conn_check - Bluetooth Connection Check
;
;Forces UART 2 to recieve from the bluetooth module
;regardless of whether there is an active connection.
;*********************************************************
bt_conn_check:
btsc
INT_PORT,#BT_CONN
;Check for bluetooth connection
bset
MAIN_FLAGS,#9
;Bluetooth connected, set flag
btss
INT_PORT,#BT_CONN
;Check for bt connection
bclr
MAIN_FLAGS,#9
;Bluetooth not connected, clear flag
return
;
;*********************************************************
;********MAIN ROUTINES***************************************
;*********************************************************
;bt_receive_check - Bluetooth Receive Check
;
;Check whether bluetooth connection is active, if so set
;UART 2 to receive from the bluetooth module. If
;bluetooth is not active, set UART 2 to receive from the
;computer.
;*********************************************************
bt_receive_check:
btsc
INT_PORT,#BT_CONN
;Check for bluetooth connection
goto
bt_connected
;Bluetooth connected
bt_unconnected:
bclr
U2SS_PORT,#RXSS0
;Connect UART2 RX to the
nop
bset
U2SS_PORT,#RXSS1
;Computer TX
return
;
bt_connected:
bset
U2SS_PORT,#RXSS0
;Connect UART2 RX to the
nop
bclr
U2SS_PORT,#RXSS1
;Bluetooth TX
return
;
;*********************************************************
;*********************************************************
;bt_transmit_check - Bluetooth Transmit Check
;
;Check whether bluetooth connection is active, if so set
;UART 2 to transmit to the bluetooth module. If
;bluetooth is not active, set UART 2 to transmit to the
;computer.
;*********************************************************
bt_transmit_check:
btsc
INT_PORT,#BT_CONN
;Check for bluetooth connection
goto
btt_connected
;Bluetooth connected
btt_unconnected:
bclr
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
;
bset
U2SS_PORT,#TXSS1
;the Computer RX
return
;
btt_connected:
bset
U2SS_PORT,#TXSS0
;Connect UART2 TX to
nop
;
bclr
U2SS_PORT,#TXSS1
;the BT RX
return
;
182
;*********************************************************
;*********************************************************
;main_init - Main Init Routine
;
;Initializes all pins which wouldn't be initialized
;otherwise
;*********************************************************
main_init:
bclr
U2SS_TRIS,#RXSS0
;UART2 Source Select
nop
bclr
U2SS_TRIS,#RXSS1
;Pins Outputs
nop
bclr
U2SS_TRIS,#TXSS0
;
nop
bclr
U2SS_TRIS,#TXSS1
;
nop
bclr
U2SS_PORT,#RXSS0
;UART2 Default Source
nop
bset
U2SS_PORT,#RXSS1
;RX - COMPUTER
nop
bclr
U2SS_PORT,#TXSS0
;TX - COMPUTER
nop
bset
U2SS_PORT,#TXSS1
;
nop
bset
INT_TRIS,#GPS_INT
;GPS 1PPS, INPUT
nop
bset
INT_TRIS,#BT_CONN
;BT CONN, INPUT
return
;*********************************************************
;*************************************************************
;********DELAY ROUTINES***************************************
;*********************************************************
;delay1s ;
;Delays ~ 1 second
;*********************************************************
delay1s:
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
call
delay100ms
;
return
;*********************************************************
;*********************************************************
;delay100ms ;
;Delays 100ms
;*********************************************************
delay100ms:
mov
#20,W4
;delay ~100ms
delayloop100ms:
call
delay5ms
;
dec
W4,W4
;
bra
NZ,delayloop100ms
;
return
;
;
183
delay5ms:
mov
#40,W2
;load counter
delayloop:
call
delay125us
;delay 125us
dec
W2,W2
;Delayed 5ms?
bra
NZ,delayloop
;
return
;yep
;*********************************************************
;*********************************************************
;delay125us ;
;Delays 125us
;*********************************************************
delay125us:
mov
#0x014B,W3
;load counter (0x014B for Fcyl=8MHz)
delayloop2:
;
dec
W3,W3
;
bra
NZ,delayloop2
;Delayed 125us?
nop
nop
return
;yep
;*********************************************************
;*********************************************************
;delay71us ;
;Delays 71us
;*********************************************************
delay71us:
mov
#0x00BB,W3
;load counter (0x014B for Fcyl=8MHz)
delayloop271:
;
dec
W3,W3
;
bra
NZ,delayloop271
;Delayed 71us?
nop
nop
return
;yep
;*********************************************************
;*************************************************************
.end
184
;**********************************************************************************
;******************* ACOUSTIC PROCESSOR MICROCONTROLLER CODE **********************
;**********************************************************************************
.equ __33FJ128GP306, 1
.include "p33FJ128GP306.inc"
;..............................................................................
;Configuration bits:
;..............................................................................
config __FOSC, FCKSM_CSDCMD & OSCIOFNC_ON & POSCMD_EC
;Turn off clock
switching and
;fail-safe clock monitoring and
;use the External Clock as the
;system clock
config __FWDT, FWDTEN_OFF & WINDIS_OFF;Turn off Watchdog Timer
config __FPOR, FPWRT_PWR128
;Set Power on Timer 128 mSec
config __FOSCSEL, FNOSC_PRIPLL & IESO_OFF & TEMP_OFF ;Setup Oscillator
(w/PLL)
config __FGS, GSS_OFF & GCP_OFF & GWRP_OFF ;Disable Code protect
config __FSS, RSS_NO_RAM & SSS_NO_FLASH & SWRP_WRPROTECT_OFF ;No security
config __FBS, RBS_NO_RAM & BSS_NO_FLASH & BWRP_WRPROTECT_OFF ;
;..............................................................................
;Program Specific Constants (literals used in code)
;..............................................................................
;----ACOUSTIC PROCESSOR EQUATES------------------.equ
AP_PORT,PORTG
;
.equ
AP_TRIS,TRISG
;
.equ
AP_SS,9
;
.equ
AP_SDO,8
;
.equ
AP_SCK,6
;
.equ
AP_SDI,7
;
.equ
AP_APORT,PORTC
;
.equ
AP_AIO1,2
;
;----DIGITAL POTENTIOMETER EQUATES-------------.equ
DP_CPORT,PORTG
;Control Line Port
.equ
DP_CTRIS,TRISG
;
.equ
DP_CS_PRE,0
;
.equ
DP_CS_PST,14
;
.equ
DP_SCLK_PRE,12
;
.equ
DP_SCLK_PST,13
;
.equ
.equ
.equ
.equ
.equ
.equ
.equ
.equ
;----TRANSDUCER
.equ
.equ
DP_DPORT,PORTD
;Data Line Port
DP_DTRIS,TRISD
;
DP_DIN1_PRE,2
;Pre-Amp Digital Pot Line
DP_DIN1_PST,3
;Post-Amp Digital Pot Line
DP_DIN2_PRE,4
;Pre-Amp Digital Pot Line
DP_DIN2_PST,5
;Post-Amp Digital Pot Line
DP_DIN3_PRE,6
;Pre-Amp Digital Pot Line
DP_DIN3_PST,7
;Post-Amp Digital Pot Line
DRIVER EQUATES-----------------TDC_PORT,PORTD
;
TDC_TRIS,TRISD
;
185
.equ
.equ
TDC_PHASE,1
TDC_ENABLE,0
;
;
.equ
.equ
.equ
.equ
TDC_EPORT,PORTC
TDC_ETRIS,TRISC
TDC_MODE,14
TDC_NFLT,13
;
;
;
;
;----TONE DECODER EQUATES----------------------.equ
TD_PORT,PORTD
;
.equ
TD_TRIS,TRISD
;
.equ
TD_OUT1,8
;
.equ
TD_OUT2,9
;
.equ
TD_OUT3,10
;
;----ANALOG INPUT EQUATES----------------------.equ
AI_PORT,PORTB
;
.equ
AI_TRIS,TRISB
;
.equ
AI_PRE1,4
;
.equ
AI_POST1,0
;
.equ
AI_PRE2,1
;
.equ
AI_POST2,2
;
.equ
AI_PRE3,5
;
.equ
AI_POST3,3
;
;----LED EQUATES-------------------------------.equ
LED_PORT,PORTF
;
.equ
LED_TRIS,TRISF
;
.equ
LED1,6
;
.equ
LED2,2
;
.equ
LED3,3
;
;..............................................................................
;Global Declarations:
;..............................................................................
.global
.global
.global
;.global
__reset
__SPI2Interrupt
__SPI2ErrInterrupt
__T1Interrupt
;The label for the first line of code.
;
;
;Declare Timer 1 ISR name global
;..............................................................................
;Constants stored in Program space
;..............................................................................
;
.section .myconstbuffer, code
;
.palign 2
;Align next word stored in Program space to an
;
;address that is a multiple of 2
;ps_coeff:
;
.hword
0x0002, 0x0003, 0x0005, 0x000A
;..............................................................................
;Uninitialized variables
;..............................................................................
TEMP:
PRE1:
PRE2:
PRE3:
POST1:
POST2:
.section vars,bss,address(0x900)
.space 2
.space 2 ;default 0x00FF
.space 2 ;default 0x00FF
.space 2 ;default 0x00FF
.space 2 ;default 0x00FF
.space 2 ;default 0x00FF
186
POST3:
.space 2 ;default 0x00FF
BIGTOH:
.space 2 ;default 0x0131
BIGTOL:
.space 2 ;default 0x2D00
SMALLTOH:
.space 2 ;default 0x0000
SMALLTOL:
.space 2 ;default 0x4E20
TDC_DLYVAL: .space 2 ;default #145 = 45khz @ 40 MIPS
TDC_NUMCYC: .space 2 ;default #45 (1ms)
SPITRANSMIT: .space 20
SPIRECEIVE: .space 50
SPIRNUM:
.space 2
SPITNUM:
.space 2
SPIFLAGS:
.space 2
RANGETIMEH: .space 2
RANGETIMEL: .space 2
TRANSTIME1: .space 2
TRANSTIME2: .space 2
TRANSTIME3: .space 2
TRANSORDER: .space 2
;0,0,0 = No Returned Signal Timeout
;0,0,1 = 1,2,3
;0,1,0 = 1,3,2
;0,1,1 = 2,1,3
;1,0,0 = 2,3,1
;1,0,1 = 3,1,2
;1,1,0 = 3,2,1
;1,1,1 = Not All Returned Timeout
;..............................................................................
;Uninitialized variables in Y-space in data memory
;..............................................................................
;
;y_input:
.section .ybss, bss, ymemory
.space 2*SAMPLES
;..............................................................................
;Initialized variables in data memory
;..............................................................................
;..............................................................................
;Uninitialized variables in Near data memory (Lower 8Kb of RAM)
;..............................................................................
;
.section .nbss, bss, near
.section vars,bss,address(0x900)
;..............................................................................
;Code Section in Program Memory
;..............................................................................
.text
__reset:
mov #__SP_init, W15
mov #__SPLIM_init, W0
mov W0, SPLIM
nop
;Start of Code section
;Initalize the Stack Pointer
;Initialize the Stack Pointer Limit Register
;Add NOP to follow SPLIM initialization
main:
;Initialize PLL to proper frequency
;PLL will work on startup if 4mHz<Fin<8mHz
;(it was checked that PLL settings will within
;limits during this PLL update process)
mov
#0x0000,W0
;PLLPRE=2,PLLPOST=2 (for 40MIPS)
mov
W0,CLKDIV
;
187
mov
mov
#0x004E,W0
W0,PLLFBD
;Set PLL Multiplier=80 (for 40MIPS)
;
;Disable all modules
mov
#0xFFFF,W0
mov
W0,PMD1
mov
W0,PMD2
mov
W0,PMD3
;
;
;
;
;Disable all open-drains
mov
#0x0000,W0
mov
W0,ODCD
mov
W0,ODCF
mov
W0,ODCG
;
;Port D
;Port F
;Port G
;Disable Analog Function of I/O Pins
mov
#0xFFFF,W0
;
mov
W0,AD1PCFGH
;
mov
W0,AD1PCFGL
;
;Initialize
call
call
call
call
call
call
call
Everything
DP_INIT
TD_INIT
TDC_INIT
LED_INIT
Timer45_init
SPI_INIT
IC_INIT
;Initialize
;Initialize
;Initialize
;Initialize
;Initialize
;Initialize
;Initialize
Digital Potentiometers
Tone Decoder
Transducer Driver Circuit
LEDs
Timer
SPI
Input Capture
main_loop:
nop
bset
LED_PORT,#LED1
;
cp0
SPIRNUM
;Wait for a command to be received
bra
Z,main_loop
;
;Byte Received, Check for a command
bclr
LED_PORT,#LED1
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
mov
#0X474D,W0
;'GM'
cp
SPIRECEIVE
;
bra
Z,GMCMD
;Get Measurement Command Received
mov
cp
bra
#0X4742,W0
SPIRECEIVE
Z,GBCMD
;'GB'
;
;Get Bearing Command Received
mov
cp
bra
#0X5352,W0
SPIRECEIVE
Z,SRCMD
;'SR'
;
;Send Reading Command Received
mov
cp
bra
#0X5152,W0
SPIRECEIVE
Z,QRCMD
;'QR'
;
;Quit Reading Command Received
mov
cp
bra
#0X5450,W0
SPIRECEIVE
Z,TPCMD
;'TP'
;
;Transmit Pulse Command Received
mov
cp
bra
#0X5350,W0
SPIRECEIVE
Z,SPCMD
;'SP'
;
;Set Parameters Command Received
188
mov
cp
bra
#0X5241,W0
SPIRECEIVE
Z,SPCMD
;'RA'
;
;Read ADC Command Received
;No recognized command received, clear it
clr
SPIRNUM
;
goto
main_loop
;
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GMCMD: ;GET MEASUREMENT COMMAND
bset
LED_PORT,#LED3
;
clr
RANGETIMEH
;Clear variables
clr
RANGETIMEL
;
clr
TRANSTIME1
;
clr
TRANSTIME2
;
clr
TRANSTIME3
;
clr
TRANSORDER
;
call
call
TDC_TOUT
IC_START
;Transmit Pulse, start big timeout
;Capture Ping
mov
#SPITRANSMIT,W2
;Formulate Message in memory
mov
mov
#0X474D,W0
W0,[W2++]
;1 'GM'
;
mov
mov
RANGETIMEH,W0
W0,[W2++]
;2
;
mov
mov
RANGETIMEL,W0
W0,[W2++]
;3
;
mov
mov
TRANSORDER,W0
W0,[W2++]
;4
;
mov
mov
TRANSTIME1,W0
W0,[W2++]
;5
;
mov
mov
TRANSTIME2,W0
W0,[W2++]
;6
;
mov
mov
TRANSTIME3,W0
W0,[W2++]
;7
;
mov
mov
#0x0007,W0
W0,SPITNUM
;
;
call
SPI_WRITE
;Send data out
bclr
LED_PORT,#LED3
;
clr
SPIRNUM
;clear byte count
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GBCMD: ;GET BEARING COMMAND
bset
LED_PORT,#LED2
;
clr
TRANSTIME1
;
clr
TRANSTIME2
;
clr
TRANSTIME3
;
clr
TRANSORDER
;
bclr
bclr
T4CON,#15
IFS1,#T5IF
;Ensure timer is stopped
;Clear interrupt flag
call
IC_START
;
189
mov
#SPITRANSMIT,W2
;Formulate Message in memory
mov
mov
#0X4742,W0
W0,[W2++]
;1 'GB'
;
mov
mov
TRANSORDER,W0
W0,[W2++]
;4
;
mov
mov
TRANSTIME1,W0
W0,[W2++]
;5
;
mov
mov
TRANSTIME2,W0
W0,[W2++]
;6
;
mov
mov
TRANSTIME3,W0
W0,[W2++]
;7
;
mov
mov
#0x0005,W0
W0,SPITNUM
;
;
call
SPI_WRITE
;Send data out
clr
SPIRNUM
;clear byte count
bclr
LED_PORT,#LED2
;
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SRCMD: ;Send Readings Command
nop
;Not yet implemented
clr
SPIRNUM
;clear byte count
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
QRCMD: ;Quit Readings Command
nop
;not yet implemented
clr
SPIRNUM
;clear byte count
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
TPCMD: ;Transmit Pulse Command
mov
#0x0002,W0
;Check that at least 2 bytes are waiting
cp
SPIRNUM
;
bra
LTU,TPCMD
;not enough bytes yet, wait
;
;
;
;
;
mov
mov
#SPIRECEIVE,W2
[++W2],W0
;Retrieve Pulse length
;
mov
mov
call
mov
TDC_NUMCYC,W5
W0,TDC_NUMCYC
TDC_TOUT
W5,TDC_NUMCYC
;Store previous number of cycles
;replace cycle count with that from command
;send out requested pulse
;Restore cycle count
mov
mov
mov
mov
call
#0X5450,W0
W0,SPITRANSMIT
#0x0001,W0
W0,SPITNUM
SPI_WRITE
;Send out 'TP'
;
;
;
;Send data out
clr
SPIRNUM
;clear byte count
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SPCMD: ;Set Parameters
mov
#0x000C,W0
;Check that at least 12 bytes are waiting
cp
SPIRNUM
;
bra
LTU,SPCMD
;not enough bytes yet, wait
190
;
;
;
;
;
mov
mov
mov
#SPIRECEIVE,W2
[++W2],W0
W0,TDC_NUMCYC
;
;Retrieve number of transmit cycles
;
mov
mov
[++W2],W0
W0,PRE1
;Retrieve Pre-Amp 1 Gain
;
mov
mov
[++W2],W0
W0,PRE2
;Retrieve Pre-Amp 2 Gain
;
mov
mov
[++W2],W0
W0,PRE3
;Retrieve Pre-Amp 3 Gain
;
mov
mov
[++W2],W0
W0,POST1
;Retrieve Post-Amp 1 Gain
;
mov
mov
[++W2],W0
W0,POST2
;Retrieve Post-Amp 2 Gain
;
mov
mov
[++W2],W0
W0,POST3
;Retrieve Post-Amp 3 Gain
;
mov
mov
[++W2],W0
W0,BIGTOH
;Retrieve Big Timeout High Word
;
mov
mov
[++W2],W0
W0,BIGTOL
;Retrieve Big Timeout Low Word
;
mov
mov
[++W2],W0
W0,SMALLTOH
;Retrieve Small Timeout High Word
;
mov
mov
[++W2],W0
W0,SMALLTOL
;Retrieve Small Timeout Low Word
;
mov
mov
mov
mov
call
#0X5350,W0
W0,SPITRANSMIT
#0x0001,W0
W0,SPITNUM
SPI_WRITE
;Send out 'SP'
;
;
;
;Send data out
clr
SPIRNUM
;clear byte count
mov
mov
mov
call
mov
mov
mov
call
PRE1,W0
PRE2,W1
PRE3,W2
DP_SET_PRE
POST1,W0
POST2,W1
POST3,W2
DP_SET_PST
;Set Pre-Amp Gain
;
;
;
;Set Post-Amp Gain
;
;
;
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
RACMD: ;Read ADC Command
nop
;not yet implemented
clr
SPIRNUM
;clear byte count
goto
main_loop
;return to main loop
;+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
191
;********SPI Routines*****************************************
;*********************************************************
;SPI_INIT SPI Initialization
;*********************************************************
SPI_INIT:
clr
SPIRNUM
;Clear Counters
clr
SPITNUM
;
clr
SPIFLAGS
;Clear Flags
bclr
AP_TRIS,#AP_SS
;Use SS pin for telling master
;that data is ready
bclr
PMD1,#SPI2MD
;Make sure module isn't disabled
clr
SPI2BUF
;Clear SPIxBUF register
clr
SPI2STAT
;
;if using interrupts:
;
bclr
IFS2,#SPI2IF
;
nop
bclr
IFS2,#SPI2EIF
;
nop
;
bset
IEC2,#SPI2IE
;
nop
;
bset
IEC2,#SPI2EIE
;
bset
IPC8,#9
;Set Interrupt Priority 3
nop
;
bclr
IPC8,#10
;
nop
;
bset
IPC8,#8
;
mov
#0x0400,W0
;Setup 1st SPI Control Register
mov
W0,SPI2CON1
;Slave Mode
mov
#0x0000,W0
;Setup 2nd SPI Control Register
mov
W0,SPI2CON2
;No framed SPI support
bclr
SPI2CON1,#SMP
;Clear SMP bit
bclr
SPI2STAT,#SPIROV
;Clear SPIROV bit
nop
bset
SPI2STAT,#SPIEN
;Enable SPI Operation
return
;*********************************************************
;*********************************************************
;__SPI2Interrupt SPI Interrupt Routine
;*********************************************************
__SPI2Interrupt:
push
W0
;
push
W1
;
push
W2
;
bclr
bclr
IFS2,#SPI2IF
SPIFLAGS,#0
;Clear Interrupt Flag
;Clear SPI Interrupt Occurred? Flag
cp0
bra
SPITNUM
NZ,SPIINTTRANSMIT
;Check if any data to be transmitted
;
rlnc
mov
add
SPIRNUM,WREG
#SPIRECEIVE,W1
W0,W1,W2
;Load Address Location
;
;
mov
mov
SPI2BUF,W0
W0,[W2++]
;Retrieve word
;Save received word
inc
SPIRNUM
SPIINTRETURN:
pop
W2
pop
W1
pop
W0
;Increment Received Word Count
;
;
;
192
retfie
;
SPIINTTRANSMIT:
dec
SPITNUM
;decrement Transmit Counter
mov
SPI2BUF,W0
;Read data out to prevent overflow
cp0
SPITNUM
;Check if any more data to be transmitted
bra
Z,SPITRANSNOMORE
;
bset
AP_PORT,#AP_SS
;ensure data ready line is high
goto
SPIINTRETURN
;
SPITRANSNOMORE:
bclr
AP_PORT,#AP_SS
;
goto
SPIINTRETURN
;
;*********************************************************
;*********************************************************
;__SPI2ErrInterrupt SPI ERROR Interrupt
;
;If an SPI overflow occurs, overflow flag must be cleared
;to re-enable reception
;*********************************************************
__SPI2ErrInterrupt:
bclr
IFS2,#SPI2EIF
;Clear interrupt flag
bclr
SPI2STAT,#SPIROV
;Clear SPIROV bit
retfie
;
;*********************************************************
;*********************************************************
;SPI_WRITE SPI Word Write
;
;Data to be written is to be stored at location SPITRANSMIT
;Number of words to be written stored in SPITNUM
;*********************************************************
SPI_WRITE:
mov
#SPITRANSMIT,W2
;Load start address
SPI_REWRITE:
cp0
SPITNUM
;Check if any data to be transmitted
bra
NZ,SPI_WRITEWORD
;
return
;no data to send
SPI_WRITEWORD:
bset
SPIFLAGS,#0
;Set interrupt check flag
mov
[W2++],W0
;
mov
W0,SPI2BUF
;Send Received word out
bset
AP_PORT,#AP_SS
;Set data ready line
SPI_WRITE_WAIT:
btsc
SPIFLAGS,#0
;Transfer occur yet?
goto
SPI_WRITE_WAIT
;no
goto
SPI_REWRITE
;yes
;*********************************************************
;*************************************************************
;********Digital Potentiometer Routines***********************
;*********************************************************
;DP_INIT - Digital Potentiometer Initialization
;*********************************************************
DP_INIT:
bclr
DP_CTRIS,#DP_CS_PRE
;CS Output
bset
DP_CPORT,#DP_CS_PRE
;
bclr
DP_CTRIS,#DP_CS_PST
;CS Output
bset
DP_CPORT,#DP_CS_PST
;
bclr
bset
bclr
bset
DP_CTRIS,#DP_SCLK_PRE
DP_CPORT,#DP_SCLK_PRE
DP_CTRIS,#DP_SCLK_PST
DP_CPORT,#DP_SCLK_PST
;SCLK Output
;
;SCLK Output
;
193
bclr
bclr
bclr
bclr
DP_DTRIS,#DP_DIN1_PRE
DP_DPORT,#DP_DIN1_PRE
DP_DTRIS,#DP_DIN1_PST
DP_DPORT,#DP_DIN1_PST
;DIN Output
;
;DIN Output
;
bclr
bclr
bclr
bclr
DP_DTRIS,#DP_DIN2_PRE
DP_DPORT,#DP_DIN2_PRE
DP_DTRIS,#DP_DIN2_PST
DP_DPORT,#DP_DIN2_PST
;DIN Output
;
;DIN Output
;
bclr
bclr
bclr
bclr
DP_DTRIS,#DP_DIN3_PRE
DP_DPORT,#DP_DIN3_PRE
DP_DTRIS,#DP_DIN3_PST
DP_DPORT,#DP_DIN3_PST
;DIN Output
;
;DIN Output
;
mov
mov
mov
mov
mov
mov
mov
#0x00FF,W0
W0,PRE1
W0,PRE2
W0,PRE3
W0,POST1
W0,POST2
W0,POST3
;Set minimum gain to start
;
;
;
;
;
;
mov
PRE1,W0
;Set Pre-Amp Gain
mov
PRE2,W1
;
mov
PRE3,W2
;
call
DP_SET_PRE
;
mov
POST1,W0
;Set Post-Amp Gain
mov
POST2,W1
;
mov
POST3,W2
;
call
DP_SET_PST
;
return
;
;*********************************************************
;*********************************************************
;DP_SET_PRE - Digital Potentiometer Set Value
;
;Uses value in W0(7:0) to set Pre-Amp 1 Potentiometer
;Uses value in W1(7:0) to set Pre-Amp 2 Potentiometer
;Uses value in W2(7:0) to set Pre-Amp 3 Potentiometer
;*********************************************************
DP_SET_PRE:
bclr
DP_CPORT,#DP_SCLK_PRE
;Clock Pin Low
nop
;
nop
nop
nop
bclr
DP_CPORT,#DP_CS_PRE
;CS Low
nop
;
nop
nop
nop
;Bit 7 (MSb)
bclr
DP_DPORT,#DP_DIN1_PRE
btsc
W0,#7
bset
DP_DPORT,#DP_DIN1_PRE
nop
bclr
DP_DPORT,#DP_DIN2_PRE
btsc
W1,#7
bset
DP_DPORT,#DP_DIN2_PRE
nop
bclr
DP_DPORT,#DP_DIN3_PRE
btsc
W2,#7
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
194
bset
call
;Bit 6
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 5
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 4
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 3
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 2
bclr
btsc
bset
nop
bclr
btsc
bset
nop
DP_DPORT,#DP_DIN3_PRE
DP_Clock_Pulse_PRE
;Bit Set
;Clock bit through
DP_DPORT,#DP_DIN1_PRE
W0,#6
DP_DPORT,#DP_DIN1_PRE
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
DP_DPORT,#DP_DIN2_PRE
W1,#6
DP_DPORT,#DP_DIN2_PRE
DP_DPORT,#DP_DIN3_PRE
W2,#6
DP_DPORT,#DP_DIN3_PRE
DP_Clock_Pulse_PRE
DP_DPORT,#DP_DIN1_PRE
W0,#5
DP_DPORT,#DP_DIN1_PRE
DP_DPORT,#DP_DIN2_PRE
W1,#5
DP_DPORT,#DP_DIN2_PRE
DP_DPORT,#DP_DIN3_PRE
W2,#5
DP_DPORT,#DP_DIN3_PRE
DP_Clock_Pulse_PRE
DP_DPORT,#DP_DIN1_PRE
W0,#4
DP_DPORT,#DP_DIN1_PRE
DP_DPORT,#DP_DIN2_PRE
W1,#4
DP_DPORT,#DP_DIN2_PRE
DP_DPORT,#DP_DIN3_PRE
W2,#4
DP_DPORT,#DP_DIN3_PRE
DP_Clock_Pulse_PRE
DP_DPORT,#DP_DIN1_PRE
W0,#3
DP_DPORT,#DP_DIN1_PRE
DP_DPORT,#DP_DIN2_PRE
W1,#3
DP_DPORT,#DP_DIN2_PRE
DP_DPORT,#DP_DIN3_PRE
W2,#3
DP_DPORT,#DP_DIN3_PRE
DP_Clock_Pulse_PRE
DP_DPORT,#DP_DIN1_PRE
W0,#2
DP_DPORT,#DP_DIN1_PRE
DP_DPORT,#DP_DIN2_PRE
W1,#2
DP_DPORT,#DP_DIN2_PRE
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
195
bclr
DP_DPORT,#DP_DIN3_PRE
btsc
W2,#2
bset
DP_DPORT,#DP_DIN3_PRE
call
DP_Clock_Pulse_PRE
;Bit 1
bclr
DP_DPORT,#DP_DIN1_PRE
btsc
W0,#1
bset
DP_DPORT,#DP_DIN1_PRE
nop
bclr
DP_DPORT,#DP_DIN2_PRE
btsc
W1,#1
bset
DP_DPORT,#DP_DIN2_PRE
nop
bclr
DP_DPORT,#DP_DIN3_PRE
btsc
W2,#1
bset
DP_DPORT,#DP_DIN3_PRE
call
DP_Clock_Pulse_PRE
;Bit 0 (LSb)
bclr
DP_DPORT,#DP_DIN1_PRE
btsc
W0,#0
bset
DP_DPORT,#DP_DIN1_PRE
nop
bclr
DP_DPORT,#DP_DIN2_PRE
btsc
W1,#0
bset
DP_DPORT,#DP_DIN2_PRE
nop
bclr
DP_DPORT,#DP_DIN3_PRE
btsc
W2,#0
bset
DP_DPORT,#DP_DIN3_PRE
call
DP_Clock_Pulse_PRE
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
nop
;
nop
;
nop
;
nop
;
bset
DP_CPORT,#DP_CS_PRE
;CS High
nop
;
nop
;
nop
;
nop
;
bset
DP_CPORT,#DP_SCLK_PRE
;Clock Pin High
return
;
;*********************************************************
;*********************************************************
;DP_SET_PST - Digital Potentiometer Set Value
;
;Uses value in W0(7:0) to set Post-Amp 1 Potentiometer
;Uses value in W1(7:0) to set Post-Amp 2 Potentiometer
;Uses value in W2(7:0) to set Post-Amp 3 Potentiometer
;*********************************************************
DP_SET_PST:
bclr
DP_CPORT,#DP_SCLK_PST
;Clock Pin Low
nop
;
nop
;
nop
;
nop
;
bclr
DP_CPORT,#DP_CS_PST
;CS Low
nop
;
nop
;
nop
;
nop
;
;Bit 7 (MSb)
196
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 6
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 5
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 4
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
call
;Bit 3
bclr
btsc
bset
nop
bclr
btsc
bset
nop
bclr
btsc
bset
DP_DPORT,#DP_DIN1_PST
W0,#7
DP_DPORT,#DP_DIN1_PST
DP_DPORT,#DP_DIN2_PST
W1,#7
DP_DPORT,#DP_DIN2_PST
DP_DPORT,#DP_DIN3_PST
W2,#7
DP_DPORT,#DP_DIN3_PST
DP_Clock_Pulse_PST
DP_DPORT,#DP_DIN1_PST
W0,#6
DP_DPORT,#DP_DIN1_PST
DP_DPORT,#DP_DIN2_PST
W1,#6
DP_DPORT,#DP_DIN2_PST
DP_DPORT,#DP_DIN3_PST
W2,#6
DP_DPORT,#DP_DIN3_PST
DP_Clock_Pulse_PST
DP_DPORT,#DP_DIN1_PST
W0,#5
DP_DPORT,#DP_DIN1_PST
DP_DPORT,#DP_DIN2_PST
W1,#5
DP_DPORT,#DP_DIN2_PST
DP_DPORT,#DP_DIN3_PST
W2,#5
DP_DPORT,#DP_DIN3_PST
DP_Clock_Pulse_PST
DP_DPORT,#DP_DIN1_PST
W0,#4
DP_DPORT,#DP_DIN1_PST
DP_DPORT,#DP_DIN2_PST
W1,#4
DP_DPORT,#DP_DIN2_PST
DP_DPORT,#DP_DIN3_PST
W2,#4
DP_DPORT,#DP_DIN3_PST
DP_Clock_Pulse_PST
DP_DPORT,#DP_DIN1_PST
W0,#3
DP_DPORT,#DP_DIN1_PST
DP_DPORT,#DP_DIN2_PST
W1,#3
DP_DPORT,#DP_DIN2_PST
DP_DPORT,#DP_DIN3_PST
W2,#3
DP_DPORT,#DP_DIN3_PST
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
197
call
DP_Clock_Pulse_PST
;Bit 2
bclr
DP_DPORT,#DP_DIN1_PST
btsc
W0,#2
bset
DP_DPORT,#DP_DIN1_PST
nop
bclr
DP_DPORT,#DP_DIN2_PST
btsc
W1,#2
bset
DP_DPORT,#DP_DIN2_PST
nop
bclr
DP_DPORT,#DP_DIN3_PST
btsc
W2,#2
bset
DP_DPORT,#DP_DIN3_PST
call
DP_Clock_Pulse_PST
;Bit 1
bclr
DP_DPORT,#DP_DIN1_PST
btsc
W0,#1
bset
DP_DPORT,#DP_DIN1_PST
nop
bclr
DP_DPORT,#DP_DIN2_PST
btsc
W1,#1
bset
DP_DPORT,#DP_DIN2_PST
nop
bclr
DP_DPORT,#DP_DIN3_PST
btsc
W2,#1
bset
DP_DPORT,#DP_DIN3_PST
call
DP_Clock_Pulse_PST
;Bit 0 (LSb)
bclr
DP_DPORT,#DP_DIN1_PST
btsc
W0,#0
bset
DP_DPORT,#DP_DIN1_PST
nop
bclr
DP_DPORT,#DP_DIN2_PST
btsc
W1,#0
bset
DP_DPORT,#DP_DIN2_PST
nop
bclr
DP_DPORT,#DP_DIN3_PST
btsc
W2,#0
bset
DP_DPORT,#DP_DIN3_PST
call
DP_Clock_Pulse_PST
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;
;Assume Bit Clear
;Send out bit
;Bit Set
;Clock bit through
nop
;
nop
;
nop
;
nop
;
bset
DP_CPORT,#DP_CS_PST
;CS High
nop
;
nop
;
nop
;
nop
;
bset
DP_CPORT,#DP_SCLK_PST
;Clock Pin High
return
;
;*********************************************************
;*****DP Clock Pulse PRE**********************************
DP_Clock_Pulse_PRE:
bset
DP_CPORT,#DP_SCLK_PRE
;Clock Pin High
nop
;
nop
;
nop
;
nop
;
nop
;
bclr
DP_CPORT,#DP_SCLK_PRE
;Clock Pin Low
return
;
198
;*********************************************************
;*****DP Clock Pulse POST*********************************
DP_Clock_Pulse_PST:
bset
DP_CPORT,#DP_SCLK_PST
;Clock Pin High
nop
;
nop
;
nop
;
nop
;
nop
;
bclr
DP_CPORT,#DP_SCLK_PST
;Clock Pin Low
return
;
;*********************************************************
;*************************************************************
;********Tone Decoder Routines********************************
;TD_INIT - Tone Decoder Initialization
;*********************************************************
TD_INIT:
bset
TD_TRIS,#TD_OUT1
;Input
nop
bset
TD_TRIS,#TD_OUT2
;Input
nop
bset
TD_TRIS,#TD_OUT3
;Input
return
;
;*********************************************************
;*************************************************************
;********LED Routines*************************************
;LED_INIT - LED Initialization
;*********************************************************
LED_INIT:
bclr
LED_TRIS,#LED1
;Output
bclr
LED_PORT,#LED1
;
;
bclr
LED_TRIS,#LED2
;Output
; bclr
LED_PORT,#LED2
;
bclr
LED_TRIS,#LED3
;Output
bclr
LED_PORT,#LED3
;
return
;
;*********************************************************
;*************************************************************
;********Transducer Driver Routines***************************
;*********************************************************
;TDC_INIT - Transducer Driver Circuit Initialization
;*********************************************************
TDC_INIT:
bclr
TDC_ETRIS,#TDC_MODE
;Output
bset
TDC_EPORT,#TDC_MODE
;
bset
TDC_ETRIS,#TDC_NFLT
;Output
nop
bclr
bset
bclr
bclr
mov
mov
mov
mov
mov
mov
mov
TDC_TRIS,#TDC_PHASE
TDC_PORT,#TDC_PHASE
TDC_TRIS,#TDC_ENABLE
TDC_PORT,#TDC_ENABLE
#145,W0
W0,TDC_DLYVAL
#0x0131,W0
W0,BIGTOH
#0x2D00,W0
W0,BIGTOL
#0x0000,W0
;Output
;
;Output
;
;load counter
;
;Set Big Timeout Default Value
;
;
;
;
199
mov
W0,SMALLTOH
;Set Small Timeout Default Value
mov
#0x4E20,W0
;
mov
W0,SMALLTOL
;
mov
#45,W0
;
mov
W0,TDC_NUMCYC
;
return
;
;*********************************************************
;*********************************************************
;TDC_OUT - Transducer Driver Circuit Output
;*********************************************************
TDC_OUT:
bset
TDC_PORT,#TDC_ENABLE
;Start Output
nop
;
TDC_RPT:
bset
TDC_PORT,#TDC_PHASE
;Change Current Direction
call
TDC_DLY
;delay
nop
nop
;(make up for loop time)
nop
;
bclr
TDC_PORT,#TDC_PHASE
;Change Current Direction
call
TDC_DLY
;
nop
;match time output routine
goto
TDC_RPT
;
bclr
TDC_PORT,#TDC_ENABLE
;Stop Output
return
;
;*********************************************************
;*********************************************************
;TDC_TOUT - Transducer Driver Circuit Timed Output
;Amount of cycles to output passed via TDC_NUMCYC
;*********************************************************
TDC_TOUT:
mov
TDC_NUMCYC,W1
;Set number of cycles
mov
mov
mov
mov
call
bset
nop
TDC_TRPT:
bset
call
nop
nop
nop
bclr
call
dec
bra
BIGTOH,W0
W0,PR5
BIGTOL,W0
W0,PR4
Timer45_clear
TDC_PORT,#TDC_ENABLE
;Set Big Timout value
;
;
;
;Reset Timeout Timer
;Start Output
;
TDC_PORT,#TDC_PHASE
TDC_DLY
TDC_PORT,#TDC_PHASE
TDC_DLY
W1,W1
NZ,TDC_TRPT
;Change Current Direction
;delay
;(make up for loop time)
;
;Change Current Direction
;
;decrement cycle count
;if not yet 0, repeat
bclr
TDC_PORT,#TDC_ENABLE
;Stop Output
return
;
;*********************************************************
;*********************************************************
;TDC_DLY - Transducer Driver Circuit Delay (Sets Freq)
;*********************************************************
TDC_DLY:
mov
TDC_DLYVAL,W0
;load counter
;#145 = 45kHz @ 40 MIPS
TDC_DLY_2:
;
200
dec
W0,W0
;
bra
NZ,TDC_DLY_2
;Delayed Set Time?
return
;yep
;*********************************************************
;*************************************************************
;*************************************************************
;********Timer Routines***************************************
;*********************************************************
;Timer45_init - Timers 4/5 Init (use as a 32-bit timer)
;
;Setup Timer 4/5 to operated as a single 32-bit timer
;*********************************************************
Timer45_init:
bclr
PMD1,#T4MD
;Make sure timers are enabled
nop
;
bclr
PMD1,#T5MD
;
;mov
#0x0038,W0
;Setup 32-bit timer, 1:256 prescaler
mov
#0x0008,W0
;Setup 32-bit
timer, 1:1 prescaler
mov
W0,T4CON
;(Timer 4/5)
;Not setting up any interrupts
bclr
T4CON,#15
;Ensure timer is stopped
return
;
;*********************************************************
;*********************************************************
;Timer45_clear - Timers 4/5 Clear
;
;Clear 32-bit timer (Start at 0x00000000)
;*********************************************************
Timer45_clear:
bclr
T4CON,#15
;stop timer
mov
#0x0000,W0
;Write 0x0000 to MSW
mov
W0,TMR5HLD
;(Timer 4/5)
mov
W0,TMR4
;(Timer 4/5)
bclr
IFS1,#T5IF
;Clear interrupt flag
bset
T4CON,#15
;start timer
return
;
;*********************************************************
;*********************************************************
;Timer45_read - Timers 4/5 Read
;
;Read the 32-bit timer
;*********************************************************
Timer45_read:
mov
TMR4,W0
;Transfer the LSW into W1
mov
TMR5HLD, W1
;Transfer the MSW from the
;holding register to W0
return
;
;*********************************************************
;*********************************************************
;IC_INIT - Setup input capture
;
;Will setup the input capture feature to record the
;arrival of the three tone decoder inputs
;*********************************************************
IC_INIT:
mov
#0x0080,W0
;Ensure IC is disabled
mov
W0,IC1CON
;
mov
W0,IC2CON
;
mov
W0,IC3CON
;
bclr
PMD2,#IC1MD
;Make sure input capture modules
201
nop
bclr
PMD2,#IC2MD
;are not disabled
nop
bclr
PMD2,#IC3MD
;
bclr
PMD1,#T2MD
;Make sure timer2 module is enabled
mov
#0x8000,W0
;Setup Timer2 as 16-bit timer
mov
W0,T2CON
;
return
;
;*********************************************************
;*********************************************************
;IC_START - Start Input Capture Routine
;
;Will begin checking for incoming signals from the tone
;decoders. If no signals are detected within 500ms, it
;will timeout. If a signal is detected, it will wait up
;to 500us for the other signals to be detected as well.
;If these are not detected, a timeout will occur. If all
;signals are detected, the value of the 16-bit timer will
;be returned in the TRANSTIME variables. The 32-bit range
;time will be returned in RANGETIME variables.
;*********************************************************
;MUST SET PR REGISTERS FOR TIMEOUT BEFORE STARTING TIMER!!!!!
IC_START:
mov
#0x0082,W0
;Setup IC registers, falling edge detect
mov
W0,IC1CON
;
mov
W0,IC2CON
;
mov
W0,IC3CON
;
clr
TRANSORDER
;Clear transducer order register
;Detection Enabled, big Timeout Should've been Started
IC_WAIT:
btsc
IC1CON,#3
;Check if IC1 event occurred
goto
ICEO1
;IC1 Event Occurred
btsc
IC2CON,#3
;Check if IC2 event occurred
goto
ICEO2
;IC2 Event Occurred
btsc
IC3CON,#3
;Check if IC3 event occurred
goto
ICEO3
;IC3 Event Occurred
btsc
IFS1,#T5IF
;Check for big timeout
goto
IC_NORETURN
;no return signal received
goto
IC_WAIT
;
;Transducer 1 received signal first, start small Timeout
;wait for next 2 signals
ICEO1:
call
Timer45_read
;store ranging time
mov
W0,RANGETIMEL
;
mov
W1,RANGETIMEH
;
mov
IC1BUF,W0
;Retrieve event time
mov
W0,TRANSTIME1
;
mov
mov
mov
mov
call
ICEO1_wait:
btsc
goto
btsc
goto
btsc
SMALLTOH,W0
W0,PR5
SMALLTOL,W0
W0,PR4
Timer45_clear
;Set Small Timeout value
;
;
;
;Reset Timeout Timer
IC2CON,#3
ICEO12
IC3CON,#3
ICEO13
IFS1,#T5IF
;Check if IC2 event occurred
;IC2 Event Occurred 2nd
;Check if IC3 event occurred
;IC3 Event Occurred 2nd
;Check for small timeout
202
goto
IC_NOTALLRETURN
goto
ICEO1_wait
;1,2
ICEO12:
mov
IC2BUF,W0
mov
W0,TRANSTIME2
ICEO12_wait:
btsc
IC3CON,#3
goto
ICEO123
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO12_wait
;1,3
ICEO13:
mov
IC3BUF,W0
mov
W0,TRANSTIME3
ICEO13_wait:
btsc
IC2CON,#3
goto
ICEO132
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO13_wait
;1,2,3
ICEO123:
mov
#0x01,W0
mov
W0,TRANSORDER
mov
IC3BUF,W0
mov
W0,TRANSTIME3
return
;1,3,2
ICEO132:
mov
#0x02,W0
mov
W0,TRANSORDER
mov
IC2BUF,W0
mov
W0,TRANSTIME2
return
;not all transducers received signal
;
;Retrieve event time
;
;Check if IC3 event occurred
;IC3 Event Occurred last
;Check for small timeout
;not all transducers received signal
;
;Retrieve event time
;
;Check if IC2 event occurred
;IC2 Event Occurred last
;Check for small timeout
;not all transducers received signal
;
;Set Transducer Order
;
;Retrieve event time
;
;All 3 transducers responded.. return
;Set Transducer Order
;
;Retrieve event time
;
;All 3 transducers responded.. return
;Transducer 2 received signal first, start small Timeout
;wait for next 2 signals
ICEO2:
call
Timer45_read
;store ranging time
mov
W0,RANGETIMEL
;
mov
W1,RANGETIMEH
;
mov
IC2BUF,W0
;Retrieve event time
mov
W0,TRANSTIME2
;
mov
mov
mov
mov
call
ICEO2_wait:
btsc
goto
btsc
goto
btsc
goto
goto
;2,1
ICEO21:
mov
mov
SMALLTOH,W0
W0,PR5
SMALLTOL,W0
W0,PR4
Timer45_clear
;Set Small Timeout value
;
;
;
;Reset Timeout Timer
IC1CON,#3
ICEO21
IC3CON,#3
ICEO23
IFS1,#T5IF
IC_NOTALLRETURN
ICEO2_wait
;Check if IC1 event occurred
;IC1 Event Occurred 2nd
;Check if IC3 event occurred
;IC3 Event Occurred 2nd
;Check for small timeout
;not all transducers received signal
;
IC1BUF,W0
W0,TRANSTIME1
;Retrieve event time
;
203
ICEO21_wait:
btsc
IC3CON,#3
goto
ICEO213
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO21_wait
;2,3
ICEO23:
mov
IC3BUF,W0
mov
W0,TRANSTIME3
ICEO23_wait:
btsc
IC1CON,#3
goto
ICEO231
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO23_wait
;2,1,3
ICEO213:
mov
#0x03,W0
mov
W0,TRANSORDER
mov
IC3BUF,W0
mov
W0,TRANSTIME3
return
;2,3,1
ICEO231:
mov
#0x04,W0
mov
W0,TRANSORDER
mov
IC1BUF,W0
mov
W0,TRANSTIME1
return
;Check if IC3 event occurred
;IC3 Event Occurred last
;Check for small timeout
;not all transducers received signal
;
;Retrieve event time
;
;Check if IC1 event occurred
;IC1 Event Occurred last
;Check for small timeout
;not all transducers received signal
;
;Set Transducer Order
;
;Retrieve event time
;
;All 3 transducers responded.. return
;Set Transducer Order
;
;Retrieve event time
;
;All 3 transducers responded.. return
;Transducer 3 received signal first, start small Timeout
;wait for next 2 signals
ICEO3:
call
Timer45_read
;store ranging time
mov
W0,RANGETIMEL
;
mov
W1,RANGETIMEH
;
mov
IC3BUF,W0
;Retrieve event time
mov
W0,TRANSTIME3
;
mov
SMALLTOH,W0
mov
W0,PR5
mov
SMALLTOL,W0
mov
W0,PR4
call
Timer45_clear
ICEO3_wait:
btsc
IC1CON,#3
goto
ICEO31
btsc
IC2CON,#3
goto
ICEO32
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO3_wait
;3,1
ICEO31:
mov
IC1BUF,W0
mov
W0,TRANSTIME1
ICEO31_wait:
btsc
IC2CON,#3
goto
ICEO312
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO31_wait
;Set Small Timeout value
;
;
;
;Reset Timeout Timer
;Check if IC1 event occurred
;IC1 Event Occurred 2nd
;Check if IC2 event occurred
;IC2 Event Occurred 2nd
;Check for small timeout
;not all transducers received signal
;
;Retrieve event time
;
;Check if IC2 event occurred
;IC2 Event Occurred last
;Check for small timeout
;not all transducers received signal
;
204
;3,2
ICEO32:
mov
IC2BUF,W0
mov
W0,TRANSTIME2
ICEO32_wait:
btsc
IC1CON,#3
goto
ICEO321
btsc
IFS1,#T5IF
goto
IC_NOTALLRETURN
goto
ICEO32_wait
;3,1,2
ICEO312:
mov
#0x05,W0
mov
W0,TRANSORDER
mov
IC2BUF,W0
mov
W0,TRANSTIME2
return
;3,2,1
ICEO321:
mov
#0x06,W0
mov
W0,TRANSORDER
mov
IC1BUF,W0
mov
W0,TRANSTIME1
return
;Time out routines
IC_NORETURN:
mov
#0x00,W0
mov
W0,TRANSORDER
return
;Retrieve event time
;
;Check if IC1 event occurred
;IC1 Event Occurred last
;Check for small timeout
;not all transducers received signal
;
;Set Transducer Order
;
;Retrieve event time
;
;All 3 transducers responded.. return
;Set Transducer Order
;
;Retrieve event time
;
;All 3 transducers responded.. return
;Set Transducer Order to timeout
;
;
IC_NOTALLRETURN:
mov
#0x07,W0
;Set Transducer Order to timeout
mov
W0,TRANSORDER
;
return
;
;*********************************************************
;********DELAY ROUTINES***************************************
;*********************************************************
;delay100ms ;
;Delays 100ms
;*********************************************************
delay100ms:
mov
#20,W4
;delay ~100ms
delayloop100ms:
call
delay5ms
;
dec
W4,W4
;
bra
NZ,delayloop100ms
;
return
;
delay5ms:
mov
#40,W2
;load counter
delayloop:
call
delay125us
;delay 125us
dec
W2,W2
;Delayed 5ms?
bra
NZ,delayloop
;
return
;yep
;*********************************************************
;*********************************************************
;delay125us ;
205
;Delays 125us
;*********************************************************
delay125us:
mov
#1664,W3
;load counter
delayloop2:
;
dec
W3,W3
;
bra
NZ,delayloop2
;Delayed 125us?
nop
nop
return
;yep
;*********************************************************
;*************************************************************
.end
206
;**********************************************************************************
;************************ TRANSPONDER MICROCONTROLLER CODE ************************
;**********************************************************************************
.equ __33FJ12GP202, 1
.include "p33FJ12GP202.inc"
;..............................................................................
;Configuration bits:
;..............................................................................
config __FOSC, FCKSM_CSDCMD & OSCIOFNC_ON & POSCMD_EC
;Turn off clock switching and
;fail-safe clock monitoring and
;use the External Clock as the
;system clock
config __FWDT, FWDTEN_OFF & WINDIS_OFF;Turn off Watchdog Timer
config __FPOR, FPWRT_PWR128
;Set Power on Timer 128 mSec
config __FOSCSEL, FNOSC_PRIPLL & IESO_OFF ;Setup Oscillator (w/PLL)
config __FGS, GSS_OFF & GCP_OFF & GWRP_OFF ;Disable Code protect
config __FBS, BSS_NO_FLASH & BWRP_WRPROTECT_OFF ;
;..............................................................................
;Program Specific Constants (literals used in code)
;..............................................................................
;----DIGITAL POTENTIOMETER EQUATES-------------.equ
DP_PORT,PORTA
;
.equ
DP_TRIS,TRISA
;
.equ
DP_CS,0
;Chip Select
.equ
DP_SCLK,1
;Clock
.equ
DP_DIN_PRE,4
;Pre-Amp Digital Pot Line
.equ
DP_DIN_PST,3
;Post-Amp Digital Pot Line
;----TRANSDUCER
.equ
.equ
.equ
.equ
.equ
.equ
DRIVER EQUATES-----------------TDC_PORT,PORTB
;
TDC_TRIS,TRISB
;
TDC_MODE,8
;
TDC_PHASE,7
;
TDC_ENABLE,6
;
TDC_NFLT,0
;
;----TONE DECODER EQUATES----------------------.equ
TD_PORT,PORTB
;
.equ
TD_TRIS,TRISB
;
.equ
TD_OUT,11
;
;----ANALOG INPUT EQUATES----------------------.equ
AI_PORT,PORTB
;
.equ
AI_TRIS,TRISB
;
.equ
AI_PRE,13
;
.equ
AI_POST,12
;
;..............................................................................
;Global Declarations:
;..............................................................................
.global __reset
;The label for the first line of code.
207
;..............................................................................
;Constants stored in Program space
;..............................................................................
;
.section .myconstbuffer, code
;
.palign 2
;Align next word stored in Program space to an
;
;address that is a multiple of 2
;ps_coeff:
;
.hword
0x0002, 0x0003, 0x0005, 0x000A
;..............................................................................
;Uninitialized variables in X-space in data memory
;..............................................................................
.section .xbss, bss, xmemory
TEMP:
TDC_DLYVAL:
TDC_NUMCYC:
.space
.space
.space
2
2
2
;..............................................................................
;Uninitialized variables in Y-space in data memory
;..............................................................................
;
;y_input:
.section .ybss, bss, ymemory
.space 2*SAMPLES
;..............................................................................
;Initialized variables in data memory
;..............................................................................
;..............................................................................
;Uninitialized variables in Near data memory (Lower 8Kb of RAM)
;..............................................................................
;
.section .nbss, bss, near
.section vars,bss,address(0x900)
;..............................................................................
;Code Section in Program Memory
;..............................................................................
.text
;Start of Code section
__reset:
MOV #__SP_init, W15
;Initalize the Stack Pointer
MOV #__SPLIM_init, W0
;Initialize the Stack Pointer Limit Register
MOV W0, SPLIM
NOP
;Add NOP to follow SPLIM initialization
main:
;Initialize PLL to proper frequency
;PLL will work on startup if 4mHz<Fin<8mHz
;(it was checked that PLL settings will within
;limits during this PLL update process)
mov
#0x00C0,W0
;PLLPRE=2,PLLPOST=8 (FOR IC=8mhz)
mov
W0,CLKDIV
;
mov
#0x003E,W0
;Set PLL Multiplier=64 (For IC = 8mhz)
mov
W0,PLLFBD
;
208
;Disable all modules
mov
#0xFFFF,W0
mov
W0,PMD1
mov
W0,PMD2
;
;
;
;Disable all open-drains
mov
#0x0000,W0
mov
W0,ODCA
mov
W0,ODCB
;
;Port A
;Port B
;Disable Analog Function of I/O Pins
mov
#0xFFFF,W0
;
mov
W0,AD1PCFGL
;
;Initialize
call
call
call
Everything
DP_INIT
TD_INIT
TDC_INIT
;Initialize Digital Potentiometers
;Initialize Tone Decoder
;Initialize Transducer Driver Circuit
mov
#35,W0
;load counter (#35 = ~35khz, #170 =
audible tone)
mov
W0,TDC_DLYVAL
;delay value (sets
transmit frequency)
mov
#7000,W0
;Load number of
transmit cycles
mov
W0,TDC_NUMCYC
;sets pulse length (70
= 2ms)
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
call
delay100ms
;
mov
#90,W0
;Set front end gain ~100
mov
#127,W1
;Set back end gain min
call
DP_SET
;Set Potentiometer Values
mainloop:
call
TDC_TOUT
;Main Loop Programmed to output 1ms pulse
call
delay100ms
;approximately 3 times per second
call
delay100ms
;
call
delay100ms
;In actual practice, this loop should wait for
goto
mainloop
;an interrogation pulse, then respond.
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
;..............................................................................
;Timer 1 Interrupt Service Routine
;Example context save/restore in the ISR performed using PUSH.D/POP.D
;instruction. The instruction pushes two words W4 and W5 on to the stack on
;entry into ISR and pops the two words back into W4 and W5 on exit from the ISR
;..............................................................................
;__T1Interrupt:
;
PUSH.D W4
;Save context using double-word PUSH
;<<insert more user code here>>
;
BCLR IFS0, #T1IF
;
;
POP.D W4
RETFIE
;Clear the Timer1 Interrupt flag Status
;bit.
;Retrieve context POP-ping from Stack
;Return from Interrupt Service routine
;--------End of All Code Sections ---------------------------------------------
209
;********Digital Potentiometer Routines***********************
;*********************************************************
;DP_INIT - Digital Potentiometer Initialization
;*********************************************************
DP_INIT:
bclr
DP_TRIS,#DP_CS
;CS Output
bset
DP_PORT,#DP_CS
;
bclr
DP_TRIS,#DP_SCLK
;SCLK Output
bset
DP_PORT,#DP_SCLK
;
bclr
DP_TRIS,#DP_DIN_PRE
;DIN Output
bclr
DP_PORT,#DP_DIN_PRE
;
bclr
DP_TRIS,#DP_DIN_PST
;DIN Output
bclr
DP_PORT,#DP_DIN_PST
;
return
;
;*********************************************************
;*********************************************************
;DP_SET - Digital Potentiometer Set Value
;
;Uses value in W0(7:0) to set Pre-Amp Potentiometer
;Uses value in W1(7:0) to set Post-Amp Potentiometer
;*********************************************************
DP_SET:
bclr
DP_PORT,#DP_SCLK
;Clock Pin Low
nop
;
bclr
DP_PORT,#DP_CS
;CS Low
nop
;
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#7
;Send out MSB (#7)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#7
;Send out MSB (#7)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#6
;Send out bit (#6)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#6
;Send out bit (#6)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#5
;Send out bit (#5)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#5
;Send out bit (#5)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#4
;Send out bit (#4)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#4
;Send out bit (#4)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
210
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#3
;Send out bit (#3)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#3
;Send out bit (#3)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#2
;Send out bit (#2)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#2
;Send out bit (#2)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
bclr
btsc
bset
nop
bclr
btsc
bset
call
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
W0,#1
;Send out bit (#1)
DP_PORT,#DP_DIN_PRE ;Bit Set
;
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
W1,#1
;Send out bit (#1)
DP_PORT,#DP_DIN_PST ;Bit Set
DP_Clock_Pulse
;Clock bit through
bclr
DP_PORT,#DP_DIN_PRE ;Assume Bit Clear
btsc
W0,#0
;Send out bit (#0)
bset
DP_PORT,#DP_DIN_PRE ;Bit Set
nop
;
bclr
DP_PORT,#DP_DIN_PST ;Assume Bit Clear
btsc
W1,#0
;Send out bit (#0)
bset
DP_PORT,#DP_DIN_PST ;Bit Set
call
DP_Clock_Pulse
;Clock bit through
nop
;
nop
;
bset
DP_PORT,#DP_CS
;CS High
nop
;
bset
DP_PORT,#DP_SCLK
;Clock Pin High
return
;
;*********************************************************
;*****DP Clock Pulse**************************************
DP_Clock_Pulse:
bset
DP_PORT,#DP_SCLK
;Clock Pin High
nop
;
;
nop
;
bclr
DP_PORT,#DP_SCLK
;Clock Pin Low
return
;
;*********************************************************
;*************************************************************
;********Tone Decoder Routines********************************
;TD_INIT - Tone Decoder Initialization
;*********************************************************
TD_INIT:
bset
TD_TRIS,#TD_OUT
;Input
return
;
;*********************************************************
;*************************************************************
;********Transducer Driver Routines***************************
211
;*********************************************************
;TDC_INIT - Transducer Driver Circuit Initialization
;*********************************************************
TDC_INIT:
bclr
TDC_TRIS,#TDC_MODE
;Output
bset
TDC_PORT,#TDC_MODE
;
bclr
TDC_TRIS,#TDC_PHASE
;Output
bset
TDC_PORT,#TDC_PHASE
;
bclr
TDC_TRIS,#TDC_ENABLE
;Output
bclr
TDC_PORT,#TDC_ENABLE
;
bset
TDC_TRIS,#TDC_NFLT
;Output
mov
mov
#293,W0
W0,TDC_DLYVAL
;
;load counter
mov
mov
#45,W0
W0,TDC_NUMCYC
;
;
return
;
;*********************************************************
;*********************************************************
;TDC_OUT - Transducer Driver Circuit Output
;*********************************************************
TDC_OUT:
bset
TDC_PORT,#TDC_ENABLE
;Start Output
nop
;
TDC_RPT:
bset
TDC_PORT,#TDC_PHASE
;Change Current Direction
call
TDC_DLY
;delay
nop
;(make up for loop time)
nop
;
bclr
TDC_PORT,#TDC_PHASE
;Change Current Direction
call
TDC_DLY
;
goto
TDC_RPT
;
bclr
TDC_PORT,#TDC_ENABLE
;Stop Output
return
;
;*********************************************************
;*********************************************************
;TDC_TOUT - Transducer Driver Circuit Timed Output
;Amount of cycles to output passed via TDC_NUMCYC
;*********************************************************
TDC_TOUT:
mov
TDC_NUMCYC,W1
;Set number of cycles
bset
TDC_PORT,#TDC_ENABLE
;Start Output
nop
TDC_TRPT:
bset
call
nop
nop
nop
bclr
call
dec
bra
TDC_PORT,#TDC_PHASE
TDC_DLY
TDC_PORT,#TDC_PHASE
TDC_DLY
W1,W1
NZ,TDC_TRPT
;Change Current Direction
;delay
;(make up for loop time)
;
;Change Current Direction
;
;decrement cycle count
;if not yet 0, repeat
bclr
TDC_PORT,#TDC_ENABLE
;Stop Output
return
;
;*********************************************************
212
;*********************************************************
;TDC_DLY - Transducer Driver Circuit Delay (Sets Freq)
;*********************************************************
TDC_DLY:
mov
TDC_DLYVAL,W0
;load counter (35 = ~35kHz)
;#170 = Audible Tone
TDC_DLY_2:
;
dec
W0,W0
;
bra
NZ,TDC_DLY_2
;Delayed Set Time?
return
;yep
;*********************************************************
;*************************************************************
;*************************************************************
;;********Timer Routines***************************************
;;*********************************************************
;;Timer23_init - Timers 2/3 Init (use as a 32-bit timer)
;;
;;Setup Timer 2/3 to operated as a single 32-bit timer
;;*********************************************************
;Timer23_init:
;
mov
#0x0038,W0
;Setup 32-bit timer
;
mov
W0,T2CON
;(Timer 2/3)
;
;Not setting up any interrupts
;
bset
T2CON,#15
;start timer
;
return
;
;;*********************************************************
;;*********************************************************
;;Timer23_clear - Timers 2/3 Clear
;;
;;Clear 32-bit timer (Start at 0x00000000)
;;*********************************************************
;Timer23_clear:
;
mov
#0x0000,W0
;Write 0x0000 to MSW
;
mov
W0,TMR3HLD
;(Timer 2/3)
;
mov
W0,TMR2
;(Timer 2/3)
;
return
;
;;*********************************************************
;;*********************************************************
;;Timer23_read - Timers 2/3 Read
;;
;;Read the 32-bit timer
;;*********************************************************
;Timer23_read:
;
mov
TMR2,W0
;Transfer the LSW into W1
;
mov
TMR3HLD, W1
;Transfer the MSW from the
;
;holding register to W0
;
return
;
;;*********************************************************
;;*********************************************************
;;OnePPS_init - 1PPS Init (INT4)
;;
;;Setup 1PPS pin as input and interrupt used to clear timer
;;*********************************************************
;OnePPS_init:
;
bset
INT_TRIS,#GPS_INT
;Set 1PPS pin as an input
;;Setup INT4 interrupt
;
bclr
INTCON2,#INT4EP
;Interrupt on positive edge
;
bset
IPC13,#9
;Set interrupt priority
;
;to level 6 (default level 4)
;
bclr
INTCON1,#NSTDIS
;Enable nested interrupts
;
bclr
IFS3,#INT4IF
;Clear interrupt flag
213
;
bset
IEC3,#INT4IE
;Enable INT4 interrupt
;
return
;
;;*********************************************************
;;*********************************************************
;;__INT4Interrupt - INT4 Interrupt Routine (1PPS)
;;
;;Clears Timer2/3 when 1PPS interrupt is received
;;*********************************************************
;__INT4Interrupt:
;
;Save context
;
push
W0
;Save W0
;
;Clear Timer 2/3 value
;
mov
#0x0000,W0
;Write 0x0000 to MSW
;
mov
W0,TMR3HLD
;(Timer 2/3)
;
mov
W0,TMR2
;(Timer 2/3)
;
;Restore context
;
pop
W0
;Restore W0
;
bclr
IFS3,#INT4IF
;Clear interrupt flag
;
retfie
;return
;;*********************************************************
;;*************************************************************
;
;
;
;;********Conversion Routines**********************************
;;*********************************************************
;;SNUM2ASCII - Signed Number to ASCII
;;Converts Signed 16-bit value in W0 to 5 ASCII characters plus
;;a sign character.
;;The ASCII values are stored in
;;W5 - Sign (' ' or '-')
;;W4 - ten thousands
;;W3 - thousands
;;W2 - hundreds
;;W1 - tens
;;W0 - ones
;;*********************************************************
;snum2ascii:
;
btss
W0,#15
;Check the sign bit
;
goto
snum2asciipos
;Positive Number
;;negative number, must reverse the
;;2's complement procedure before calling num2bcd
;
mov
#0xFFFF,W5
;Invert all the bits
;
xor
W0,W5,W0
;
;
inc
W0,W0
;add 1
;
call
num2bcd
;
;
mov
#0x002D,W5
;Set Sign character
;
add
#0x30,W0
;Convert BCD to ASCII
;
add
#0x30,W1
;
;
add
#0x30,W2
;
;
add
#0x30,W3
;
;
add
#0x30,W4
;
;
return
;
;snum2asciipos:
;
mov
#0x0020,W5
;Set Sign character
;
call
num2bcd
;Convert digits
;
add
#0x30,W0
;Convert BCD to ASCII
;
add
#0x30,W1
;
;
add
#0x30,W2
;
;
add
#0x30,W3
;
;
add
#0x30,W4
;
;
return
;
;;***********************************************
214
;
;;*********************************************************
;;SNUM2BCD - Signed Number to BCD
;;Converts Signed 16-bit value in W0 to 5 BCD digits plus sign bit
;;The BCD values are stored in
;;W5 - Bit 0 indicates sign... 1 if negative, 0 if positive
;;W4 - ten thousands
;;W3 - thousands
;;W2 - hundreds
;;W1 - tens
;;W0 - ones
;;*********************************************************
;snum2bcd:
;
btss
W0,#15
;Check the sign bit
;
goto
num2bcd
;positive number, will be returned by num2bcd
;
;negative number, must reverse the
;
;2's complement procedure before calling num2bcd
;
mov
#0xFFFF,W5
;Invert all the bits
;
xor
W0,W5,W0
;
;
inc
W0,W0
;add 1
;
call
num2bcd
;
;
mov
#0x0001,W5
;Set sign bit, show negative number
;
return
;
;;***********************************************
;
;;*********************************************************
;;NUM2BCD - Number to BCD
;;Converts 16-bit value in W0 to 5 BCD digits
;;The BCD values are stored in
;;W5 - Temporary Register,
;;W4 - ten thousands
;;W3 - thousands
;;W2 - hundreds
;;W1 - tens
;;W0 - ones
;;*********************************************************
;num2bcd:
;
clr
W4
;Clear the variables to be stored
;
clr
W3
;
;
clr
W2
;
;
clr
W1
;
;
;tenthoutest:
;
mov
#0x2710,W5
;(10,000 in hex)
;
cp
W0,W5
;
;
bra
ltu,thoutest
;Check if W0 >= 10,000
;
;it is
;
sub
W0,W5,W0
;Subtract 10,000 from Incoming number
;
inc.b
W4,W4
;Increment BCD digit
;
bra
tenthoutest
;repeat
;
;thoutest:
;
mov
#0x03E8,W5
;(1,000 in hex)
;
cp
W0,W5
;
;
bra
ltu,hundtest
;Check if W0 >= 1,000
;
;it is
;
sub
W0,W5,W0
;Subtract 1,000 from Incoming number
;
inc.b
W3,W3
;Increment BCD digit
;
bra
thoutest
;repeat
;
;hundtest:
;
mov
#0x0064,W5
;(100 in hex)
;
cp
W0,W5
;
215
;
bra
ltu,tentest
;Check if W0 >= 100
;
;it is
;
sub
W0,W5,W0
;Subtract 100 from Incoming number
;
inc.b
W2,W2
;Increment BCD digit
;
bra
hundtest
;repeat
;
;tentest:
;
mov
#0x000A,W5
;(10 in hex)
;
cp
W0,W5
;
;
bra
ltu,onetest
;Check if W0 >= 10
;
;it is
;
sub
W0,W5,W0
;Subtract 10 from Incoming number
;
inc.b
W1,W1
;Increment BCD digit
;
bra
tentest
;repeat
;
;onetest:
;
;Rest are ones and already stored in W0
;
clr
W5
;Clear to indicate positive sign for snum2bcd
;
return
;done
;;***********************************************
;;*************************************************************
;
;
;
;********DELAY ROUTINES***************************************
;*********************************************************
;delay100ms ;
;Delays 100ms
;*********************************************************
delay100ms:
mov
#20,W4
;delay ~100ms
delayloop100ms:
call
delay5ms
;
dec
W4,W4
;
bra
NZ,delayloop100ms
;
return
;
delay5ms:
mov
#40,W2
;load counter
delayloop:
call
delay125us
;delay 125us
dec
W2,W2
;Delayed 5ms?
bra
NZ,delayloop
;
return
;yep
;*********************************************************
;*********************************************************
;delay125us ;
;Delays 125us
;*********************************************************
delay125us:
mov
#0x014B,W3
;load counter (0x014B for Fcyl=8MHz)
delayloop2:
;
dec
W3,W3
;
bra
NZ,delayloop2
;Delayed 125us?
nop
nop
return
;yep
;*********************************************************
;*********************************************************
;delay71us -
216
;
;Delays 71us
;*********************************************************
delay71us:
mov
#0x00BB,W3
;load counter (0x014B for Fcyl=8MHz)
delayloop271:
;
dec
W3,W3
;
bra
NZ,delayloop271 ;Delayed 71us?
nop
nop
return
;yep
;*********************************************************
;*************************************************************
.end
217
Appendix D: MATLAB Code
218
%GPS Accuracy Computer
%Authored By Doug Bowlus on Dec 28, 2007
close all
clear all
clc
%Known Coordinate
%KC_Lat = DMS_2_DD(34,22,30.97170); %RINCON, PID: EW7931
%KC_Long = DMS_2_DD(119,28,36.69745);
KC_Lat = DMS_2_DD(34,15,13.54656); %Fugro Point 6, In back of office
KC_Long = DMS_2_DD(119,13,34.15617);
[KC_X,KC_Y,KC_Z] = WGS84_2_Cart(KC_Lat,KC_Long,0);
%Load Observed Data
filename = 'LOG4a.txt';
[AP_Hdr,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt,RangeTime,TransOrder,Tran1Time,Tran2Time,Tr
an3Time,AP_STime,AP_ETime,Tilt_Time,Mag_Time,GPS_Hdr,UTC_Time,Latitude,N_S,Longitud
e,E_W,Fix_Type,Satellites,HDOP,MSL_Alt,MSL_Units,Geoid_Sep,Geoid_Units,Age_of_Corr,
Checksum] = importuwgps(filename);
[OC_X,OC_Y,OC_Z] = WGS84_2_Cart(Latitude,Longitude,0);
%Compute
Diff_X =
Diff_Y =
Diff_Z =
the difference between observed and known positions
OC_X - KC_X;
OC_Y - KC_Y;
OC_Z - KC_Z;
%Plot the differences
plot3(Diff_X,Diff_Y,Diff_Z,'o')
hold on
%Plot Last Posistion
plot3(Diff_X(end),Diff_Y(end),Diff_Z(end),'go')
%Find Average Position
[AOC_X,AOC_Y,AOC_Z] = WGS84_2_Cart(mean(Latitude),mean(Longitude),0);
%Plot Average Position Difference
AEX = AOC_X - KC_X;
AEY = AOC_Y - KC_Y;
AEZ = AOC_Z - KC_Z;
plot3(AEX,AEY,AEZ,'ro');
hold off
grid on
%Display computed overall Error
E2DRMS = sqrt(Diff_X.^2+Diff_Y.^2+Diff_Z.^2);
SD2DRMS = std(E2DRMS)
Mean_2DRMS = mean(E2DRMS)
Max_2DRMS = max(E2DRMS)
Min_2DRMS = min(E2DRMS)
Fix = mean(Fix_Type)
DurationMIN = length(Fix_Type)./60
219
function
[AP_Hdr,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt,RangeTime,TransOrder,Tran1Time,Tran2Time,Tr
an3Time,AP_STime,AP_ETime,Tilt_Time,Mag_Time,GPS_Hdr,UTC_Time,Latitude,N_S,Longitud
e,E_W,Fix_Type,Satellites,HDOP,MSL_Alt,MSL_Units,Geoid_Sep,Geoid_Units,Age_of_Corr,
Checksum] = importuwgps(filename)
%
%importuwgps
%
%Authored by Doug Bowlus on 12/27/2007
%
%[AP_Hdr,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt,RangeTime,TransOrder,Tran1Time,Tran2Time,T
ran3Time,AP_STime,AP_ETime,Tilt_Time,Mag_Time,GPS_Hdr,UTC_Time,Latitude,N_S,Longitu
de,E_W,Fix_Type,Satellites,HDOP,MSL_Alt,MSL_Units,Geoid_Sep,Geoid_Units,Age_of_Corr
,Checksum] = importuwgps(filename)
%
%Given the filename for UWGPS log data, this script imports all of it into
%appropriate variable names
%
fid = fopen(filename);
data =
textscan(fid,'%s%f%f%f%f%f%f%f%f%f%f%f%f%f%f%s%f%f%s%f%s%f%f%f%f%s%f%s%f%s',1,'delimiter',',');
AP_Hdr = cellstr(data{1});
X_Mag = double(data{2});
Y_Mag = double(data{3});
Z_Mag = double(data{4});
X_Tilt = double(data{5})./10;
Y_Tilt = double(data{6})./10;
RangeTime = double(data{7}).*125e-9;
TransOrder = double(data{8});
Tran1Time = double(data{9}).*125e-9;
Tran2Time = double(data{10}).*125e-9;
Tran3Time = double(data{11}).*125e-9;
AP_STime = double(data{12}).*125e-9;
AP_ETime = double(data{13}).*125e-9;
Tilt_Time = double(data{14}).*125e-9;
Mag_Time = double(data{15}).*125e-9;
GPS_Hdr = cellstr(data{16});
UTC_Time = double(data{17});
Latitude = strjust(num2str(double(data{18})),'left');
Latitude = DMS_2_DD(str2num(Latitude(:,1:2)),str2num(Latitude(:,3:end)),0);
N_S = cellstr(data{19});
Longitude = strjust(num2str(double(data{20})),'left');
Longitude = DMS_2_DD(str2num(Longitude(:,1:3)),str2num(Longitude(:,4:end)),0);
E_W = cellstr(data{21});
Fix_Type = double(data{22});
Satellites = double(data{23});
HDOP = double(data{24});
MSL_Alt = double(data{25});
MSL_Units = cellstr(data{26});
Geoid_Sep = double(data{27});
Geoid_Units = cellstr(data{28});
Age_of_Corr = double(data{29});
Checksum = char(data{30});
Checksum = hex2dec(Checksum(:,2:3));
220
function [D,M,S] = DD_2_DMS(deg)
%Decimal Degrees to Degrees, Minutes, Seconds
%[D,M,S] = DD_2_DMS(deg)
%
%Authored By: Doug Bowlus 24 Oct. 2007
if deg >= 0
MS = mod(deg,1);
D = deg - MS;
M = (MS - mod(MS,(1./60))).*60;
S = ((MS.*60) - M) .* 60;
else
deg = abs(deg);
MS = mod(deg,1);
D = (deg - MS) .* -1;
M = (MS - mod(MS,(1./60))).*60;
S = ((MS.*60) - M) .* 60;
End
function [DD] = DMS_2_DD(deg,min,sec)
%Degrees, Minutes, Seconds to Decimal Degrees
%[DD] = DMS_2_DD(deg,min,sec)
%
%Authored By: Doug Bowlus 24 Oct. 2007
if deg >= 0
DD = deg + (min./60) + (sec./60./60);
else
DD = deg - (min./60) - (sec./60./60);
End
function [X,Y,Z,Z2] = WGS84_2_Cart(lat,long,hp)
%WGS84 to Cartesian Coordinates transformation function
%[X,Y,Z] = WGS84_2_Cart(lat,long,ht)
%
%North = Positive Latitude, South = Negative Latitude
%East = Positive Longitude, West = Negative Longitude
%Authored By: Doug Bowlus 24 Oct. 2007
%
%X,Y,Z in meters
%WGS84 Parameters
a = 6378137.0; %Semi-Major Axis in meters
b = 6356752.3142; %Semi-Minor Axis
ecc = 0.00669437999013; %Eccentricity squared
flat = 1.0./298.257223563; %Flattening f = (a-b)/a
221
%Perform conversion
vp = a ./ (1.0-ecc .* sind(lat).^2).^(1/2);
X = (vp + hp) .* cosd(lat) .* cosd(long);
Y = (vp + hp) .* cosd(lat) .* sind(long);
Z = (vp .* (1.0-ecc) + hp) .* sind(lat);
%Heading Data Analysis Script
clear all
clc
close all
%Load Calibration File and Computer Calibration Parameters
filename = 'LOG3.TXT';
known_heading = 165.6;
[Xos,Yos,Xrange,Yrange,RF] = MagCalibrate(filename,known_heading);
%Load Data File
filename = 'LOG10.TXT';
known_heading = 144;
[OS_Hdr,Time,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt] = importosdata(filename);
Heading = Mag2Heading(X_Mag,Y_Mag,Xos,Yos,Xrange,Yrange,RF);
error = known_heading - Heading;
Time = Time - Time(1);
subplot(2,1,1)
hold on
plot(Time,Heading,'b')
plot(Time,known_heading.*ones(length(Time)),'r')
hold off
grid on
title('Heading Vs. Time')
xlabel('Elapsed Time (seconds)')
ylabel('Heading (degrees)')
legend('Measured Heading','Gyro Heading')
subplot(2,1,2)
plot(Time,error,'k')
grid on
title('Heading Error Vs. Time')
xlabel('Elapsed Time (seconds)')
ylabel('Heading Error (degrees)')
avg_heading = mean(Heading)
avg_error = mean(error)
std_heading = std(Heading)
duration = Time(end) - Time(1)
% RF1 = 178.1 - 191 = -12.9
% RF2 = 179.4 - 190 = -10.6
% RF3 = 165.6 - 203.2 = -37.6
%
%
%
%
%
%
%
%
figure
plot(X_Mag,'r')
hold on
grid on
plot(Y_Mag,'b')
plot(Z_Mag,'k')
plot(Heading,'m')
legend('X','Y','Z','Heading')
% OUTPUT GRAPHS
he1 = [.39 .28 .12 -.10 -.42 -.75 -.50 -.09 -0.12 0.21 0.45 0.27 -0.4 -1.23 -0.62];
he2 = [-0.44 -0.56 -0.57 -0.46 -0.33 -0.28 0.12 0.36 -.1 -.23 -.32 -.61 -1.22 -1.95
-1.37];
he3 = [.41 .5 .58 .58 .44 .19 .40 .65 .37 .43 .44 .11 -.62 -1.4 -.73];
val = 0:24:336;
222
%
% figure
% plot(val,he1,'ro-')
% hold on
% grid on
% plot(val,he2,'bo-')
% plot(val,he3,'ko-')
% legend('Calibration 1','Calibration 2','Calibration 3')
% title('Mean Heading Error Vs. Heading')
% ylabel('Mean Heading Error (degrees)')
% xlabel('Heading (degrees)')
%
sd1 = [.18 .13 .14 .12 .12 .11 .11 .13 .14 .13 .11 .1 .09 .1 .12 .15 .01 .13 .12
.14 .11 .18 .1 .12];
sd2 = [.18 .13 .13 .12 .12 .11 .11 .14 .15 .13 .11 .1 .09 .1 .12 .15 .1 .13 .12 .14
.11 .19 .1 .12];
sd3 = [.18 .13 .13 .12 .12 .11 .11 .14 .15 .13 .11 .1 .09 .1 .12 .15 .1 .13 .12 .14
.11 .18 .1 .12];
%
% figure
% %plot(val,sd1(1:15),'ro-')
% hold on
% grid on
% plot(val,sd2(1:15),'ko-')
% %plot(val,sd3(1:15),'ko-')
% %legend('Calibration 1','Calibration 2','Calibration 3')
% title('Standard Deviation Vs. Heading')
% ylabel('Standard Deviation (degrees)')
% xlabel('Heading (degrees)')
OMHE1
OMHE2
OMHE3
OMSD1
OMSD2
OMSD3
=
=
=
=
=
=
mean(he1)
mean(he2)
mean(he3)
mean(sd1)
mean(sd2)
mean(sd3)
function [OS_Hdr,Time,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt] = importosdata(filename)
%
%importuwgps
%
%Authored by Doug Bowlus on 12/27/2007
%
223
%[OS_Hdr,Time,X_Mag,Y_Mag,Z_Mag,X_Tilt_Y_Tilt] = importosdata(filename)
%
%Given the filename for OSSEN log data, this script imports all of it into
%appropriate variable names
%
fid = fopen(filename);
data = textscan(fid,'%s%f%f%f%f%f%f',-1,'delimiter',',');
OS_Hdr = cellstr(data{1});
Time = double(data{2}) .* 125e-9;
X_Mag = double(data{3});
Y_Mag = double(data{4});
Z_Mag = double(data{5});
X_Tilt = double(data{6})./10;
Y_Tilt = double(data{7})./10;
224
function [Xos,Yos,Xrange,Yrange,RF] = MagCalibrate(filename,known_heading)
[OS_Hdr,Time,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt] = importosdata(filename);
%Calibrate Magnetometer Heading according to PNI Multipoint
%Primer Application Note. Assumes imported data comes from
%magnetometer being rotated in a circle. The known heading
%of the unit at the start of the collected data. A gyro or
%be used to obtain this value.
Xmax = max(X_Mag);
Xmin = min(X_Mag);
Ymax = max(Y_Mag);
Ymin = min(Y_Mag);
Calibration
the
is the heading
other means can
Xos = (Xmax + Xmin)./2;
Yos = (Ymax + Ymin)./2;
Xrange = (Xmax - Xmin);
Yrange = (Ymax - Ymin);
[Heading] = Mag2Heading(X_Mag,Y_Mag,Xos,Yos,Xrange,Yrange,0);
RF = known_heading - mean(Heading(1:10)); %Use the average of the first 10 points
to compute the rotation
function [Heading] = Mag2Heading(X_Mag,Y_Mag,Xos,Yos,Xrange,Yrange,RF)
%
%Heading = Mag2Heading(Xvalue,Yvalue,Xos,Yos,Xrange,Yrange,RF)
%
%Authored by Doug Bowlus on 1/27/2007
%
%Turns raw magnetometer values into a heading given the parameters
%from calibration. RF = Rotation Factor
%
Xvalue = X_Mag - Xos;
Yvalue = Y_Mag - Yos;
if Xrange > Yrange
Yvalue = (Yvalue .* Xrange) ./ Yrange;
else
Xvalue = (Xvalue .* Yrange) ./ Xrange;
end
Heading = (atan2(Yvalue,Xvalue) .* (180./pi));
for u = 1:length(Heading), %Compensate for -180 to 0 headings
if Heading(u) < 0
%Makes these headings 180 to 360
Heading(u) = 360 + Heading(u);
end
end
Heading = Heading + RF;
%Rotate the headings for proper north orientation
for u = 1:length(Heading), %Make headings ly between 0 - 360 after rotation
if Heading(u) >= 360
Heading(u) = Heading(u) - 360;
elseif Heading(u) < 0
Heading(u) = Heading(u) + 360;
end
end
225
%Tilt Test Data Processing Script
%Authored by Doug Bowlus on 1/31/2008
clear all
clc
close all
%Calibration Values
XCal = -1.19 - (-0.0766); %F180 Neg Roll Value - Avg Xtilt (values from log1.txt
test. Reference Level
YCal = 2.89 - 2.7641; %F180 Pitch Value - Avg Ytilt (values from log1.txt test.
Reference Level
%Load Data File
filename = 'LOG10.TXT';
known_xtilt = 2.79; %Negative Roll
known_ytilt = 2.94; %Pitch
[OS_Hdr,Time,X_Mag,Y_Mag,Z_Mag,X_Tilt,Y_Tilt] = importosdata(filename);
%Apply Calibration Values
X_Tilt = X_Tilt + XCal;
Y_Tilt = Y_Tilt + YCal;
Time = Time - Time(1);
xerror = known_xtilt - X_Tilt;
yerror = known_ytilt - Y_Tilt;
%X TILT PLOTS
subplot(2,1,1)
hold on
plot(Time,X_Tilt,'b')
plot(Time,known_xtilt.*ones(length(Time)),'r')
hold off
grid on
title('X Tilt Vs. Time')
xlabel('Elapsed Time (seconds)')
ylabel('X Tilt (degrees)')
legend('Measured X Tilt','F180 Tilt')
subplot(2,1,2)
plot(Time,xerror,'k')
grid on
title('X Tilt Error Vs. Time')
xlabel('Elapsed Time (seconds)')
ylabel('X Tilt Error (degrees)')
%Y TILT PLOTS
figure
subplot(2,1,1)
hold on
plot(Time,Y_Tilt,'b')
plot(Time,known_ytilt.*ones(length(Time)),'r')
hold off
grid on
title('Y Tilt Vs. Time')
xlabel('Elapsed Time (seconds)')
ylabel('Y Tilt (degrees)')
legend('Measured Y Tilt','F180 Tilt')
subplot(2,1,2)
plot(Time,yerror,'k')
grid on
title('Y Tilt Error Vs. Time')
xlabel('Elapsed Time (seconds)')
ylabel('Y Tilt Error (degrees)')
226
avg_x = mean(X_Tilt)
avg_y = mean(Y_Tilt)
avg_xerror = mean(xerror)
avg_yerror = mean(yerror)
std_xtilt = std(X_Tilt)
std_ytilt = std(Y_Tilt)
duration = Time(end) - Time(1)
%Create Plots of processed data
xe = [-.27 -.32 -.44 -.6 .28 .45 .45 .05 .18 .05 .08 .16 .13 -.1 -.05 -0.02]; %X
Error
xsd = [0.21 .25 .24 .26 .24 .21 .3 .3 .36 .32 .35 .31 .29 .32 .25 .22]; %X Standard
Deviation
ye = [.02 .02 -.06 .08 .04 -.1 -.03 .1 -.16 -.14 -.32 -.38 .34 .43 .23 -0.03]; %Y
Error
ysd = [.27 .27 .28 .31 .26 .34 .34 .33 .37 .36 .3 .28 .27 .28 .27 .25];% Y Standard
Deviation
xa = [-1.22 -1.5 -1.42 -1.71 -.89 -.75 -1.32 -.83 2.79 7.74 12.85 20.36 -17.50 23.04 -11.56 -1.26]; %X Angles
ya = [-5.32 -9.12 -14.29 -21.25 11.8 18.18 23.48 7.46 2.94 2.83 2.77 2.74 2.93 3.08
2.84 2.82]; %Y Angles
%Pick out pertinent test data
[xa1,I] = sort(xa(9:16));
xe1 = xe(9:16);
xe1 = xe1(I);
xsd1 = xsd(9:16);
xsd1 = xsd1(I);
[ya1,I] = sort([ya(1:8) ya(16)]);
ye1 = [ye(1:8) ye(16)];
ye1 = ye1(I);
ysd1 = [ysd(1:8) ysd(16)];
ysd1 = ysd1(I);
% %Plot Error and Standard Deviation
% figure
% plot(xa1,xe1,'ro-')
% hold on
% grid on
% plot(ya1,ye1,'bo-')
% axis([-25 25 -.25 .25])
% legend('X-Axis','Y-Axis')
% title('Mean Tilt Error Vs. Tilt Angle')
% ylabel('Mean Tilt Error (degrees)')
% xlabel('Tilt Angle (degrees)')
%
% figure
% plot(xa1,xsd1,'ro-')
% hold on
% grid on
% plot(ya1,ysd1,'bo-')
% axis([-25 25 0 .4])
% legend('X-Axis','Y-Axis')
% title('Standard Deviation Vs. Tilt Angle')
% ylabel('Standard Deviation (degrees)')
% xlabel('Tilt Angle (degrees)')
%Test Data Statistics
OMXE = mean(xe1)
OMYE = mean(ye1)
OMXSD = mean(xsd1)
OMYSD = mean(ysd1)
227
Appendix E: Supporting Test Documents
228
229
230
231
232
233
234
235
236
237
238