UAV_navegacion y EstabilidadFinalPaper

8/11/2019 UAV_navegacion y EstabilidadFinalPaper

1/44

STABILIZATION AND NAVIGATION CONTROL SYSTEMFOR 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 smallairplane. A custom-designed printed circuit board (PCB) was utilized to interface computers anda host of sensors including a GPS receiver, infrared thermopiles, a three-axis accelerometer, anda barometric altimeter. The aircraft system model was created using Simulink. Data collectedfrom the airplane while it was in flight was used to obtain feedback gain values for stability andoptimal 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.


2/44

1

INTRODUCTIONAutonomous flight is currently a

subject of much discussion, anticipation, andresearch. Many large organizations areinvesting 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 therapidly developing technology and designmethodology surrounding UAVs for the

purpose of developing a fully autonomousaircraft 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 providessuperior stability and easy access to theinterior of the fuselage. The airframe is alsocapable of supporting the extra weight thatis added by auxiliary batteries andelectronics including: a custom designedmicro-computer system, a GPS receiver, andan RF modem. Aircraft stabilization isenabled by using infrared thermopiles.These sensors provide feedback on theattitude angle of the aircraft; this allows thesoftware-based control system tocompensate for disturbances like wind andturbulence. Telemetry information iscontinuously broadcast to a ground station.This data is streamed into a MATLABgraphical user interface (GUI) which bothdisplays the data and records it for futureanalysis. Figures that are referencedthroughout this document have been placedin the Appendix.

PREVIOUS WORKA large amount of research has

previously been performed in the area ofaircraft stabilization and autonomous flight.Many of the methods used in this project are

based on previous work. Research atMonash University has been especially

influential in the direction of this project [2].A method of aircraft stabilization usingthermopiles keyed to sense blackbodyradiation emitted from objects within theatmosphere has been developed at Monash

University. Several variations on this designare described in Horizon Sensing AttitudeStabilisation: A VMC Autopilot [3]. Someuses of this system include stabilizing theaircraft itself (tracking reference pitch androll angles) and stabilizing a platformattached to the aircraft which could be usedfor a stabilized camera mount, etc.

Other previous work has beenreviewed which focuses on applications ofautonomous aircraft, rather than the control

details. Currently, the most obvious anddominant use of UAVs is in the realm ofmilitary operations [4]. Unmanned aerialsurveillance provides greater reconnaissancecapability, precision, and opportunity, whilealso providing significantly reduced costand, most importantly, minimized risk tolife.

In the event of an emergency orother exigent circumstances such as wartimeor natural disaster, wireless communicationrange can be increased by employing anaircraft as a relay node to receive andretransmit information, thus bridging the gap

between the original source and the intendeddestination [2] [5]. Several aircraft can beused in a coordinated manner to yield largeincreases of radio range. This is most

beneficial when the physical communicationinfrastructure is nonfunctional (immediatelyfollowing a hurricane, for example), as wellas in geographic areas where suchinfrastructure is impractical (such as the

battlefield, ocean, or remote wilderness).Autonomous aircraft are also well suited forground-survey applications such as searchand rescue, disaster assessment, battle

planning, etc [6]. The use of severalautonomous but interlinked aircraft has beenresearched for this purpose [7]. Intelligent


3/44

2

computing/network systems have beendeveloped and investigated that would alloweach aircraft to communicate with the othersand, through coordination and intelligentdecision making algorithms, objectives such

as locating a lost person or vehicle can beaccomplished very quickly.

CURRENT WORKThis project does not focus on any

specific application of autonomous aircraft;rather, it deals with the design andimplementation of stabilization andnavigation control systems to control suchan aircraft. It is believed that, once thisgroundwork is laid, future applications

utilizing this system will be easier and fasterto implement, thus increasing the efficiencyand productivity of future research anddevelopment efforts.Stabilization Control System

The first step in creating astabilization control system for an aircraft isto understand and model (as much as

possible) the dynamics of the open loopsystem. Simulink was used to model the rolland pitch dynamics of the aircraft as twoseparate decoupled systems. These modelsare very simple and neglect dynamics ofsome subsystems as well as nonlinearitiesand coupling between the longitudinal andlateral axes.

Figure 1 shows the longitudinalsystem model which approximates the

