STABILIZATION AND NAVIGATION CONTROL SYSTEM

FOR AUTONOMOUS AIRCRAFT

Nick Smith & Aaron Greer



U of U
Dept. of ECE




ABSTRACT

An embedded computer system was designed and implemented to autonomously control a small
airplane. A custom-designed printed circuit board (PCB) was utilized to interface computers and
a host of sensors including a GPS receiver, infrared thermopiles, a three-axis accelerometer, and
a barometric altimeter. The aircraft system model was created using Simulink. Data collected
from the airplane while it was in flight was used to obtain feedback gain values for stability and
optimal speed of response. After gathering flight data and configuring the feedback control loop,
the system was tested and verified as operational for short-term autonomous flights.

1
INTRODUCTION
Autonomous flight is currently a
subject of much discussion, anticipation, and
research. Many large organizations are
investing a great deal of money into the
research and development of small scale,
long range, Unmanned Aerial Vehicles
(UAVs) [1].
This project involves exploration of the
rapidly developing technology and design
methodology surrounding UAVs for the
purpose of developing a fully autonomous
aircraft with the ability to collect and
provide real-time data as it follows a
predetermined flight path.
A trainer-type radio control model
airplane is selected as the airframe for this
project. The trainer body style provides
superior stability and easy access to the
interior of the fuselage. The airframe is also
capable of supporting the extra weight that
is added by auxiliary batteries and
electronics including: a custom designed
micro-computer system, a GPS receiver, and
an RF modem. Aircraft stabilization is
enabled by using infrared thermopiles.
These sensors provide feedback on the
attitude angle of the aircraft; this allows the
software-based control system to
compensate for disturbances like wind and
turbulence. Telemetry information is
continuously broadcast to a ground station.
This data is streamed into a MATLAB
graphical user interface (GUI) which both
displays the data and records it for future
analysis. Figures that are referenced
throughout this document have been placed
in the Appendix.

PREVIOUS WORK
A large amount of research has
previously been performed in the area of
aircraft stabilization and autonomous flight.
Many of the methods used in this project are
based on previous work. Research at
Monash University has been especially
influential in the direction of this project [2].
A method of aircraft stabilization using
thermopiles keyed to sense blackbody
radiation emitted from objects within the
atmosphere has been developed at Monash
University. Several variations on this design
are described in Horizon Sensing Attitude
Stabilisation: A VMC Autopilot [3]. Some
uses of this system include stabilizing the
aircraft itself (tracking reference pitch and
roll angles) and stabilizing a platform
attached to the aircraft which could be used
for a stabilized camera mount, etc.
Other previous work has been
reviewed which focuses on applications of
autonomous aircraft, rather than the control
details. Currently, the most obvious and
dominant use of UAVs is in the realm of
military operations [4]. Unmanned aerial
surveillance provides greater reconnaissance
capability, precision, and opportunity, while
also providing significantly reduced cost
and, most importantly, minimized risk to
life.
In the event of an emergency or
other exigent circumstances such as wartime
or natural disaster, wireless communication
range can be increased by employing an
aircraft as a relay node to receive and
retransmit information, thus bridging the gap
between the original source and the intended
destination [2] [5]. Several aircraft can be
used in a coordinated manner to yield large
increases of radio range. This is most
beneficial when the physical communication
infrastructure is nonfunctional (immediately
following a hurricane, for example), as well
as in geographic areas where such
infrastructure is impractical (such as the
battlefield, ocean, or remote wilderness).
Autonomous aircraft are also well suited for
ground-survey applications such as search
and rescue, disaster assessment, battle
planning, etc [6]. The use of several
autonomous but interlinked aircraft has been
researched for this purpose [7]. Intelligent
2
computing/network systems have been
developed and investigated that would allow
each aircraft to communicate with the others
and, through coordination and intelligent
decision making algorithms, objectives such
as locating a lost person or vehicle can be
accomplished very quickly.

CURRENT WORK
This project does not focus on any
specific application of autonomous aircraft;
rather, it deals with the design and
implementation of stabilization and
navigation control systems to control such
an aircraft. It is believed that, once this
groundwork is laid, future applications
utilizing this system will be easier and faster
to implement, thus increasing the efficiency
and productivity of future research and
development efforts.
Stabilization Control System
The first step in creating a
stabilization control system for an aircraft is
to understand and model (as much as
possible) the dynamics of the open loop
system. Simulink was used to model the roll
and pitch dynamics of the aircraft as two
separate decoupled systems. These models
are very simple and neglect dynamics of
some subsystems as well as nonlinearities
and coupling between the longitudinal and
lateral axes.
Figure 1 shows the longitudinal
system model which approximates the
behavior of the airplane around the pitch
axis. The input,
E
, is the elevator deflection
angle. K
E
is the elevator effectiveness and
relates the elevator deflection angle to the
rate of change of the pitch angle. K
L
is the
lift coefficient and relates the angle of attack
to the vertical acceleration of the aircraft.
The flight path angle is equal to the ratio of
the vertical velocity to the airspeed for small
angles. Closing the loop, the angle of attack
is then defined as the difference between the
pitch angle and the flight path angle.
Figure 2 shows the lateral system
model which corresponds to the roll axis.
The input,
A
, is the aileron deflection angle.
K
A
is the aileron effectiveness, which relates
the aileron deflection angle to the rate of
change of the roll angle.
Two proportional control loops were
designed and simulated using Simulink for
the roll and pitch angles of the aircraft. The
roll and pitch angle references for these
loops are provided by the outer navigational
control loop. For feedback, thermopile
sensors provide the roll and pitch angles of
the aircraft. Figures 3 and 4 show these
control loops. For simulation purposes, the
sensor signal is scaled and given an offset to
mimic the actual data returned by the
thermopiles. Proportional control loops
were chosen because of their simplicity, as
well as their ability to be tuned for a desired
step response. The tuning of gain
parameters for these loops creates a response
that is fast enough to overcome gusting
breezes, yet damped enough to prevent
excess overshoot and oscillations.
Figures 5 and 6 show the step
responses of the simulated roll and pitch
control loops, respectively. The response to
a 30 degree roll angle reference input shows
a large aileron deflection initially, which
decays to zero as the roll angle reference is
tracked. Notice that the heading angle
changes linearly at about 15 degrees per
second. Similarly, the response to a 30
degree pitch angle reference input shows a
large elevator deflection initially, eventually
decaying to zero along with the angle of
attack. The altitude increases linearly at
about 3 meters per second.
Figure 7 shows the step response of
the actual system while in flight. The initial
condition for this case had the airplane in a
steep left bank, while the step input was for
a 15 degree right bank. For comparison,
Figure 8 shows the response of the
simulated control system to similar initial
3
conditions and reference input. The aileron
deflection angle saturates at 30 degrees for
this simulation. The response time is about
2 seconds for both the simulated and
measured data.
Navigation Control System
The navigation system is designed to
use two independent but simultaneous
proportional feedback control loops. The
first loop, shown in Figure 9, manipulates
the bank angle reference to change the
heading angle of the aircraft. This loop
starts with a heading angle reference
(pointing to the next waypoint) and subtracts
the actual heading of the aircraft, resulting in
a heading angle error. This error is
converted to a roll angle reference. The
result,

, is passed to the latitudinal

stabilization control loop, which controls the
roll angle of the aircraft. These outer-loop
calculations are performed once per second
using the most recent GPS data.
The second navigation loop, shown
in Figure 10, controls the aircraft altitude.
Using a pressure based altitude sensor
(sampled at 2 Hz), this loop is able to update
more frequently than the GPS loop. A
reference altitude and the actual altitude are
combined to create an altitude error which is
then proportionally scaled and converted to
a pitch angle reference,
h
. This error is then
passed to the longitudinal stabilization loop
which controls the pitch of the plane.
Hardware Design
Several combinations of sensors
were considered for this project. The
control system requires the ability to
somehow measure the attitude angles (pitch
and roll) of the aircraft as well as the
altitude, position, and velocity. The sensors
selected for use in this project are discussed
in the following section.
A pressure sensor was used to
measure altitude. Readings from this sensor,
received on an SPI bus, were converted to
an altitude by using a linear approximation
based on empirical atmospheric data from
two sources [8] [9]. Plots of this data are
shown in Figure 11. MATLAB was used to
generate a linear fit for the range of 0 to
3000m. This linear model (equation 1) was
then used to convert pressure data to an
altitude relative to the local pressure and
elevation on the ground.
( ) m 9668.38 + Pressure 02 . 96 - Altitude =
(1)

The sensor selected for this purpose is VTI
part number SCP1000-D01. With 16 bits of
precision, the current design is capable of
measuring changes in altitude as small as
20cm.
Infrared thermopiles (part number
MLX90247, from Melexis) were used to
measure the pitch and roll angles of the
aircraft. These sensors measure blackbody
radiation emitted by objects within the
atmosphere and produce an analog voltage.
Optical filtering minimizes the effect of
other light sources (including visible light)
on the measurements. According to the
device datasheet, these sensors produce over
70% of their output based on wavelengths
between 7.5m and 13.5m, which
corresponds to the expected wavelength of
black body radiation in our atmosphere with
temperatures between -53 C and 113 C
(found using Wien's displacement law).
This selectivity causes the sensor to measure
radiation given off by terrestrial objects
(such as the ground) and ignore objects from
deep space (such as the sun). Two pairs of
thermopiles were placed on the aircraft
facing in opposite directions (see Figures 12
and 13). The corresponding circuitry
connects the thermopiles opposite each other
in a differential configuration. Subsequent
measurements of the resultant voltage
correspond to the ratio of energy detected by
the skyward looking thermopile to the
earthward looking thermopile. The voltages
resulting from these two pairs of thermopiles
4
are measured and used to detect the pitch
and roll angles of the aircraft.
The global positioning system (GPS)
receiver (Trimble Lassen-IQ) provides the
information necessary for the aircraft to
navigate a course consisting of a series of
waypoints. The GPS module updates once
per second, and provides (through a serial
interface) longitude, latitude, and velocity
(in east, north, and vertical vector
components). Longitude and latitude data is
used to calculate the bearing angle and
distance to the next waypoint. The velocity
data is used to derive the heading and speed
of the aircraft.
The microcontroller (mcu) selected
for this project is the 8-bit Freescale part
number MC9S08GB60. This mcu was
selected for its low power consumption,
availability of peripherals (10-bit analog to
digital converter, hardware PWM/timer
generator, serial communication ports, etc),
20MHz maximum CPU frequency, and
generous Flash-ROM and RAM memory
resources (necessary for storing a large
number of GPS waypoints and efficient
mathematical routines needed for some
control system functionality).
RF link modems (Shenzhen part
number UM12) operating at 433 MHz,
provide data transmission from the aircraft
to the ground. The model used provides a
maximum range of 1000 m (line of sight)
based on the optimum orientation of the
provided monopole antenna. The data rate
of these modems is 1200 baud, which
provides 150 bytes/second of throughput.
Sensor data from the aircraft is received by a
computer on the ground via a serial
connection for viewing in a custom
MATLAB GUI. The received data is also
stored (with time stamps) in comma
delimited text files. Real time feedback in
addition to stored data provided a
convenient way to identify the system
behavior, tune the control system
parameters, and monitor the system status.
System Integration
The integration of all the hardware
subsystems was achieved efficiently by
using a Printed Circuit Board (PCB) design.
The PCB resides in the airplane fuselage
near the front, mounted securely to the
walls. See Figures 14 and 15 for the front
and back view of the PCB design.
The power system is divided into
two subsystems, each with its own battery.
The first battery, a 4.8V 2000mAh NiMH 4
cell package, provides power to the servo
motors. The second battery (purposely
isolated from the servo motors) is a 6V
2000mAh NiMH 5 cell package that
provides power to the PCB. The PCB has
two onboard voltage regulators (3.3V and
5V) which supply power to the computer
and subsystems. The average power
consumed by the PCB is shown in equation
( )( ) 718mW 6.3V 114mA Power = (2)

Selecting the aircraft control source
(computer or human) is a crucial yet
relatively simple aspect of the overall
design. It is here that servo motors are
assigned a control source. Reliable and
failsafe operation of this function is an
absolute necessity. A custom analog
integrated circuit (IC) was designed and
fabricated to fulfill this need. The IC
monitors the otherwise unused 5
th
channel of
the radio controller. The radio control signal
consists of a sequence of 1 to 2 ms pulses at
an interval of 20 ms. When the IC detects a
pulse width less than 1.5 ms for channel 5, it
connects the servo control circuits to the
computer. When a pulse width greater than
1.5 ms is detected, servo control is routed
from channels 1-4 of the radio controller.
This allows for rapid manual takeover of the
aircraft in the event of an emergency by
simply toggling the 5
th
channel on the radio
controller. In addition, the IC is constantly
5
monitoring the ground signal presence. If
the plane doesnt receive the 5
th
channel
signal from the ground for more than 4
cycles (80 ms), servo control is defaulted to
the onboard computer. Normal switching
operation resumes at the rising edge of the
restored ground signal.
From the design perspective, the IC
recognizes an incoming pulse and produces
a positive potential spike at the gate of a
dissipative switch that drains any previous
charge from the integrating capacitor. This
process takes less than 5 s. Then, the
incoming 1 to 2 ms wide pulse is integrated
for the duration of the pulse. Once the
falling edge is detected, the integrated
charge is clamped until the next pulse
arrives. The clamped charge is held at the
gate of a Schmitt trigger with 600 mV
between thresholds (providing hysteresis) to
avoid undesired state changes between
pulses. The final result is a restored signal
that controls a quad pole double throw
switch, allowing either all four
microcontroller channels or all four radio
control channels to become connected to the
servo motors.
Embedded Software
When writing embedded software for
a safety critical system, there are many risks
and advantages. Risks include unexpected
worst case conditions which affect or
completely halt critical time constrained
behavior (such as controlling the ailerons,
throttle, etc). Problems like this can be a
result of stack overflow, memory leaks, or
race conditions. All of these conditions can
be difficult to foresee, debug, and eliminate.
This is especially true when using library
functions such as floating point math
operations. The problem is exasperated
further by the limited resources of an 8-bit
microcontroller like the one used here.
Despite these and other risks associated with
a software based system, there are also many
advantages. Embedded software allows for
quick field changes and tuning of control
parameters, as well as the possibility of
comparing multiple software solutions and
picking the best one without the obstacle of
having to redesign hardware.
The responsibilities of the computer
system are divided into three categories: 1)
GPS and altitude data processing, 2) attitude
angle data processing and aileron, elevator,
and throttle signal generation, 3)
communication with a ground station via the
RF data link.
All of the required software
functionality, with the exception of the RF
data link transmission, is handled in an
interrupt context. Separate interrupt service
routines (ISRs) were designed to manage the
various external data sources such as GPS
and altimeter. These ISRs ensured that the
data was accessed before new data arrived
from the external source. A timer ISR was
used to ensure a constant sampling rate of
10Hz for the thermopile sensors. Also, to
reduce the latency between when a
measurement was taken and when a new
output was produced, the control loop
equations were implemented directly in the
appropriate ISR.
Each data transfer from the plane to
the ground station is preceded by a two byte
header. The first byte of the header is an
escape byte (0xA0). Whenever this value is
seen in the serial data stream, it indicates the
beginning of a new data packet. The second
byte is a type identifier which distinguishes
the type of data that follows. For example,
header [0xA0 0x01] indicates that new
thermopile data has arrived and MATLAB
then processes the next two bytes of data as
the state of the roll and pitch thermopile
pairs. In contrast, header [0xA0 0x05] tells
MATLAB to expect two 32 bit floating
point values that describe the velocity of the
plane. In the event that the escape character
occurs naturally in the data being
transmitted, it is padded with another escape
6
character. This double escape sequence tells
MATLAB that this is actual data and not the
beginning of a new packet.

RESULTS
The system that was designed and
implemented for this project was highly
successful. The stabilization control system
was able to stabilize the aircraft and reject
disturbances such as wind and manually
induced maneuvers (initial conditions) such
as dives and rolls (see Figure 16). The
navigation system was able to track the
position and speed of the plane, however the
computer resources required to calculate the
position and heading error were not
available in the current implementation.

FUTURE WORK
The next step for this project is to
complete the control system implementation
by devising a solution to the navigation
calculation requirements. Some possible
remedies include using a higher powered
microprocessor such as a 32-bit CPU, using
programmable logic such as an FPGA, or
using a floating point coprocessor to offload
the floating point math from the 8-bit
microcontroller. In addition to overcoming
the calculation shortcomings, a faster and
more accurate GPS receiver will also be
utilized. The next revision of the main PCB
for this project will also be refined and
shrunk in size to reduce size and weight
aboard the plane. A more effective RF
modem will be used that provides a higher
data throughput and uses a better suited
antenna such as a dipole or patch. Beyond
just improving upon the current iteration of
this design, efforts will be made to utilize
this functionality to explore some of the
UAV applications mentioned previously.

CONCLUSION
This project involved the design and
implementation of a stabilization and
navigation control system for a small UAV.
The design was based around two control
loops, one for the roll axis of the aircraft,
and the other for the pitch axis. The aircraft
dynamics were simulated using Simulink
before the control systems were designed
and implemented. After implementing the
stabilization control system, it was tested
and shown to effectively stabilize the
aircraft as well as reject disturbances such as
wind. Additional work will be directed
towards completing the navigation control
system as well as applying this overall
system to some interesting uses such as
coordinated search and wireless
communication range augmentation.

ETHICAL AND SOCIETAL ISSUES
During the development of this
project, many compromises had to be made.
There were choices associated with
component selection and the overall
functionality that would be implemented.
Compromises and choices were based on the
need to keep the system cost low while still
providing some minimum amount of
functionality. Some of the functions in
question relate to the navigation capabilities
of the plane as well as the multiplexing
between manual and automatic flight
control. Choices were made and
components were selected that would
provide the most safety to those on the
ground and in the air, while providing some
measure of navigation capability as well.
There are many safety issues
associated with mixing manned and
unmanned aircraft in the sky. The most
obvious concern is the risk of injury or loss
of life faced by those in the manned aircraft.
Balancing these concerns is the need for
capabilities provided by unmanned aircraft
(UAVs). Small UAVs provide a variety of
contributions to society including search and
rescue during times of disaster and aerial
reconnaissance for military or civilian uses.
7
The aerial reconnaissance capabilities of
small UAVs allow observation of areas
where manned flight is prohibited because
of some danger associated with the
battlefield, forest or structural fire,
chemicals, or weather hazards. An example
a situation where UAVs could (and should)
have been used is in the New Orleans area
following the hurricane Katrina disaster
[10]. Requests were made to have UAVs
available for search and rescue as well as
damage assessment needs. While many
UAVs were provided for this purpose,
because of ambiguity in the regulations
governing the use of UAVs and manned
aircraft in the same airspace, the UAVs
where never allowed to take flight.
These ethical and regulatory
conflicts need to be resolved. There are
many instances where using a UAV in the
place of a manned aircraft is the preferred
choice. Human lives can be kept from
unnecessary risk and those people who need
to be rescued can be found more effectively
by employing small groups of UAVs.


ACKNOWLEDGMENTS

The authors of this work wish to
acknowledge the contributions made by the
following individuals and organizations:

Professor Marc Bodson (Dept. of ECE,
University of Utah, Project Advisor)
Provided direction and technical
guidance throughout the project

Colmek Systems Engineering
(www.colmek.com) Provided
monetary sponsorship for the project
which allowed for purchases of the
aircraft, electronics, and support
equipment

Ute RC Club (www.uterc.org) Provided
flight training and invaluable
assistance during test flights, without
which, this project would not have
been possible due to the guaranteed
destruction of the aircraft

Advanced Circuits (www.4pcb.com)
Provided PCB fabrication services
free of charge

In addition to the above noted individuals
and organizations, the following software
enabled much of this project to be
completed successfully:

Codewarrior IDE (Freescale) - C compiler
for embedded software design

Altium DXP - PCB layout and design

Cadence Suite - Simulation and layout of
multiplexing IC

MATLAB/Simulink System modeling,
control design, calculations, data
capture, viewing, and analysis

National Geographic TOPO - GPS
navigation pattern layout and plane tracking.

Servoy - Parts inventory and ordering
database design










1

REFERENCES


[1] Insitu Group, InstituScanEagle. [Online]. Available:
http://www.insitu.com/prod_scaneagle.cfm [Accessed July 19, 2006].

[2] Egan, G.K., Cooper, R.J., and Taylor, B., Unmanned Aerial Vehicle Research at
Monash University, Proc. of the Eleventh Australian International Aerospace Congress,
Melbourne, Australia, Mar 2005.