behavior of the airplane around the pitchaxis. The input, E, is the elevator deflectionangle. K E is the elevator effectiveness andrelates the elevator deflection angle to therate of change of the pitch angle. K L is thelift coefficient and relates the angle of attackto the vertical acceleration of the aircraft.The flight path angle is equal to the ratio ofthe vertical velocity to the airspeed for smallangles. Closing the loop, the angle of attackis then defined as the difference between the

pitch angle and the flight path angle.

Figure 2 shows the lateral systemmodel which corresponds to the roll axis.The input, A, is the aileron deflection angle.K A is the aileron effectiveness, which relatesthe aileron deflection angle to the rate of

change of the roll angle.Two proportional control loops weredesigned and simulated using Simulink forthe roll and pitch angles of the aircraft. Theroll and pitch angle references for theseloops are provided by the outer navigationalcontrol loop. For feedback, thermopilesensors provide the roll and pitch angles ofthe aircraft. Figures 3 and 4 show thesecontrol loops. For simulation purposes, thesensor signal is scaled and given an offset to

mimic the actual data returned by thethermopiles. Proportional control loopswere chosen because of their simplicity, aswell as their ability to be tuned for a desiredstep response. The tuning of gain

parameters for these loops creates a responsethat is fast enough to overcome gusting

breezes, yet damped enough to preventexcess overshoot and oscillations.

Figures 5 and 6 show the stepresponses of the simulated roll and pitchcontrol loops, respectively. The response toa 30 degree roll angle reference input showsa large aileron deflection initially, whichdecays to zero as the roll angle reference istracked. Notice that the heading anglechanges linearly at about 15 degrees persecond. Similarly, the response to a 30degree pitch angle reference input shows alarge elevator deflection initially, eventuallydecaying to zero along with the angle ofattack. The altitude increases linearly atabout 3 meters per second.

Figure 7 shows the step response ofthe actual system while in flight. The initialcondition for this case had the airplane in asteep left bank, while the step input was fora 15 degree right bank. For comparison,Figure 8 shows the response of thesimulated control system to similar initial


4/44

3

conditions and reference input. The ailerondeflection angle saturates at 30 degrees forthis simulation. The response time is about2 seconds for both the simulated andmeasured data.

Navigation Control SystemThe navigation system is designed touse two independent but simultaneous

proportional feedback control loops. Thefirst loop, shown in Figure 9, manipulatesthe bank angle reference to change theheading angle of the aircraft. This loopstarts with a heading angle reference(pointing to the next waypoint) and subtractsthe actual heading of the aircraft, resulting ina heading angle error. This error is

converted to a roll angle reference. Theresult, , is passed to the latitudinalstabilization control loop, which controls theroll angle of the aircraft. These outer-loopcalculations are performed once per secondusing the most recent GPS data.

The second navigation loop, shownin Figure 10, controls the aircraft altitude.Using a pressure based altitude sensor(sampled at 2 Hz), this loop is able to updatemore frequently than the GPS loop. Areference altitude and the actual altitude arecombined to create an altitude error which isthen proportionally scaled and converted toa pitch angle reference, h. This error is then

passed to the longitudinal stabilization loopwhich controls the pitch of the plane.

Hardware DesignSeveral combinations of sensors

were considered for this project. Thecontrol system requires the ability tosomehow measure the attitude angles (pitchand roll) of the aircraft as well as thealtitude, position, and velocity. The sensorsselected for use in this project are discussedin the following section.

A pressure sensor was used tomeasure altitude. Readings from this sensor,received on an SPI bus, were converted toan altitude by using a linear approximation

based on empirical atmospheric data fromtwo sources [8] [9]. Plots of this data areshown in Figure 11. MATLAB was used togenerate a linear fit for the range of 0 to3000m. This linear model (equation 1) was

then used to convert pressure data to analtitude relative to the local pressure andelevation on the ground.

( ) m9668.38+Pressure02.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 ofmeasuring changes in altitude as small as20cm.

Infrared thermopiles (part numberMLX90247, from Melexis) were used tomeasure the pitch and roll angles of theaircraft. These sensors measure blackbodyradiation emitted by objects within theatmosphere and produce an analog voltage.Optical filtering minimizes the effect ofother light sources (including visible light)on the measurements. According to thedevice datasheet, these sensors produce over70% of their output based on wavelengths