[3] Taylor, B., Bil, C., Watkins, S. and Egan, G.K., Horizon Sensing Attitude Stabilisation:
A VMC Autopilot, Proc. of the 18th International UAV Systems Conference, Bristol,
UK, Mar 2003.

[4] Federation of American Scientists (FAS), Unmanned Aerial Vehicles (UAVs) Military
Aircraft, Available: http://www.fas.org/irp/program/collect/uav.htm

[5] Gu, D., Pei, G., Ly, H., Gerla, M., Zhang, B., and Hong, X., UAV Aided Intelligent
Routing for Ad-Hoc Wireless Network in Single-Area Theater, Proc. of the Wireless
Communications and Networking Conference, Chicago, Illinois, Sept 2000.

[6] Jin, Y., Minai, A., and Polycarpou, M., Cooperative Real-Time Search and Task
Allocation in UAV Teams, Proc. of the 42nd IEEE Conference on Decision and
Control, Dec 2003.

[7] Ryan, A., Zennaro, M., Howell, A., Sengupta, and R., Hedrick, J., An Overview of
Emerging Results in Cooperative UAV Control, Proc. Of the 43rd IEEE Conference on
Decision and Control, Atlantis, Paradise Island, Bahamas, Dec 2004.

[8] Wikipedia, Altimeter. [Online]. Available: http://en.wikipedia.org/wiki/Altimeter
[Accessed June 3, 2006].

[9] Quantitative Environmental Learning Project, Altitude VS Pressure. [Online].
Available: http://www.seattlecentral.org/qelp/sets/024/024.html [Accessed June 23,
2006].

[10] Karp, J. and Pasztor, A., Drones in Domestic Skies?, Wall Street Journal, Page B1,
Aug 2006.






2
APPENDIX



















Figure 1
Longitudinal Aircraft Model






















Figure 2
Lateral Aircraft Model


Heading
Angle
Aileron
Deflection

A


g

sV
Lateral Aircraft Model
K
A

s
Roll Angle
Sensor
Elevator
Deflection

E
-
A of A
Attitude
Sensor
s
1


K
L

Longitudinal Aircraft Model
s
K
E

s
1

V
1
h a
n
g
Normal
Accelerometer
h Altimeter
Flight Path Angle
Pitch Angle
Sensor
h Vertical Velocity

Angle of Attack
3


Figure 3
Roll Angle Control Loop



Figure 4
Pitch Angle Control Loop
4

Figure 5
Simulated Lateral Closed Loop Step Response


Figure 6
Simulated Longitudinal Closed Loop Step Response
5


Figure 7
Measured Closed Loop Step Response


Figure 8
Simulated Closed Loop Step Response

6

Figure 9
Heading Control Loop


Figure 10
Altitude Control Loop





Longitudinal
Stabilization
Lateral
Stabilization
7

Figure 11
Altitude VS Pressure
8

Figure 12
Roll Axis Thermopiles


Figure 13
Pitch Axis Thermopiles


9

Figure 14
PCB Front


Figure 15
PCB Back

10

Figure 16
Dashed Box Denotes Time When Control System Was Engaged
(The Aircraft Was in Manual Control Before and After)


275 280 285 290 295 300 305 310 315 320 325
80
100
120
140
160
180
200
220
240
Thermopile Data
AutoPilot Stability Test
Seconds
Pitch
Roll
11
EMBEDDED C CODE

/*
Autonomous Airplane Control System
Aaron Greer & Nick Smith
University of Utah, 2006-2007
*/

#include <hidef.h> /* for EnableInterrupts macro */
#include <math.h>
#include "derivative.h" /* include peripheral declarations */

#define BYTE unsigned char
#define BOOL unsigned char
#define WORD unsigned short
#define TIMER1_1MS 157
#define TIMER1_100MS 15625

// BAUDxxxx=(MAINCLKFREQ/(16*BAUDRATE))
#define BAUD1200 1041
#define BAUD9600 120
#define initSCI1C2 0x08 // %00001000 ;SCI1 Control Register 2 - RF
/* ||||||||
; |||||||+-SBK ----- not sending break
; ||||||+--RWU ----- receiver wakeup not on
; |||||+---RE ------ receiver disabled
; ||||+----TE ------ transmitter enabled
; |||+-----ILIE ---- idle line interrupt disabled
; ||+------RIE ----- Rx interrupts disabled
; |+-------TCIE ---- Tx complete interrupt disabled
; +--------TIE ----- Tx interrupt disabled (TDRE)
*/

#define initSCI2C2 0x04 // %00000100 ;SCI2 Control Register 2 - GPS
/* ||||||||
; |||||||+-SBK ----- not sending break
; ||||||+--RWU ----- receiver wakeup not on
; |||||+---RE ------ receiver enabled
; ||||+----TE ------ transmitter enabled
; |||+-----ILIE ---- idle line interrupt disabled
; ||+------RIE ----- Rx interrupts disabled
; |+-------TCIE ---- Tx complete interrupt disabled
; +--------TIE ----- Tx interrupt disabled (TDRE)
*/

// TSIP packet control bytes
#define DLE 0x10
#define ETX 0x03
#define GPSPOS 0x4A
#define GPSVEL 0x56
#define GPSHEALTH 0x46
#define GPSDGPSMODE 0x82
#define GPSSTATUS 0x4B

// telemetry packet ids for packets sent to PC
#define RF_ESC 0x0A
#define RF_PRESSURE 0x01
#define RF_ACCEL 0x02
#define RF_IR 0x03
#define RF_SPDHEAD 0x04 // spead and heading over ground
#define RF_LATLON 0x05 // current position data
#define RF_BEARDIST 0x06 // bearing and distance to target
12
#define RF_VEL 0x07 // East and North velocity vectors
#define RF_PRESSURE_HOME 0x08 // baseline pressure
#define RF_LATLON_HOME 0x09 // initial position


// GPS states
#define GPSWAIT 1 // waiting for a packet
#define GPSGOTFIRST 2 // got DLE
#define GPSGOTPOS 3 // got id for Single LLA Position (0x4A)
#define GPSGOTVEL 4 // got id for Single ENU Velocity (0x56)
#define GPSGOTCONFIG 5 // got id for Pos and Vel configuration (0x55)
#define GPSENDPACKET 6 // get DLE,ETX to end the packet
#define GPSSTOP 7 // use this state to park for debugging
#define GPSGOTHEALTH 8 // got the RX health packet
#define GPSGOTDGPSMODE 9 // got the DGPS mode packet
#define GPSGOTSTATUS 10


// radio trim settings
/*
rudder: center=1.58ms right=1.27ms left=1.89ms
elevator: center=1.53ms up=1.2ms down=1.88ms
throttle: closed=1.35ms open=1.84ms
aileron: center=1.48ms right=1.75ms left=1.22ms

*/

// servo PWM values: T=400nsec, 2500*T=1ms, 3750*T=1.5ms, 5000*T=2ms, 50000*T=20ms
// servo range: 2500-5000 = 2500 ticks
#define ONEMS 2500
#define ONEPFIVEMS 3750
#define ONEPSEVENFIVEMS 4375
#define TWOMS 5000
#define TWENTYMS 50000


#define AILERON_MIN 3050 // full left: 1.22ms
#define AILERON_CENTER 3700 // 1.48ms
#define AILERON_MAX 4375 // full right: 1.75ms

#define ELEVATOR_MIN 3000 // full up: 1.2ms
#define ELEVATOR_CENTER 3825 // 1.53ms
#define ELEVATOR_MAX 4700 // full down: 1.88ms

#define THROTTLE_MIN 3375 // full closed: 1.35ms
#define THROTTLE_CENTER 4450 // 4293 == too small // ~75% open: 1.71ms
#define THROTTLE_MAX 4600 // full open: 1.84ms

#define RUDDER_MIN 3175 // full right: 1.27ms
#define RUDDER_CENTER 3950 // 1.58ms
#define RUDDER_MAX 4725 // full left: 1.89ms

#define AILERON_VAL TPM2C0V // PWM compare value regs
#define ELEVATOR_VAL TPM2C1V
#define THROTTLE_VAL TPM2C2V
#define RUDDER_VAL TPM2C3V

// PID control values
#define AILERON_KP 8
#define AILERON_KI 0
#define AILERON_KD 0
#define ELEVATOR_KP 8 // 16== too much , might need to reduce the endpoints
#define ELEVATOR_KI 0
13
#define ELEVATOR_KD 0





// ************ Globals ************
//*

byte gGPSstate;
byte gGPSbytecount;
byte gGPSDLEcount;

struct position {
float Lat;
float Lon;
float Alt;
byte IsValid;
};
struct position gGPSPosition; // hold current position
struct position gGPSPositionTarget; // hold target position
struct position gGPSPositionHome; // initial position of the plane
byte *LatPtr; // use for loading floats from serial stream
byte *LonPtr;
byte *AltPtr;

struct velocity {
float ESpeed;
float NSpeed;
float USpeed;
byte IsValid;
};
struct velocity gGPSVelocity;
byte *ESpeedPtr;
byte *NSpeedPtr;
byte *USpeedPtr;

struct velocity_and_heading {
float Velocity;
float Heading;
byte IsValid;
};
struct velocity_and_heading gGPSVelocityAndHeading;
float gGPSHeadingToTarget;
float gCosLat;

const float gEarthRadiusMeters= 6378137.5275; // radius of the earth in meters
const float gRadToDeg= 180./_M_PI; // converts Radians to Degrees
const float gDegToRad= _M_PI/180.; // converts Degrees to Radians
const float gOneRadDist=6378137.5275; // distance between 1 Rad in meters
//*/


BYTE gALT_STATE;
enum ALTIMETER_STATES {
ALT_WAIT,
ALT_TX1,
ALT_TX2,
ALT_TX3,
ALT_TX4,
ALT_TX5
};
unsigned long gPressure;
14
unsigned long gPressureHome;
BYTE gPressureReadyForTx;

BYTE gATD_state;
enum ATD_STATES{
ATD_WAIT,
ATD_GET_IR1,
ATD_GET_IR2,
ATD_GET_Z,
ATD_GET_X,
ATD_GET_Y
};
WORD gIR1; // decide if these can be 8 bits (12.89mV resolution)
WORD gIR2;
WORD gZ;
WORD gX;
WORD gY;
BYTE gIRReadyForTx;
BYTE gXYZReadyForTx;

const BYTE gDoStabPID = 1; // turn on and off the stablization PID code
const BYTE gDoServos = 1; // turn on and off the servo code

signed short gRollRef = (130<<2); // nice right bank about 15 degrees = (118<<2); //
IR2 reference
signed short gPitchRef = (118<<2); // IR1 reference



// ************ end Globals ************



// ************ function prototypes ************
void ICG_init(void);
void TIMER1_init(void);
void TIMER1_delay_1ms(void);
interrupt Vtpm1ch1 void TIMER1_CH1_isr(void);
void TIMER1_CH1_isr_disable(void);
void TIMER1_CH1_isr_enable(void);

void SCI1_init(void);
void RF_init(void);
BOOL SCI1_HasRxData(void);
BOOL SCI1_ReadyToTx(void);
void SCI1_put(BYTE b);
BYTE SCI1_get(void);
void SCI1_encode_and_transmit(BYTE *Bptr,BYTE numBytes,BYTE dataType);

void SPI_init(void);
void ALTIMETER_init(void);
void ALTIMETER_write_indirect(BYTE address, BYTE data);
void ALTIMETER_write_direct(BYTE address,BYTE data);
WORD ALTIMETER_read_direct(BYTE address, BYTE num_bytes);
WORD ALTIMETER_read_indirect(BYTE address);
void ALTIMETER_select(void);
void ALTIMETER_deselect(void);
unsigned long ALTIMETER_RX_pressure(void);
BOOL SPI_HasRxData(void);
BYTE SPI_get(void);
BOOL SPI_ReadyToTx(void);
void SPI_put(BYTE b);

15
void ATD_init(void);
interrupt Vatd void ATD_isr(void);
void ATD_set_channel(BYTE channel);
void ATD_isr_enable(void);
void ATD_isr_disable(void);

void GPS_init(void);
void SCI2_init(void);
void SCI2_Rx_ISR_disable(void);
void SCI2_Rx_ISR_enable(void);
interrupt Vsci2rx void SCI2RDRFisr(void);
float GPS_GetBearing(float dx,float dy);
float GPS_GetDist(float dx,float dy);
void GPS_GetDxDy(float *dx, float *dy, float lat1, float lon1, float lat2, float
lon2);
float GPS_GetHeading(float ESpeed, float NSpeed);
float GPS_GetSpeed(float ESpeed, float NSpeed);

// ************ end function prototypes ************




// ************ ICG code ************
void ICG_init(void){
//FLL loop multiplier N=8, R=1, P=1 (because in low freq xtal mode)
// 5MHz*P*N/R -> 40MHz (/2 for bus rate) -> 20MHz bus/instruction clock
rate T=50nsec
ICGC2 = ICGC2_MFD1_MASK;
// select FLL engaged external, enable oscillator amplifier
ICGC1 = ICGC1_REFS_MASK|ICGC1_CLKS1_MASK|
ICGC1_CLKS0_MASK|ICGC1_RANGE_MASK|ICGC1_OSCSTEN_MASK;
while(ICGS1_LOCK==0); // loop until FLL locks

}
// ************ end ICG code ************

// ************ TIMER1 code ************
void TIMER1_init(void){
// setup timer 1 module
// use bus rate clock, prescale by 128 T= 50ns * 128 = 6.4us (20,000 ticks
= 1ms)
TPM1SC = TPM1SC_CLKSA_MASK|TPM1SC_PS2_MASK|TPM1SC_PS1_MASK|TPM1SC_PS0_MASK;

// setup timer 1 channel 0
// for output compare (used for delay subroutine)
// output compare mode, software only
TPM1C0SC = TPM1C0SC_MS0A_MASK;
TPM1C0SC_CH0F; // read flag CH0F
TPM1C0SC_CH0F = 0; // to clear flag, write zero to CH0F

// setup timer 1 channel 1
// for output compare with interrupt
// will trigger every 100ms to start a new round of ATD conversions
TPM1C1SC = TPM1C1SC_MS1A_MASK;
TPM1C1SC_CH1F; // read flag CH1F
TPM1C1SC_CH1F = 0; // to clear flag, write zero to CH1F
}

void TIMER1_CH1_isr_enable(void){
TPM1C1SC_CH1IE = 1;
}
void TIMER1_CH1_isr_disable(void){
16
TPM1C1SC_CH1IE = 0;
}
interrupt Vtpm1ch1 void TIMER1_CH1_isr(void){ // start an ATD sequence very 100ms
//PTAD &= ~1; // turn D2 on
TPM1C1SC_CH1F; // read flag CH1F
TPM1C1SC_CH1F = 0; // to clear flag, write zero to CH1F

// setup the new compare value for another 100ms
TPM1C1V = TPM1CNT + TIMER1_100MS;

// start the ATD conversion
ATD_set_channel(0);
ATD_isr_enable();
gATD_state = ATD_GET_IR1;
//PTAD |= 1; // turn D2 off
}

void TIMER1_delay_1ms(void){ // uses timer1 channel 0 for a delay of 1ms
TPM1C0V = TPM1CNT + TIMER1_1MS; // setup compare value for 1ms worth of ticks
while( TPM1C0SC_CH0F==0); // wait 1ms for CH0F to be set
TPM1C0SC_CH0F = 0; // to clear flag, write zero to CH0F
}
// ************ end TIMER1 code ************


// ************ TIMER2/servo code ************

void TIMER2_init(void){
// setup timer 2 module -- use to drive servo signals
// use bus rate clock, prescale by 8, T=400nsec
// servo values: 2500*T=1ms, 3750*T=1.5ms, 5000*T=2ms, 50000*T=20ms
// servo range: 2500-5000 = 2500 ticks
TPM2SC = TPM2SC_CLKSA_MASK|TPM2SC_PS1_MASK|TPM2SC_PS0_MASK;
TPM2MOD = TWENTYMS; // setup modulo for PWM period of 20ms
// setup channels 0-3 to drive servos
TPM2C0SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
TPM2C1SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
TPM2C2SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
TPM2C3SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;

AILERON_VAL = ONEPFIVEMS; // initial pulse width is 1.5ms (SET TO PROPER
MIDPOINT LATER)
ELEVATOR_VAL = ONEPFIVEMS;
THROTTLE_VAL = ONEPFIVEMS;
RUDDER_VAL = ONEPFIVEMS;
}

// ************ end TIMER2/servo code ************


// ************ SCI1/RF code ************
void RF_init(void){
unsigned short i;

PTGDD |= (1<<6); // make the RF_reset pin an output
PTGD |= (1<<6); // make the RF module reset

SCI1_init();

PTGD &= ~(1<<6); // bring the RF module out of reset

for (i=0;i<60000;i++);
PTGD |= (1<<6); // make the RF module reset again
17
for (i=0;i<60000;i++);
for (i=0;i<60000;i++);
PTGD &= ~(1<<6); // bring the RF module out of reset again
for (i=0;i<60000;i++);

SCI1D = 0; // transmit a dummy byte
while (SCI1S1_TDRE!=1); // wait until serial port is finished txing
for (i=0;i<60000;i++);
for (i=0;i<60000;i++);
}

void SCI1_init(void){
SCI1BD = BAUD1200; // setup the serial port hardware for RF comm
SCI1C2 = initSCI1C2;
}
BOOL SCI1_HasRxData(void){
if (SCI1S1_RDRF==0)
return FALSE;
else
return TRUE;
}
BOOL SCI1_ReadyToTx(void){
if (SCI1S1_TDRE==0)
return FALSE;
else
return TRUE;
}
void SCI1_put(BYTE b){
SCI1D = b;
}
BYTE SCI1_get(void){
return SCI1D;
}
// packet format: {RF_ESC,ID,DATA} (RF_ESC is used to precede any esc values in data)
void SCI1_encode_and_transmit(BYTE *Bptr,BYTE numBytes,BYTE dataType){
while(!SCI1_ReadyToTx()); // wait until ready to TX again
SCI1_put(RF_ESC); // send the start byte
while(!SCI1_ReadyToTx()); // wait until ready to TX again
SCI1_put(dataType); // send the type identifier
while (numBytes>0){ // transmit the data section

while(!SCI1_ReadyToTx()); // wait until ready to TX again
if (*Bptr == RF_ESC){ // check if this data byte is an escape code
SCI1_put(RF_ESC); // precede data byte with an escape
while(!SCI1_ReadyToTx()); // wait until ready to TX again
}
// now transmit the actual data byte (MSB first, big endian)
SCI1_put(*Bptr);
Bptr++; // increment the data pointer
numBytes--;
}
}

// ************ end SCI1/RF code ************