between 7.5 m and 13.5 m, whichcorresponds to the expected wavelength of black body radiation in our atmosphere withtemperatures between -53 C and 113 C(found using Wien's displacement law).This selectivity causes the sensor to measureradiation given off by terrestrial objects(such as the ground) and ignore objects fromdeep space (such as the sun). Two pairs ofthermopiles were placed on the aircraftfacing in opposite directions (see Figures 12

and 13). The corresponding circuitryconnects the thermopiles opposite each otherin a differential configuration. Subsequentmeasurements of the resultant voltagecorrespond to the ratio of energy detected bythe skyward looking thermopile to theearthward looking thermopile. The voltagesresulting from these two pairs of thermopiles


5/44

4

are measured and used to detect the pitchand roll angles of the aircraft.

The global positioning system (GPS)receiver (Trimble Lassen-IQ) provides theinformation necessary for the aircraft to

navigate a course consisting of a series ofwaypoints. The GPS module updates once per second, and provides (through a serialinterface) longitude, latitude, and velocity(in east, north, and vertical vectorcomponents). Longitude and latitude data isused to calculate the bearing angle anddistance to the next waypoint. The velocitydata is used to derive the heading and speedof the aircraft.

The microcontroller (mcu) selected

for this project is the 8-bit Freescale partnumber MC9S08GB60. This mcu wasselected for its low power consumption,availability of peripherals (10-bit analog todigital converter, hardware PWM/timergenerator, serial communication ports, etc),20MHz maximum CPU frequency, andgenerous Flash-ROM and RAM memoryresources (necessary for storing a largenumber of GPS waypoints and efficientmathematical routines needed for somecontrol system functionality).

RF link modems (Shenzhen partnumber UM12) operating at 433 MHz,

provide data transmission from the aircraftto the ground. The model used provides amaximum range of 1000 m (line of sight)

based on the optimum orientation of the provided monopole antenna. The data rateof these modems is 1200 baud, which

provides 150 bytes/second of throughput.Sensor data from the aircraft is received by acomputer on the ground via a serialconnection for viewing in a customMATLAB GUI. The received data is alsostored (with time stamps) in commadelimited text files. Real time feedback inaddition to stored data provided aconvenient way to identify the system

behavior, tune the control system parameters, and monitor the system status.System Integration

The integration of all the hardwaresubsystems was achieved efficiently by

using a Printed Circuit Board (PCB) design.The PCB resides in the airplane fuselagenear the front, mounted securely to thewalls. See Figures 14 and 15 for the frontand back view of the PCB design.

The power system is divided intotwo subsystems, each with its own battery.The first battery, a 4.8V 2000mAh NiMH 4cell package, provides power to the servomotors. The second battery (purposelyisolated from the servo motors) is a 6V

2000mAh NiMH 5 cell package that provides power to the PCB. The PCB hastwo onboard voltage regulators (3.3V and5V) which supply power to the computerand subsystems. The average powerconsumed by the PCB is shown in equation

( )( ) 718mW6.3V114mAPower = (2)

Selecting the aircraft control source(computer or human) is a crucial yetrelatively simple aspect of the overall

design. It is here that servo motors areassigned a control source. Reliable andfailsafe operation of this function is anabsolute necessity. A custom analogintegrated circuit (IC) was designed andfabricated to fulfill this need. The ICmonitors the otherwise unused 5 th channel ofthe radio controller. The radio control signalconsists of a sequence of 1 to 2 ms pulses atan 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 thecomputer. When a pulse width greater than1.5 ms is detected, servo control is routedfrom channels 1-4 of the radio controller.This allows for rapid manual takeover of theaircraft in the event of an emergency bysimply toggling the 5 th channel on the radiocontroller. In addition, the IC is constantly


6/44

5

monitoring the ground signal presence. Ifthe plane doesnt receive the 5 th channelsignal from the ground for more than 4cycles (80 ms), servo control is defaulted tothe onboard computer. Normal switching

operation resumes at the rising edge of therestored ground signal.From the design perspective, the IC

recognizes an incoming pulse and producesa positive potential spike at the gate of adissipative switch that drains any previouscharge from the integrating capacitor. This

process takes less than 5 s. Then, theincoming 1 to 2 ms wide pulse is integratedfor the duration of the pulse. Once thefalling edge is detected, the integrated

charge is clamped until the next pulsearrives. The clamped charge is held at thegate of a Schmitt trigger with 600 mV

between thresholds (providing hysteresis) toavoid undesired state changes between

pulses. The final result is a restored signalthat controls a quad pole double throwswitch, allowing either all fourmicrocontroller channels or all four radiocontrol channels to become connected to theservo motors.

Embedded SoftwareWhen writing embedded software for

a safety critical system, there are many risksand advantages. Risks include unexpectedworst case conditions which affect orcompletely halt critical time constrained

behavior (such as controlling the ailerons,throttle, etc). Problems like this can be aresult of stack overflow, memory leaks, orrace conditions. All of these conditions can

be difficult to foresee, debug, and eliminate.This is especially true when using libraryfunctions such as floating point mathoperations. The problem is exasperatedfurther by the limited resources of an 8-bitmicrocontroller like the one used here.Despite these and other risks associated witha software based system, there are also manyadvantages. Embedded software allows for

quick field changes and tuning of control parameters, as well as the possibility ofcomparing multiple software solutions and

picking the best one without the obstacle ofhaving to redesign hardware.

The responsibilities of the computersystem are divided into three categories: 1)GPS and altitude data processing, 2) attitudeangle data processing and aileron, elevator,and throttle signal generation, 3)communication with a ground station via theRF data link.

All of the required softwarefunctionality, with the exception of the RFdata link transmission, is handled in aninterrupt context. Separate interrupt service

routines (ISRs) were designed to manage thevarious external data sources such as GPSand altimeter. These ISRs ensured that thedata was accessed before new data arrivedfrom the external source. A timer ISR wasused to ensure a constant sampling rate of10Hz for the thermopile sensors. Also, toreduce the latency between when ameasurement was taken and when a newoutput was produced, the control loopequations were implemented directly in theappropriate ISR.

Each data transfer from the plane tothe ground station is preceded by a two byteheader. The first byte of the header is anescape byte (0xA0). Whenever this value isseen in the serial data stream, it indicates the

beginning of a new data packet. The second byte is a type identifier which distinguishesthe type of data that follows. For example,header [0xA0 0x01] indicates that newthermopile data has arrived and MATLABthen processes the next two bytes of data asthe state of the roll and pitch thermopile

pairs. In contrast, header [0xA0 0x05] tellsMATLAB to expect two 32 bit floating

point values that describe the velocity of the plane. In the event that the escape characteroccurs naturally in the data beingtransmitted, it is padded with another escape


7/44

6

character. This double escape sequence tellsMATLAB that this is actual data and not the

beginning of a new packet.

RESULTS

The system that was designed andimplemented for this project was highlysuccessful. The stabilization control systemwas able to stabilize the aircraft and rejectdisturbances such as wind and manuallyinduced maneuvers (initial conditions) suchas dives and rolls (see Figure 16). Thenavigation system was able to track the

position and speed of the plane, however thecomputer 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 tocomplete the control system implementation

by devising a solution to the navigationcalculation requirements. Some possibleremedies include using a higher poweredmicroprocessor such as a 32-bit CPU, using

programmable logic such as an FPGA, orusing a floating point coprocessor to offloadthe floating point math from the 8-bitmicrocontroller. In addition to overcomingthe calculation shortcomings, a faster andmore accurate GPS receiver will also beutilized. The next revision of the main PCBfor this project will also be refined andshrunk in size to reduce size and weightaboard the plane. A more effective RFmodem will be used that provides a higherdata throughput and uses a better suitedantenna such as a dipole or patch. Beyond

just improving upon the current iteration ofthis design, efforts will be made to utilizethis functionality to explore some of theUAV applications mentioned previously.

CONCLUSIONThis project involved the design and

implementation of a stabilization and

navigation control system for a small UAV.The design was based around two controlloops, one for the roll axis of the aircraft,and the other for the pitch axis. The aircraftdynamics were simulated using Simulink

before the control systems were designedand implemented. After implementing thestabilization control system, it was testedand shown to effectively stabilize theaircraft as well as reject disturbances such aswind. Additional work will be directedtowards completing the navigation controlsystem as well as applying this overallsystem to some interesting uses such ascoordinated search and wirelesscommunication range augmentation.

ETHICAL AND SOCIETAL ISSUES During the development of this

project, many compromises had to be made.There were choices associated withcomponent selection and the overallfunctionality that would be implemented.Compromises and choices were based on theneed to keep the system cost low while still

providing some minimum amount offunctionality. Some of the functions inquestion relate to the navigation capabilitiesof the plane as well as the multiplexing

between manual and automatic flightcontrol. Choices were made andcomponents were selected that would

provide the most safety to those on theground and in the air, while providing somemeasure of navigation capability as well.