// ************ SPI/KB/Altimeter code ************
void SPI_init(void){
// enable SPI as a master
SPI1C1 = SPI1C1_SPE_MASK|SPI1C1_MSTR_MASK;
//
SPI1C2 = 0;
// SPI baud rate: prescale by 5, divide by 8 -> 20MHz/(5*8) = 500KHz
18
SPI1BR = SPI1BR_SPPR2_MASK|SPI1BR_SPR1_MASK;

// make MOSI and SCK outputs
PTEDD |= (1<<4)|(1<<5);
// make MISO input
PTEDD &= ~(1<<3);

}
BOOL SPI_HasRxData(void){
if (SPI1S_SPRF==0)
return FALSE;
else
return TRUE;
}
BYTE SPI_get(void){
return SPI1D; }
BOOL SPI_ReadyToTx(void){
if (SPI1S_SPTEF==0)
return FALSE;
else
return TRUE;
}
void SPI_put(BYTE b){
SPI1D = b;
}
void SPI_RX_ISR_enable(void){
SPI1C1_SPIE = 1;
}
void SPI_RX_ISR_disable(void){
SPI1C1_SPIE = 0;
}
void SPI_TX_ISR_enable(void){
SPI1C1_SPTIE = 1;
}
void SPI_TX_ISR_disable(void){
SPI1C1_SPTIE = 0;
}
interrupt Vspi void SPI_isr(void){ // should only need to use the RX isr
functionality
SPI1S; // read the RX/TX flags

switch(gALT_STATE) {
case ALT_WAIT:
SPI_RX_ISR_disable(); // should never get here;
SPI_TX_ISR_disable();
break;
case ALT_TX1: // we just txed an address and rxed the first dummy byte
SPI_get(); // read the dummy byte to clear the rx buffer
SPI_put(0x00); // dummy transmit to get data back
gALT_STATE = ALT_TX2;
break;
case ALT_TX2: // we just txed the first dummy byte and read the 8-bit value
ALTIMETER_deselect(); // finished with the first transaction
gPressureReadyForTx = FALSE;
gPressure = (unsigned long)(SPI_get()); // read the 3MSbits of the pressure
value
gPressure <<= 16; // move the MSbits to the top
ALTIMETER_select(); // begin the second transaction for the 2 LSbytes
SPI_put(0x20<<2);
gALT_STATE = ALT_TX3;
break;
case ALT_TX3: // we just txed the address and rxed the first dummy byte
SPI_get(); // read the dummy byte to clear the rx buffer
19
SPI_put(0x00); // dummy transmit to get data back
gALT_STATE = ALT_TX4;
break;
case ALT_TX4: // we just txed the first dummy byte and read the upper of two
LSbytes
gPressure |= ((unsigned long)(SPI_get()<<8)); // read the upper of two LSbytes
SPI_put(0x00); // dummy transmit to get data back
gALT_STATE = ALT_TX5;
break;
case ALT_TX5: // we just txed the second dummy byte and rxed the lower of two
LSbytes
gPressure |= ((unsigned long)(SPI_get())); // read the lower of two LSbytes
ALTIMETER_deselect(); // finished with the second transaction
gPressure >>= 2; // divide by 4 to convert to units of Pascals
gALT_STATE = ALT_WAIT; // (later: go into a state to calculate new outputs)
gPressureReadyForTx = TRUE; // signal to 'main' that a pressure value can be
TXed
SPI_RX_ISR_disable(); // we have the pressure value now, so disable this
isr
break;
default:
gALT_STATE = ALT_WAIT;
break;
} // switch
//PTAD ^= 0xff; // toggle both LEDs
}

void KB_init(void){
PTAPE |= PTAPE_PTAPE5_MASK; // enable internal pull down resistors
KBI1SC = KBI1SC_KBEDG5_MASK|KBI1SC_KBACK_MASK; //rising edge (only) sensitive &
clear int flag
KBI1PE = KBI1PE_KBIPE5_MASK; // enable KB pin (PTA5) for KBI function
}

void KB_ISR_enable(void){
KBI1SC_KBIE=1;
}
void KB_ISR_disable(void){
KBI1SC_KBIE=0;
}
interrupt Vkeyboard void KB_isr(void){
KBI1SC |= KBI1SC_KBACK_MASK; // clear interrupt flag
// do this if SPI is ready and DTRDY is high
if ( SPI_ReadyToTx() && ((PTAD&(1<<5))==(1<<5)) ){
gALT_STATE=ALT_TX1;
ALTIMETER_select();
SPI_put( 0x1F<<2 ); // initiate the pressure transmission with the altimeter
SPI_RX_ISR_enable();
}

}

void ALTIMETER_init(void){
BYTE i,j;
gALT_STATE = ALT_WAIT;
gPressureReadyForTx = FALSE;
SPI_init();
KB_init();

// config the data ready pin as an input
// use a KB interrupt pin to trigger an interrupt
PTFDD &= ~(1<<7); // clear for input

20
// config chip select pin as output
ALTIMETER_deselect(); // de-select the altimeter
PTEDD |= (1<<7);


for (i=0;i<=160;i++) // POR delay of at least 150ms
TIMER1_delay_1ms();

// check startup status
for (i=0;i<7;i++){
if ( !(ALTIMETER_read_direct(0x07,1) & 0x0001) ){
break;
}
for (j=0;j<=160;j++) // delay 10ms
TIMER1_delay_1ms();

}
if (i==7)
while(1); // stop here if startup status is bad

// check EEPROM checksum
if ( !(ALTIMETER_read_direct(0x1F,1) & 0x0001) )
while(1); // stop here for back checksum


// LOW NOISE CONFIGURATION
ALTIMETER_write_indirect(0x2D, 0x03);

for (i=0;i<=110;i++) // delay of at least 100ms
TIMER1_delay_1ms();

ALTIMETER_write_direct(0x03, 0x00); // reset operation mode

for (i=0;i<=10;i++) // delay of at least 10ms
TIMER1_delay_1ms();

// start high resolution continuous sampling
// write 0x0A to OPERATION (0x03)
ALTIMETER_write_direct(0x03, 0x0A);
}

// writes an address to a pointer, and data to a temp,
// then the command to move the data to the reg
void ALTIMETER_write_indirect(BYTE address, BYTE data){
BYTE i;
ALTIMETER_write_direct(0x02, address);
ALTIMETER_write_direct(0x01, data);
ALTIMETER_write_direct(0x03, 0x02);
for (i=0;i<=50;i++) // delay of at least 50ms
TIMER1_delay_1ms();
}
// writes a data value directly to an addressable reg
void ALTIMETER_write_direct(BYTE address,BYTE data){
while( !SPI_ReadyToTx() ); // wait until ready to TX
ALTIMETER_select();
SPI_put( (address<<2)|(1<<1) ); // send address and write command
while (!SPI_HasRxData()); // wait until the first byte (dummy) is read
SPI_get(); // read the dummy byte to clear the rx buffer
while( !SPI_ReadyToTx() ); // wait until ready to TX
SPI_put( data ); // send the data
while (!SPI_HasRxData()); // wait until the second byte (dummy) is read
SPI_get(); // read the dummy byte to clear the rx buffer
ALTIMETER_deselect();
21
}

// directly reads one or two bytes
WORD ALTIMETER_read_direct(BYTE address, BYTE num_bytes){
WORD data=0;
while( !SPI_ReadyToTx() ); // wait until ready to TX
ALTIMETER_select();
SPI_put( address<<2 ); // send address and read command
while (!SPI_HasRxData()); // wait until the first byte (dummy) is read
SPI_get(); // read the dummy byte to clear the rx buffer

while(num_bytes > 0){
num_bytes--;
data <<= 8;
while( !SPI_ReadyToTx() ); // wait until ready to TX
SPI_put(0x00); // dummy transmit to get data back
while (!SPI_HasRxData()); // wait until the first byte (dummy) is read
data |= SPI_get(); // read the data from altimeter
}
ALTIMETER_deselect();
return data;
}
// indirectly read two bytes from DATARD16 (0x20)
WORD ALTIMETER_read_indirect(BYTE address){
BYTE i;
WORD data = 0;
ALTIMETER_write_direct(0x02, address); // setup read pointer
ALTIMETER_write_direct(0x03, 0x01); // command for indirect read
for (i=0;i<=10;i++) // delay of at least 10ms
TIMER1_delay_1ms();
data = ALTIMETER_read_direct(0x20, 0x02); // read two bytes from DATARD16 (0x20)
return data;

}

// clears the SS pin for the altimeter
void ALTIMETER_select(void){
PTED &= ~(1<<7); // select the altimeter
}
// sets the SS pin for the altimeter
void ALTIMETER_deselect(void){
PTED |= (1<<7); // de-select the altimeter
}

// pressure value is fixed point with 2 decimal places ie. divide by 4 for fract value
unsigned long ALTIMETER_RX_pressure(void){
unsigned long pressure=0;
while ( (PTFD & (1<<7)) != (1<<7) ) // wait until pressure data is ready
TIMER1_delay_1ms();
pressure = (unsigned long)(ALTIMETER_read_direct(0x1F,1)&0x07); // read first byte
w/ 3 MSbits
pressure <<= 16; // move MSbits to the top
pressure |= (unsigned long)ALTIMETER_read_direct(0x20,2); // read two LSbytes
pressure >>= 2; // divide by four to make units of Pascals
return pressure;
}

// ************ end SPI code ************

// ************ ATD code ************
void ATD_init(void){
// enabld ATD, right aligned, 10-bit, unsigned, prescale by 10 for 2MHz conversion
clock
22
ATD1C = ATD1C_ATDPU_MASK|ATD1C_DJM_MASK|ATD1C_PRS2_MASK;
// enable PB0 to PB4 for ATD input
ATD1PE = ATD1PE_ATDPE0_MASK|ATD1PE_ATDPE1_MASK|
ATD1PE_ATDPE2_MASK|ATD1PE_ATDPE3_MASK|
ATD1PE_ATDPE4_MASK;
// single conversion, start with CH0
ATD1SC = 0;

gATD_state = ATD_WAIT;
}

interrupt Vatd void ATD_isr(void) {
signed short Error;
WORD aileron_output,elevator_output;
WORD newATDvalue = (ATD1R & 0x03FF); // read 10-bit atd value and clear CCF
//PTAD &= ~1; // turn D2 on
switch(gATD_state){
case ATD_WAIT: // should not get here
ATD_isr_disable();
break;
case ATD_GET_IR1: // should get here from timer1ch0 isr - save value from IR1
gIR1 = newATDvalue;
gIRReadyForTx = FALSE;
gATD_state = ATD_GET_IR2;

if (gDoStabPID==1){ // calculate the new elevator servo output
// (larger PW = more down elevator)
// (larger IR1 = nose is down ???? correct???)
Error = (signed short)gIR1 - gPitchRef ;
elevator_output = ELEVATOR_CENTER + Error*ELEVATOR_KP;
if (elevator_output > /*(ELEVATOR_CENTER+300)*/ ELEVATOR_MAX) {
ELEVATOR_VAL = /*(ELEVATOR_CENTER+300);*/ ELEVATOR_MAX;
} else if (elevator_output < /*(ELEVATOR_CENTER-450)*/ ELEVATOR_MIN){
ELEVATOR_VAL = /*(ELEVATOR_CENTER-450);*/ ELEVATOR_MIN;
} else {
ELEVATOR_VAL = elevator_output;
}
// adjust the throttle servo?
}

ATD_set_channel(1); // start converting channel 1 - IR2
break;
case ATD_GET_IR2: // save value from IR2
gIR2 = newATDvalue;
gATD_state = ATD_GET_Z;
gIRReadyForTx = TRUE;

if (gDoStabPID==1){ // calculate the new aileron servo output
// (larger PW = right bank)
// (larger IR2 = right bank)
Error = (signed short)gIR2 - gRollRef;
aileron_output = AILERON_CENTER + Error*AILERON_KP;
if (aileron_output > /*(AILERON_CENTER+300)*/ AILERON_MAX) {
AILERON_VAL = /*(AILERON_CENTER+300);*/ AILERON_MAX;
} else if (aileron_output < /*(AILERON_CENTER-300)*/ AILERON_MIN){
AILERON_VAL = /*(AILERON_CENTER-300);*/ AILERON_MIN;
} else {
AILERON_VAL = aileron_output;
}
}

ATD_set_channel(2); // start converting channel 2 - Z
break;
23
case ATD_GET_Z: // save value from Z
gZ = newATDvalue;
gXYZReadyForTx = FALSE;
gATD_state = ATD_GET_X;
ATD_set_channel(3); // start converting channel 3 - X
break;
case ATD_GET_X: // save value from X
gX = newATDvalue;
gATD_state = ATD_GET_Y;
ATD_set_channel(4); // start converting channel 4 - Y
break;
case ATD_GET_Y: // save value from Y
gY = newATDvalue;
gATD_state = ATD_WAIT; // (later: go into a state to calculate new
outputs)
gXYZReadyForTx = TRUE;
ATD_isr_disable(); // completed the measuring sequence, disable this isr
break;
default:
gATD_state = ATD_WAIT;
ATD_isr_disable();
break;
}
//PTAD |= 1; // turn D2 off

}
void ATD_isr_enable(void){
ATD1SC_ATDIE = 1;
}
void ATD_isr_disable(void){
ATD1SC_ATDIE = 0;
}
void ATD_set_channel(BYTE channel){
ATD1SC_ATDCH = channel;
}