There are many safety issuesassociated with mixing manned andunmanned aircraft in the sky. The mostobvious concern is the risk of injury or lossof life faced by those in the manned aircraft.Balancing these concerns is the need forcapabilities provided by unmanned aircraft(UAVs). Small UAVs provide a variety ofcontributions to society including search andrescue during times of disaster and aerialreconnaissance for military or civilian uses.


8/44

7

The aerial reconnaissance capabilities ofsmall UAVs allow observation of areaswhere manned flight is prohibited becauseof some danger associated with the

battlefield, forest or structural fire,

chemicals, or weather hazards. An examplea situation where UAVs could (and should)have been used is in the New Orleans areafollowing the hurricane Katrina disaster[10]. Requests were made to have UAVsavailable for search and rescue as well asdamage assessment needs. While manyUAVs were provided for this purpose,

because of ambiguity in the regulationsgoverning the use of UAVs and mannedaircraft in the same airspace, the UAVs

where never allowed to take flight.These ethical and regulatoryconflicts need to be resolved. There aremany instances where using a UAV in the

place of a manned aircraft is the preferredchoice. Human lives can be kept fromunnecessary risk and those people who needto be rescued can be found more effectively

by employing small groups of UAVs.

ACKNOWLEDGMENTS

The authors of this work wish toacknowledge the contributions made by thefollowing individuals and organizations:

Professor Marc Bodson (Dept. of ECE,University of Utah, Project Advisor)

Provided direction and technicalguidance throughout the project

Colmek Systems Engineering(www.colmek.com) Providedmonetary sponsorship for the projectwhich allowed for purchases of theaircraft, electronics, and supportequipment

Ute RC Club (www.uterc.org) Providedflight training and invaluableassistance during test flights, withoutwhich, this project would not have

been possible due to the guaranteeddestruction of the aircraft

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

In addition to the above noted individualsand organizations, the following softwareenabled much of this project to becompleted successfully:

Codewarrior IDE (Freescale) - C compilerfor embedded software design

Altium DXP - PCB layout and design

Cadence Suite - Simulation and layout ofmultiplexing IC

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

National Geographic TOPO - GPSnavigation pattern layout and plane tracking.

Servoy - Parts inventory and orderingdatabase design


9/44

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 atMonash 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 WirelessCommunications and Networking Conference , Chicago, Illinois, Sept 2000.

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

[7] Ryan, A., Zennaro, M., Howell, A., Sengupta, and R., Hedrick, J., An Overview ofEmerging 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.


10/44

2

APPENDIX

Figure 1Longitudinal Aircraft Model

Figure 2Lateral Aircraft Model

Heading Angle

AileronDeflection A g

sV

Lateral Aircraft Model

K A

s Roll AngleSensor

ElevatorDeflection E - A of A

Attitude

Sensor

s1

K L

Longitudinal Aircraft Model

s

K E s

1

V

1

h an g

NormalAccelerometer

h Altimeter

Flight Path Angle

Pitch Angle

Sensor

h Vertical Velocity

Angle of Attack


11/44

3

Figure 3Roll Angle Control Loop

Figure 4Pitch Angle Control Loop


12/44

4

Figure 5

Simulated Lateral Closed Loop Step Response

Figure 6Simulated Longitudinal Closed Loop Step Response


13/44

5

Figure 7Measured Closed Loop Step Response

Figure 8Simulated Closed Loop Step Response


14/44

6

Figure 9

Heading Control Loop

Figure 10Altitude Control Loop

LongitudinalStabilization

LateralStabilization


15/44

7

Figure 11

Altitude VS Pressure


16/44

8

Figure 12

Roll Axis Thermopiles

Figure 13Pitch Axis Thermopiles


17/44

9

Figure 14PCB Front

Figure 15PCB Back


18/44

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


19/44

11

EMBEDDED C CODE

/* Autonomous Airplane Control System Aaron Greer & Nick Smith

University of Utah, 2006-2007*/

#include /* for EnableInterrupts macro */#include #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


20/44

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.89mselevator: center=1.53ms up=1.2ms down=1.88msthrottle: closed=1.35ms open=1.84msaileron: 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


21/44

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 positionstruct position gGPSPositionTarget; // hold target positionstruct 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 metersconst float gRadToDeg= 180./_M_PI; // converts Radians to Degreesconst float gDegToRad= _M_PI/180.; // converts Degrees to Radiansconst 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;


22/44

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 codeconst BYTE gDoServos = 1; // turn on and off the servo code