// ************ end ATD code ************



// ************ GPS/SCI2 code ************
//*
//
void GPS_init(void){
SCI2_init();
gGPSstate=GPSWAIT;
gGPSbytecount=0;
gGPSPosition.IsValid=FALSE;
gGPSVelocity.IsValid=FALSE;
LatPtr=(BYTE *)(&gGPSPosition.Lat);
LonPtr=(BYTE *)(&gGPSPosition.Lon);
AltPtr=(BYTE *)(&gGPSPosition.Alt);
gGPSVelocity.ESpeed=0;
gGPSVelocity.NSpeed=0;
gGPSVelocity.USpeed=0;
ESpeedPtr=(BYTE *)(&gGPSVelocity.ESpeed);
NSpeedPtr=(BYTE *)(&gGPSVelocity.NSpeed);
USpeedPtr=(BYTE *)(&gGPSVelocity.USpeed);
gGPSHeadingToTarget=0;
gGPSVelocityAndHeading.Velocity=0;
gGPSVelocityAndHeading.Heading=0;
gGPSVelocityAndHeading.IsValid=FALSE;

24
}
void SCI2_init(void){
SCI2C1_PE =1; // enable parity
SCI2C1_PT =1; // parity is odd
SCI2BD = BAUD9600; // setup the serial port for com with GPS receiver
SCI2C2 = initSCI2C2;
}
void SCI2_Rx_ISR_enable(void){
SCI2C2_RIE=1; // enable rx isr
}
void SCI2_Rx_ISR_disable(void){
SCI2C2_RIE=0; // disable rx isr
}

// inputs are dx and dy
// output is distance between points in meters
float GPS_GetDist(float dx,float dy) {
float dist= sqrtf(dx*dx + dy*dy); //
return dist;
}

// returns the bearing in degrees from point1 to point2
float GPS_GetBearing(float dx,float dy) {
float Heading= atanf(dx/dy)*gRadToDeg; // * need atan? need to be in degrees?
return Heading;
}
// stores the east-west (dx) and north-south (dy) distance in meters between point1
and point2
void GPS_GetDxDy(float *dx, float *dy, float lat1, float lon1, float lat2, float
lon2){
// cartesian approximation to great sphere formula
// cos scales east-west distance to localized distance between longitude lines
*dx= gOneRadDist*(lon2 - lon1)*gCosLat;//* only do cosf(lat1) once to optimize for
speed
*dy= gOneRadDist*(lat2 - lat1);
}

// returns the speed over the ground in m/s
float GPS_GetSpeed(float ESpeed, float NSpeed){ // absolute ground speed m/s
float velocity=sqrtf(NSpeed*NSpeed + ESpeed*ESpeed);// * keep this squared to avoid
doing sqrt?
return velocity;
}

// returns the heading angle of the airplane in degrees
float GPS_GetHeading(float ESpeed, float NSpeed){
// E/N vel vectors -> heading angle from North degrees
float heading=atanf(ESpeed/NSpeed)*gRadToDeg; // * need atan? need to be in
degrees?
//float heading = atan(ESpeed/NSpeed)*gRadToDeg;
return heading;
}


// receive data from GPS, store it in gData
// setup timer isr counter to detect lost packets??
// (clear the timer after each received byte, if the timer reaches
// some large value, reset the GPSstate to start with a new packet)
// ** need to handle DLE or ETX in data section
interrupt Vsci2rx void SCI2RDRFisr(void){
byte c;
if (SCI2S1_RDRF==1) { // read flag
c=SCI2D; // read value here to clear flag
25
switch(gGPSstate){ // wait until the start byte is received
case GPSWAIT:
if (c==DLE)
gGPSstate=GPSGOTFIRST;
else
gGPSstate=GPSWAIT;
break;
case GPSGOTFIRST: // already got start byte, check id byte now
// don't get new data until old data has been used???
if ((c==GPSPOS) /*&& (gGPSPosition.IsValid==false)*/)
gGPSstate=GPSGOTPOS;
else if ( (c==GPSVEL) /*&& (gGPSVelocity.IsValid==false)*/)
gGPSstate=GPSGOTVEL;
//else if (c==0x55)
// gGPSstate=GPSENDPACKET; //GPSGOTCONFIG;
//else if (c==GPSSTATUS)
// gGPSstate = GPSGOTSTATUS;
//else if (c==GPSHEALTH)
// gGPSstate = GPSGOTHEALTH;
//else if (c==GPSDGPSMODE)
// gGPSstate = GPSGOTDGPSMODE;
else
gGPSstate=GPSENDPACKET;
gGPSbytecount=0;
gGPSDLEcount=0;
break;
/*case GPSGOTDGPSMODE:
gGPSstate =GPSENDPACKET;
c++;
break;
case GPSGOTSTATUS:
gGPSbytecount++;
if (gGPSbytecount>=2){
__RESET_WATCHDOG();
c++; // stop here to look at status byte
gGPSstate =GPSENDPACKET;
}
break;
case GPSGOTHEALTH:
gGPSbytecount++;
if (gGPSbytecount>=2){
__RESET_WATCHDOG();
c++; // stop here to look at health byte
gGPSstate =GPSENDPACKET;
}
break; */
case GPSGOTPOS: // record position packet (build floats from bytes, MSB
first)
/*
if (c == DLE){
gGPSDLEcount++;
if (gGPSDLEcount == 1) // this is the first DLE
break; // skip this byte, wait for the next DLE
(data)
}
//*/
if (gGPSbytecount<4){ // get Lat
LatPtr[gGPSbytecount]=c;
} else if (gGPSbytecount<8){ // get Lon
LonPtr[gGPSbytecount-4]= c;
} else if (gGPSbytecount<12){ // get Alt
AltPtr[gGPSbytecount-8]= c;
} else {
26
gGPSPosition.IsValid=TRUE;
gGPSstate=GPSENDPACKET;
}
gGPSbytecount++;
//gGPSDLEcount=0;
break;
case GPSGOTVEL: // record velocity packet (build floats from bytes, MSB
first)
/*
if (c == DLE){
gGPSDLEcount++;
if (gGPSDLEcount == 1) // this is the first DLE
break; // skip this byte, wait for the next DLE
(data)
}
//*/
if (gGPSbytecount<4){ // get East Speed
ESpeedPtr[gGPSbytecount]=c;
} else if (gGPSbytecount<8){ // get North Speed
NSpeedPtr[gGPSbytecount-4]= c;
} else if (gGPSbytecount<12){ // get Up Speed
USpeedPtr[gGPSbytecount-8]= c;
} else {
gGPSVelocity.IsValid=TRUE;
gGPSstate=GPSENDPACKET;
gGPSDLEcount=0;
}
gGPSbytecount++;
//gGPSDLEcount=0;
break;
/*case GPSGOTCONFIG: // record io configuration data packet
gGPSstate=GPSENDPACKET;
break; */
case GPSENDPACKET: // get DLE,ETX to end the packet
//gGPSstate=GPSWAIT;
if (c==DLE){
++gGPSDLEcount;
}else {
//check for odd num of DLE followed by ETX -> end of packet
if ( (c==ETX) && (gGPSDLEcount&0x01==0x01) )
gGPSstate=GPSWAIT; // found the end of this packet, go to wait
state
gGPSDLEcount=0; // re-start count if string of DLEs is broken
}
break;
/*case GPSSTOP: // use this state to park for debugging if needed
gGPSstate=GPSWAIT;
break; */
default:
gGPSstate=GPSWAIT; // something is wrong, start over with next packet
break;
} // switch
} // if RDRF
}

//*/
// ************ end GPS code ************




void main(void) {
unsigned long LocalPressure;
27
BYTE IR[2];
BYTE accel[3];
float LocalGPSPosition[2];
float f_temp;
float LocalGPSVelocity[2];
float dx,dy;
BYTE count=0;

__RESET_WATCHDOG(); /* feed the dog before killing it*/
// kill the dog, enable BKND
SOPT = SOPT_BKGDPE_MASK;

ICG_init();
TIMER1_init();
RF_init();
ALTIMETER_init();
ATD_init();
GPS_init();

if (gDoServos==1){
TIMER2_init();
}

PTADD |= (1<<0)|(1<<1); // setup the LEDs to blink
PTAD |= (1<<0)|(1<<1); // turn both LEDs off
//PTAD |= (1<<0); // turn LED1 off
//PTAD &= ~(1<<1); // turn LED2 on
//PTAD &= ~(1<<0); // turn LED1 on

KB_ISR_enable(); // we are ready to receive altimeter data now
TIMER1_CH1_isr_enable(); // start the timer/ATD interrupts now
SCI2_Rx_ISR_enable(); // enable the GPS isr

EnableInterrupts;



// get x many GPS samples then save one as "home", calculate cos(lat), get baseline
altitude
/*
//while(count < 2){
// if (gGPSPosition.IsValid==TRUE){

while(gGPSPosition.IsValid==FALSE);
// count++;
DisableInterrupts;
gGPSPositionHome.Lat = gGPSPosition.Lat;
gGPSPositionHome.Lon = gGPSPosition.Lon;
gGPSPosition.IsValid=FALSE;
EnableInterrupts;
// }
// }

//gCosLat = cosf( gGPSPositionHome.Lat );
LocalGPSPosition[0] = gGPSPositionHome.Lat;
LocalGPSPosition[1] = gGPSPositionHome.Lon;
SCI1_encode_and_transmit((BYTE*)(LocalGPSPosition),8,RF_LATLON_HOME);

while (gPressureReadyForTx==FALSE);
DisableInterrupts;
gPressureHome = gPressure;
gPressureReadyForTx = FALSE;
EnableInterrupts;
28
while (gPressureReadyForTx==FALSE);
DisableInterrupts;
gPressureHome = gPressure;
gPressureReadyForTx = FALSE;
EnableInterrupts;
gPressureHome >>= 1; // make 16 bits
gPressureHome <<= 2*8; // move LSbytes to MSbytes
SCI1_encode_and_transmit((BYTE*)(&gPressureHome),2,RF_PRESSURE_HOME);
//*/


while(1) {
//PTAD ^= 0xff; // toggle both LEDs
PTAD ^= 2; // toggle D1

// Altimeter
if (gPressureReadyForTx==TRUE){
//PTAD &= ~(1<<0); // turn LED1 on - 8.6ms
DisableInterrupts; // start atomic code
LocalPressure = gPressure; // make a local copy of the pressure value
gPressureReadyForTx = FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the pressure value now
LocalPressure >>= 1; // divide by two for a 16-bit value
LocalPressure <<= 2*8; // move LSbytes to MSbytes
SCI1_encode_and_transmit((BYTE*)(&LocalPressure),2,RF_PRESSURE);
//PTAD |= (1<<0); // turn LED1 off
}

// thermopiles // 4 bytes
if (gIRReadyForTx==TRUE){
DisableInterrupts; // start atomic code
IR[0] = (BYTE)(gIR1>>2); // make a local copy of the IR values
IR[1] = (BYTE)(gIR2>>2);
gIRReadyForTx = FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the IR values now
SCI1_encode_and_transmit((BYTE*)(IR),2,RF_IR);
}

// accelerometer // 5 bytes
if (gXYZReadyForTx==TRUE){
DisableInterrupts; // start atomic code
accel[0] = (BYTE)(gX>>2); // make a local copy of the accel values
accel[1] = (BYTE)(gY>>2);
accel[2] = (BYTE)(gZ>>2);
gXYZReadyForTx = FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the accel values now
SCI1_encode_and_transmit((BYTE*)(accel),3,RF_ACCEL);
}

// GPS - position
if (gGPSPosition.IsValid==TRUE){
//PTAD &= ~1; // turn D2 on
//PTAD ^= 1; // toggle D2
DisableInterrupts; // start atomic code
LocalGPSPosition[0] = gGPSPosition.Lat;// make a local copy of the position
values
LocalGPSPosition[1] = gGPSPosition.Lon;
gGPSPosition.IsValid=FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the position values now
29
SCI1_encode_and_transmit((BYTE*)(LocalGPSPosition),8,RF_LATLON);

/*
GPS_GetDxDy(&dx,&dy,LocalGPSPosition[0],LocalGPSPosition[1],
gGPSPositionHome.Lat,gGPSPositionHome.Lon);
f_temp = GPS_GetDist(dx,dy);
f_temp = GPS_GetBearing(dx,dy);
//*/

//PTAD |= 1; // turn D2 off
}

// GPS - velocity
if (gGPSVelocity.IsValid==TRUE){
PTAD &= ~1; // turn D2 on
//PTAD ^= 1; // toggle D2
DisableInterrupts; // start atomic code
LocalGPSVelocity[0] = gGPSVelocity.ESpeed;// make a local copy of the velocity
LocalGPSVelocity[1] = gGPSVelocity.NSpeed;
gGPSVelocity.IsValid=FALSE;
EnableInterrupts; // end atomic code

// just transmit the velocity vectors for now
SCI1_encode_and_transmit((BYTE*)(LocalGPSVelocity),8,RF_VEL);

/*
// calculate the compass heading and speed of the plane
f_temp = GPS_GetSpeed(
LocalGPSVelocity[0],LocalGPSVelocity[1]);
f_temp = GPS_GetHeading(
LocalGPSVelocity[0],LocalGPSVelocity[1]);
//*/
PTAD |= 1; // turn D2 off
}


} /* loop forever */

}
30
MATLAB CODE

f unct i on var ar gout = Ser i al Gui ( var ar gi n)
%SERI ALGUI M- f i l e f or Ser i al Gui . f i g
% SERI ALGUI , by i t sel f , cr eat es a new SERI ALGUI or r ai ses t he exi st i ng
% si ngl et on*.
%
% H = SERI ALGUI r et ur ns t he handl e t o a new SERI ALGUI or t he handl e t o
% t he exi st i ng si ngl et on*.
%
% SERI ALGUI ( ' Pr oper t y' , ' Val ue' , . . . ) cr eat es a new SERI ALGUI usi ng t he
% gi ven pr oper t y val ue pai r s. Unr ecogni zed pr oper t i es ar e passed vi a
% var ar gi n t o Ser i al Gui _Openi ngFcn. Thi s cal l i ng synt ax pr oduces a
% war ni ng when t her e i s an exi st i ng si ngl et on*.
%
% SERI ALGUI ( ' CALLBACK' ) and SERI ALGUI ( ' CALLBACK' , hObj ect , . . . ) cal l t he
% l ocal f unct i on named CALLBACK i n SERI ALGUI . M wi t h t he gi ven i nput
% ar gument s.
%
% *See GUI Opt i ons on GUI DE' s Tool s menu. Choose " GUI al l ows onl y one
% i nst ance t o r un ( si ngl et on) " .
%
%See al so: GUI DE, GUI DATA, GUI HANDLES

%Edi t t he above t ext t o modi f y t he r esponse t o hel p Ser i al Gui

%Last Modi f i ed by GUI DE v2. 5 12- Mar - 2007 17: 45: 37

%Begi n i ni t i al i zat i on code - DO NOT EDI T
gui _Si ngl et on = 1;
gui _St at e = st r uct ( ' gui _Name' , mf i l ename, . . .
' gui _Si ngl et on' , gui _Si ngl et on, . . .
' gui _Openi ngFcn' , @Ser i al Gui _Openi ngFcn, . . .
' gui _Out put Fcn' , @Ser i al Gui _Out put Fcn, . . .
' gui _Layout Fcn' , [ ] , . . .
' gui _Cal l back' , [ ] ) ;
i f nar gi n && i schar ( var ar gi n{1})
gui _St at e. gui _Cal l back = st r 2f unc( var ar gi n{1}) ;
end

i f nar gout
[ var ar gout {1: nar gout }] = gui _mai nf cn( gui _St at e, var ar gi n{: }) ;
el se
gui _mai nf cn( gui _St at e, var ar gi n{: }) ;
end
%End i ni t i al i zat i on code - DO NOT EDI T


%- - - Execut es j ust bef or e Ser i al Gui i s made vi si bl e.
f unct i on Ser i al Gui _Openi ngFcn( hObj ect , event dat a, handl es, var ar gi n)
%Thi s f unct i on has no out put ar gs, see Out put Fcn.
%hObj ect handl e t o f i gur e
%event dat a r eser ved - t o be def i ned i n a f ut ur e ver si on of MATLAB
%handl es st r uct ur e wi t h handl es and user dat a ( see GUI DATA)
%var ar gi n unr ecogni zed Pr oper t yName/ Pr oper t yVal ue pai r s f r omt he
% command l i ne ( see VARARGI N)

%Choose def aul t command l i ne out put f or Ser i al Gui
handl es. out put = hObj ect ;

%Updat e handl es st r uct ur e
gui dat a( hObj ect , handl es) ;
31

%UI WAI T makes Ser i al Gui wai t f or user r esponse ( see UI RESUME)
%ui wai t ( handl es. f i gur e1) ;




%t hi s r uns when t he st op but t on i s pr essed
f unct i on St opBut t on_Cal l back( hObj ect , event dat a, handl es)
%st op t he ser i al l oop bel ow
set ( handl es. St opBut t on, ' User Dat a' , 1) ;
di sp( ' cl i cked st op' )

%%t hi s r uns when t he st ar t but t on i s pr essed
%f unct i on St ar t But t on_Cal l back( hObj ect , event dat a, handl es)
%set ( handl es. Al t , ' St r i ng' , 1234) ;
%doSer i al =1;


%- - - Out put s f r omt hi s f unct i on ar e r et ur ned t o t he command l i ne.
f unct i on var ar gout = Ser i al Gui _Out put Fcn( hObj ect , event dat a, handl es)
%var ar gout cel l ar r ay f or r et ur ni ng out put ar gs ( see VARARGOUT) ;
%hObj ect handl e t o f i gur e
%event dat a r eser ved - t o be def i ned i n a f ut ur e ver si on of MATLAB
%handl es st r uct ur e wi t h handl es and user dat a ( see GUI DATA)
cl c
set ( handl es. St opBut t on, ' User Dat a' , 0) ;

%%%%%%%%%%%%%%%%%%%%%pr epar e f i l es f or dat a st or age
di r ect or y_name = ui get di r ( ) ;
dat et i me = dat est r ( now) ;