signed short gRollRef = (130


23/44

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, floatlon2);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=50nsecICGC2 = ICGC2_MFD1_MASK;// select FLL engaged external, enable oscillator amplifierICGC1 = 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 conversionsTPM1C1SC = TPM1C1SC_MS1A_MASK;TPM1C1SC_CH1F; // read flag CH1FTPM1C1SC_CH1F = 0; // to clear flag, write zero to CH1F

}

void TIMER1_CH1_isr_enable(void){TPM1C1SC_CH1IE = 1;

}void TIMER1_CH1_isr_disable(void){


24/44

16

TPM1C1SC_CH1IE = 0;}interrupt Vtpm1ch1 void TIMER1_CH1_isr(void){ // start an ATD sequence very 100ms

//PTAD &= ~1; // turn D2 onTPM1C1SC_CH1F; // read flag CH1FTPM1C1SC_CH1F = 0; // to clear flag, write zero to CH1F

// setup the new compare value for another 100msTPM1C1V = 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 1msTPM1C0V = TPM1CNT + TIMER1_1MS; // setup compare value for 1ms worth of tickswhile( TPM1C0SC_CH0F==0); // wait 1ms for CH0F to be setTPM1C0SC_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 ticksTPM2SC = TPM2SC_CLKSA_MASK|TPM2SC_PS1_MASK|TPM2SC_PS0_MASK;TPM2MOD = TWENTYMS; // setup modulo for PWM period of 20ms// setup channels 0-3 to drive servosTPM2C0SC = 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


25/44

17

for (i=0;i


26/44

18

SPI1BR = SPI1BR_SPPR2_MASK|SPI1BR_SPR1_MASK;

// make MOSI and SCK outputsPTEDD |= (1


27/44

19

SPI_put(0x00); // dummy transmit to get data backgALT_STATE = ALT_TX4;

break;case ALT_TX4: // we just txed the first dummy byte and read the upper of two

LSbytesgPressure |= ((unsigned long)(SPI_get()= 2; // divide by 4 to convert to units of PascalsgALT_STATE = ALT_WAIT; // (later: go into a state to calculate new outputs)gPressureReadyForTx = TRUE; // signal to 'main' that a pressure value can be

TXedSPI_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 resistorsKBI1SC = KBI1SC_KBEDG5_MASK|KBI1SC_KBACK_MASK; //rising edge (only) sensitive &

clear int flagKBI1PE = 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 highif ( SPI_ReadyToTx() && ((PTAD&(1


28/44

20

// config chip select pin as output ALTIMETER_deselect(); // de-select the altimeter

PTEDD |= (1


29/44

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


30/44

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 onswitch(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 IR2gIR2 = 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;


31/44

23

case ATD_GET_Z: // save value from ZgZ = 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 XgX = newATDvalue;gATD_state = ATD_GET_Y;

ATD_set_channel(4); // start converting channel 4 - Y break;

case ATD_GET_Y: // save value from YgY = 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;


32/44

24

}void SCI2_init(void){

SCI2C1_PE =1; // enable paritySCI2C1_PT =1; // parity is oddSCI2BD = BAUD9600; // setup the serial port for com with GPS receiverSCI2C2 = 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 metersfloat GPS_GetDist(float dx,float dy) {

float dist= sqrtf(dx*dx + dy*dy); //return dist;

}

// returns the bearing in degrees from point1 to point2float 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 point1and point2void GPS_GetDxDy(float *dx, float *dy, float lat1, float lon1, float lat2, floatlon2){

// 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/sfloat GPS_GetSpeed(float ESpeed, float NSpeed){ // absolute ground speed m/sfloat velocity=sqrtf(NSpeed*NSpeed + ESpeed*ESpeed);// * keep this squared to avoid

doing sqrt?return velocity;

}

// returns the heading angle of the airplane in degreesfloat GPS_GetHeading(float ESpeed, float NSpeed){

// E/N vel vectors -> heading angle from North degreesfloat 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 sectioninterrupt Vsci2rx void SCI2RDRFisr(void){

byte c;if (SCI2S1_RDRF==1) { // read flag

c=SCI2D; // read value here to clear flag


33/44

25

switch(gGPSstate){ // wait until the start byte is receivedcase GPSWAIT:

if (c==DLE)gGPSstate=GPSGOTFIRST;

elsegGPSstate=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 bytegGPSstate =GPSENDPACKET;

}

break;case GPSGOTHEALTH:gGPSbytecount++;if (gGPSbytecount>=2){

__RESET_WATCHDOG();c++; // stop here to look at health bytegGPSstate =GPSENDPACKET;

} break; */

case GPSGOTPOS: // record position packet (build floats from bytes, MSBfirst)

/*if (c == DLE){

gGPSDLEcount++;if (gGPSDLEcount == 1) // this is the first DLE

break; // skip this byte, wait for the next DLE(data)}//*/if (gGPSbytecount


34/44

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


35/44

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


36/44

28

while (gPressureReadyForTx==FALSE);DisableInterrupts;gPressureHome = gPressure;gPressureReadyForTx = FALSE;EnableInterrupts;gPressureHome >>= 1; // make 16 bitsgPressureHome 2); // make a local copy of the accel valuesaccel[1] = (BYTE)(gY>>2);accel[2] = (BYTE)(gZ>>2);gXYZReadyForTx = FALSE;EnableInterrupts; // end atomic code

// encode and transmit the accel values nowSCI1_encode_and_transmit((BYTE*)(accel),3,RF_ACCEL);

}

// GPS - positionif (gGPSPosition.IsValid==TRUE){//PTAD &= ~1; // turn D2 on//PTAD ^= 1; // toggle D2DisableInterrupts; // start atomic codeLocalGPSPosition[0] = gGPSPosition.Lat;// make a local copy of the position

valuesLocalGPSPosition[1] = gGPSPosition.Lon;gGPSPosition.IsValid=FALSE;EnableInterrupts; // end atomic code// encode and transmit the position values now


37/44

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 - velocityif (gGPSVelocity.IsValid==TRUE){

PTAD &= ~1; // turn D2 on//PTAD ^= 1; // toggle D2DisableInterrupts; // start atomic codeLocalGPSVelocity[0] = gGPSVelocity.ESpeed;// make a local copy of the velocityLocalGPSVelocity[1] = gGPSVelocity.NSpeed;gGPSVelocity.IsValid=FALSE;EnableInterrupts; // end atomic code

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

/*// calculate the compass heading and speed of the planef_temp = GPS_GetSpeed(

LocalGPSVelocity[0],LocalGPSVelocity[1]);f_temp = GPS_GetHeading(

LocalGPSVelocity[0],LocalGPSVelocity[1]);//*/PTAD |= 1; // turn D2 off

}

} /* loop forever */

}


38/44

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 eates a new SERI ALGUI or r ai ses t he exi st i ng% s i ngl et on*.%% H = SERI ALGUI r et urns t he handl e t o a new SERI ALGUI or t he handl e t o% t he exi s t i ng s i ngl et on*.%% SERI ALGUI ( ' Pr oper t y' , ' Val ue' , . . . ) creat es a new SERI ALGUI usi ng t he% gi ven pr opert y val ue pai r s. Unr ecogni zed pr opert i es ar e passed vi a% var argi n t o Seri al Gui _Openi ngFcn. Thi s cal l i ng synt ax pr oduces a% warni ng when t here i s an exi st i ng si ngl eton*.%% 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% argument 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 response 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 zati on code - DO NOT EDI Tgui _Si ngl eton = 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' , @Seri al Gui _Openi ngFcn, . . .' gui _Out put Fcn' , @Ser i al Gui _Out put Fcn, . . .' gui _ Layout F cn' , [ ] , . . .' gui _ Cal l back' , [ ] ) ;

i f nargi n && i schar( var argi 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 argout {1: nargout }] = gui _mai nf cn( gui _St ate, var argi n{: }) ;

el segui _mai nf cn( gui _St at e, var ar gi n{: }) ;

end% End i ni t i al i zati 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 Seri al Gui _Openi ngFcn( hObj ect , event dat a, handl es, var argi n)% Thi s f unct i on has no out put args, 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 ure wi t h handl es and user dat a ( see GUI DATA)% var argi n unr ecogni zed Propert yName/ Propert yVal ue pai r s f r om t he% command l i ne ( see VARARGI N)

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

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


39/44

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 gure1) ;

% t hi s r uns when t he st op but t on i s pr essedf unct i on St opBut t on_Cal l back( hObj ect , event data, handl es)% st op t he ser i al l oop bel owset ( handl es. St opBut t on, ' User Dat a' , 1) ;di sp( ' c l i cked s top' )

% % t hi s r uns when t he st art 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 om t hi s f unct i on are ret ur ned t o t he command l i ne.f unct i on var ar gout = Seri al Gui _Out put Fcn( hObj ect , event data, handl es)% var argout cel l arr 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 ure wi t h handl es and user dat a ( see GUI DATA)cl cset ( handl es. St opBut t on, ' User Dat a' , 0) ;

%%%%%%%%%%%%%%%%%%%%% pr epar e f i l es f or dat a st or agedi r ect ory_name = ui getdi r ( ) ;dat et i me = dat est r ( now) ;

I R_f i l ename = [ di r ectory_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 omdate/ t i meI R_f i l enamef i d = f open( I R_f i l ename, ' w' ) ;

f cl os e( 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 omdate/ t i meXYZ_f i l enamef i d = f open( XYZ_f i l ename, ' w' ) ;f cl os e( 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 omdate/ t i meALT_f i l enamef i d = f open( ALT_f i l ename, ' w' ) ;f cl os e( f i d) ;

LATLON_f i l ename = [ di r ect ory_name, ' \ LATLON_' , datet i me, ' . csv' ] ;LATLON_f i l ename(f i nd( LATLON_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; % r emove i l l egalchar act er s f r om dat e/ t i meLATLON_f i l enamef i d = f open( LATLON_f i l ename, ' w' ) ;f cl os e( 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 charact er sf r om dat e/ t i me


40/44

32

ENVEL_f i l enamef i d = f open( ENVEL_f i l ename, ' w' ) ;f cl os e( f i d) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% i =0;

whi l e( i


41/44

33

st at e=GOTI R;el sei f ( b==ACCELI D) % r ecei vi ng an accel eromet 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 packetst at e=GOTLAT_LON;el sei f ( b==BER_DI STI D) % r ecei vi ng a Beri 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=Seri al Recei veDat aByt e( s, ESC) ; % get I R1set ( handl es . I R1, ' St r i ng' , newf 1) ;newf 2=Seri al Recei veDat aByt e( s, ESC) ; % get I R2set ( 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 GOTACCELst at e=WAI TSTATE;newf 3=Seri al Recei veDat aByt e( s, ESC) ; % get Xset ( handl es. Accel X, ' St r i ng' , newf 3) ;newf 4=Seri al Recei veDat aByt e( s, ESC) ; % get Yset ( handl es. Accel Y, ' St r i ng' , newf 4) ;newf 5=Seri 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 GOTALTst at e=WAI TSTATE;newf 6=Ser i al Recei ve16bi t I nt ( s, ESC) *2; % get al t i t udeset ( 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_ HOMEst at e=WAI TSTATE;newf 21=Seri al Recei ve16bi t I nt ( s, ESC) *2; % get home al t i t udeset ( handl es. Al t _Home, ' St r i ng' , newf 21) ;

case GOTHEAD_VEL %% Tar get headi ngst 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 ri ng' , newf 7) ;el se

set ( handl es. Head, ' St ri ng' , NaN) ;end[ newf 8, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;i f ( er r or == 0)


42/44

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_LONst 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 seset ( 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 seset ( 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 s i on' , ' %. 6f ' ) ;

cas e GOTLATLON_HOMEst 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 seset ( 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 seset( handl es. Lon_Home, ' St r i ng' , NaN) ;

end

cas e 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 s t , ' St r i ng' , newf 12) ;el se

set ( handl es . Di s t , ' St r i ng' , NaN) ;end

case GOTENVEL %% E N vel oci t yst at e=WAI TSTATE;


43/44

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 ri ng' , newf 13) ;el se

set ( handl es. EVel , ' St ri 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 ri ng' , newf 14) ;el se

set ( handl es. NVel , ' St ri 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 s i on' , ' %. 6f ' ) ;%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

ot herwi sest at e=wai t st at e;

end % swi t ch

set ( handl es . Count , ' s t r i ng' , i )i =i +1;% Updat e handl es st r uct ur egui dat a( hObj ect , handl es) ;dr awnow;

end % whi l eSer i al Cl ose( s)Ser i al Cl oseAl lcl 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 om handl es st r uct ur evarar gout {1} = handl es. out put ;

% - - - I f Enabl e == ' on' , execut es on mouse pr ess i n 5 pi xel border .% - - - Ot herwi se, execut es on mouse pr ess i n 5 pi xel border 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 ure wi t h handl es and user dat a ( see GUI DATA)


44/44

MATLAB GUI

Documents

UAV_navegacion y EstabilidadFinalPaper