I R_f i l ename = [ di r ect or y_name, ' \ I R_' , dat et i me, ' . csv' ] ;
I R_f i l ename( f i nd( I R_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s f r om
dat e/ t i me
I R_f i l ename
f i d = f open( I R_f i l ename, ' w' ) ;
f cl ose( f i d) ;

XYZ_f i l ename = [ di r ect or y_name, ' \ XYZ_' , dat et i me, ' . csv' ] ;
XYZ_f i l ename( f i nd( XYZ_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s f r om
dat e/ t i me
XYZ_f i l ename
f i d = f open( XYZ_f i l ename, ' w' ) ;
f cl ose( f i d) ;

ALT_f i l ename = [ di r ect or y_name, ' \ ALT_' , dat et i me, ' . csv' ] ;
ALT_f i l ename( f i nd( ALT_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s f r om
dat e/ t i me
ALT_f i l ename
f i d = f open( ALT_f i l ename, ' w' ) ;
f cl ose( f i d) ;

LATLON_f i l ename = [ di r ect or y_name, ' \ LATLON_' , dat et i me, ' . csv' ] ;
LATLON_f i l ename( f i nd( LATLON_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal
char act er s f r omdat e/ t i me
LATLON_f i l ename
f i d = f open( LATLON_f i l ename, ' w' ) ;
f cl ose( f i d) ;

ENVEL_f i l ename = [ di r ect or y_name, ' \ ENVEL_' , dat et i me, ' . csv' ] ;
ENVEL_f i l ename( f i nd( ENVEL_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s
f r omdat e/ t i me
32
ENVEL_f i l ename
f i d = f open( ENVEL_f i l ename, ' w' ) ;
f cl ose( f i d) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

i =0;
whi l e( i <3)
set ( handl es. Count , ' st r i ng' , i )
i =i +1;
pause( 1)
end
%
%pause
%%%%%%% Begi n Ser i al Loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
doSer i al =1;
doSer i al Loop=1;
i f ( doSer i al )
ESC=hex2dec( ' 0A' ) ; %packet I D val ues

I RI D = hex2dec( ' 03' ) ; %%I R1 = 1 byt e, I R2 = 1 byt e
ACCELI D = hex2dec( ' 02' ) ; %%X = 1 byt e, Y = 1 byt e, Z = 1 byt e
ALTI D = hex2dec( ' 01' ) ; %%Al t = 16 bi t i nt eger
%%- - - - - - - - - - - - - - - - i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
HEAD_VELI D = hex2dec( ' 04' ) ; %%vel oci t y = 16 bi t I nt , headi ng = 16 bi t I nt
LAT_LONI D = hex2dec( ' 05' ) ; %%l at t i t ude = 4 byt e f l oat , l ongi t ude = 4 byt e f l oat
BER_DI STI D = hex2dec( ' 06' ) ; %%ber i ng = 16 bi t I nt , di st 2t ar get = 16 bi t I nt
ENVELOCI TY = hex2dec( ' 07' ) ; %East and Nor t h Vel oci t y vect or s, f l oat s
ALT_HOME = hex2dec( ' 08' ) ;
LAT_LON_HOME = hex2dec( ' 09' ) ;
%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

WAI TSTATE=0; %st at es
GOTFI RST=1;
GOTI R=2;
GOTACCEL=3;
GOTALT=4;
%%- - - - - - - - - - - - - - - - - - - i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
GOTHEAD_VEL=5;
GOTLAT_LON=6;
GOTBER_DI ST=7;
GOTENVEL=8;
GOTALT_HOME=9;
GOTLATLON_HOME=10;
%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

st at e=WAI TSTATE;

Ser i al Cl oseAl l ; %cl ose any hangi ng compor t connect i ons

s=Ser i al Connect ( 1, 1200) ; %connect usi ng ( por t , baud)
t i c;
whi l e ( get ( handl es. St opBut t on, ' User Dat a' ) == 0)

swi t ch st at e
case WAI TSTATE
b=Ser i al Recei veByt e( s) ; %get a r aw byt e
i f ( b==ESC)
st at e=GOTFI RST;
end
case GOTFI RST
b=Ser i al Recei veByt e( s) ; %get anot her r aw byt e
i f ( b==I RI D) %r ecei vi ng an I R t her mopi l e packet
33
st at e=GOTI R;
el sei f ( b==ACCELI D) %r ecei vi ng an accel er omet er packet
st at e=GOTACCEL;
el sei f ( b==ALTI D) %r ecei vi ng an al t i t ude packet
st at e=GOTALT;
el sei f ( b==HEAD_VELI D) %r ecei vi ng a Headi ng & Vel oci t y packet
st at e=GOTHEAD_VEL;
el sei f ( b==LAT_LONI D) %r ecei vi ng a Lat t i t ude and Longi t ude packet
st at e=GOTLAT_LON;
el sei f ( b==BER_DI STI D) %r ecei vi ng a Ber i ng and Di st ance packet
st at e=GOTBER_DI ST;
el sei f ( b==ENVELOCI TY) %r ecei vi ng a E N vel oci t y packet
st at e=GOTENVEL;
el sei f ( b==ALT_HOME)
st at e = GOTALT_HOME;
el sei f ( b==LAT_LON_HOME)
st at e = GOTLATLON_HOME;
el se
st at e=WAI TSTATE;
end

case GOTI R %adj ust t hi s t o r emove esc codes i n dat a
st at e=WAI TSTATE;
newf 1=Ser i al Recei veDat aByt e( s, ESC) ; %get I R1
set ( handl es. I R1, ' St r i ng' , newf 1) ;
newf 2=Ser i al Recei veDat aByt e( s, ESC) ; %get I R2
set ( handl es. I R2, ' St r i ng' , newf 2) ;
Ti meSt amp = t oc;
dl mwr i t e( I R_f i l ename, [ newf 1, newf 2, Ti meSt amp] , ' - append' ) ;

case GOTACCEL
st at e=WAI TSTATE;
newf 3=Ser i al Recei veDat aByt e( s, ESC) ; %get X
set ( handl es. Accel X, ' St r i ng' , newf 3) ;
newf 4=Ser i al Recei veDat aByt e( s, ESC) ; %get Y
set ( handl es. Accel Y, ' St r i ng' , newf 4) ;
newf 5=Ser i al Recei veDat aByt e( s, ESC) ; %get Z
set ( handl es. Accel Z, ' St r i ng' , newf 5) ;
Ti meSt amp = t oc;
dl mwr i t e( XYZ_f i l ename, [ newf 3, newf 4, newf 5, Ti meSt amp] , ' - append' ) ;

case GOTALT
st at e=WAI TSTATE;
newf 6=Ser i al Recei ve16bi t I nt ( s, ESC) *2; %get al t i t ude
set ( handl es. Al t , ' St r i ng' , newf 6) ;
Ti meSt amp = t oc;
dl mwr i t e( ALT_f i l ename, [ newf 6, Ti meSt amp] , ' - append' ) ;

case GOTALT_HOME
st at e=WAI TSTATE;
newf 21=Ser i al Recei ve16bi t I nt ( s, ESC) *2; %get home al t i t ude
set ( handl es. Al t _Home, ' St r i ng' , newf 21) ;

case GOTHEAD_VEL %%Tar get headi ng
st at e=WAI TSTATE;
[ newf 7, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. Head, ' St r i ng' , newf 7) ;
el se
set ( handl es. Head, ' St r i ng' , NaN) ;
end
[ newf 8, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
34
set ( handl es. Speed, ' St r i ng' , newf 8) ;
el se
set ( handl es. Speed, ' St r i ng' , NaN) ;
end

case GOTLAT_LON
st at e=WAI TSTATE;
[ newf 9, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 9 = newf 9 * 180 / pi ;
newf 9 = [ f l oor ( newf 9) , ( ( newf 9- f l oor ( newf 9) ) *60) ] ;
set ( handl es. Lat , ' St r i ng' , newf 9) ;
el se
set ( handl es. Lat , ' St r i ng' , NaN) ;
end
[ newf 10, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 10 = newf 10 * 180 / pi ;
newf 10 = [ f l oor ( newf 10) , ( ( newf 10- f l oor ( newf 10) ) *60) ] ;
set ( handl es. Lon, ' St r i ng' , newf 10) ;
el se
set ( handl es. Lon, ' St r i ng' , NaN) ;
end
Ti meSt amp = t oc;
dl mwr i t e( LATLON_f i l ename, [ newf 9, newf 10, Ti meSt amp] , ' -
append' , ' pr eci si on' , ' %. 6f ' ) ;

case GOTLATLON_HOME
st at e=WAI TSTATE;
[ newf 22, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 22 = newf 22 * 180 / pi ;
newf 22 = [ f l oor ( newf 22) , ( ( newf 22- f l oor ( newf 22) ) *60) ] ;
set ( handl es. Lat _Home, ' St r i ng' , newf 22) ;
el se
set ( handl es. Lat _Home, ' St r i ng' , NaN) ;
end
[ newf 23, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 23 = newf 23 * 180 / pi ;
newf 23 = [ f l oor ( newf 23) , ( ( newf 23- f l oor ( newf 23) ) *60) ] ;
set ( handl es. Lon_Home, ' St r i ng' , newf 23) ;
el se
set ( handl es. Lon_Home, ' St r i ng' , NaN) ;
end

case GOTBER_DI ST %
st at e=WAI TSTATE;
[ newf 11, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. THead, ' St r i ng' , newf 11) ;
el se
set ( handl es. THead, ' St r i ng' , NaN) ;
end
[ newf 12, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. Di st , ' St r i ng' , newf 12) ;
el se
set ( handl es. Di st , ' St r i ng' , NaN) ;
end

case GOTENVEL %%E N vel oci t y
st at e=WAI TSTATE;
35
[ newf 13, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. EVel , ' St r i ng' , newf 13) ;
el se
set ( handl es. EVel , ' St r i ng' , NaN) ;
end
[ newf 14, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. NVel , ' St r i ng' , newf 14) ;
el se
set ( handl es. NVel , ' St r i ng' , NaN) ;
end
Ti meSt amp = t oc;
dl mwr i t e( ENVEL_f i l ename, [ newf 13, newf 14, Ti meSt amp] , ' -
append' , ' pr eci si on' , ' %. 6f ' ) ;
%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

ot her wi se
st at e=wai t st at e;

end %swi t ch

set ( handl es. Count , ' st r i ng' , i )
i =i +1;
%Updat e handl es st r uct ur e
gui dat a( hObj ect , handl es) ;
dr awnow;
end %whi l e
Ser i al Cl ose( s)
Ser i al Cl oseAl l
cl ose al l %cl ose t he gui wi ndow
end %doSer i al
%%%%%%%End Ser i al Loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Get def aul t command l i ne out put f r omhandl es st r uct ur e
var ar gout {1} = handl es. out put ;


%- - - I f Enabl e == ' on' , execut es on mouse pr ess i n 5 pi xel bor der .
%- - - Ot her wi se, execut es on mouse pr ess i n 5 pi xel bor der or over St opBut t on.
f unct i on St opBut t on_But t onDownFcn( hObj ect , event dat a, handl es)
%hObj ect handl e t o St opBut t on ( see GCBO)
%event dat a r eser ved - t o be def i ned i n a f ut ur e ver si on of MATLAB
%handl es st r uct ur e wi t h handl es and user dat a ( see GUI DATA)






36
MATLAB GUI