Matlab Ufford Adam Thesis

© All Rights Reserved

17 tayangan

Matlab Ufford Adam Thesis

© All Rights Reserved

- Design and Flight Control of Miniature Air Vehicle
- USARTL-TR-79-22A Aircraft Crash Survival Design Guide
- AIAA-1970-533-510
- Adams 2dof Car Tutorial
- Avenues Math Pack
- GPS Tutorial
- Sem 6 Syllabus
- Airplane Upset Recovery by Boeing (Part 2)(1)
- gfilippini
- Gasdynamics Guidance
- etd
- Multi Variable Sliding Mode Control for AUV Steering and Diving
- Siemens 840D Alarms
- ORION FE Analysis of Foundations
- math literature lesson plan
- JOK_2016SIBILSKIKOWALSKI.docx
- C1 2008 June
- L2V4bGlicmlzL2R0bC9kM18xL2FwYWNoZV9tZWRpYS83MjIx
- SchMeiPap05
- Alg Graphing

Anda di halaman 1dari 99

by

Adam Ufford, B.S.M.E.

A Thesis

In

MECHANICAL ENGINEERING

Submitted to the Graduate Faculty

of Texas Tech University in

Partial Fulfillment of the Requirements for the Degree of

MASTER OF SCIENCE

IN

MECHANICAL ENGINEERING

Approved

Dr. Jordan M. Berg

Committee Chair

Dr. Walt Oler

Dr. Seon Han

Fred Hartmeister

Dean of the Graduate School

August, 2009

Contents

List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . .

2

Coordinate Frames and Nomenclature Preliminaries . . . .

2.1 Navigational Coordinate Frames . . . . . . . . . . . .

2.1.1 Earth Centered Earth Fixed Geoidal System . . .

2.1.2 Earth Centered Earth Fixed Rectangular System

2.1.3 Local Tangent Plane - enu Coordinate System . .

2.2 Aircraft Coordinate Frames . . . . . . . . . . . . . . .

2.2.1 Earth (Inertial) Axes . . . . . . . . . . . . . . . .

2.2.2 Vehicle Navigation Axes . . . . . . . . . . . . . .

2.2.3 Body Axes . . . . . . . . . . . . . . . . . . . . . .

2.2.4 Wind Axes . . . . . . . . . . . . . . . . . . . . . .

2.2.5 Path Axes . . . . . . . . . . . . . . . . . . . . . .

2.3 Nomenclature . . . . . . . . . . . . . . . . . . . . . .

3

Simulation Model . . . . . . . . . . . . . . . . . . . . . . .

3.1 State Equations . . . . . . . . . . . . . . . . . . . . .

3.2 Aerodynamic Forces and Moments . . . . . . . . . . .

3.2.1 Aerodynamic and Thrust Forces . . . . . . . . . .

3.2.2 Aerodynamic Moments . . . . . . . . . . . . . . .

3.2.3 Aerodynamic Coefficients . . . . . . . . . . . . . .

3.3 Inner Loop: Attitude Stabilization . . . . . . . . . . .

3.4 Outputs . . . . . . . . . . . . . . . . . . . . . . . . .

4

Attitude Stabilization System . . . . . . . . . . . . . . . . .

4.1 Infrared Horizon Sensing . . . . . . . . . . . . . . . .

4.2 FMA FS-8 Copilot . . . . . . . . . . . . . . . . . . .

4.3 Arbitrary Pitch and Bank Angle Stabilization . . . .

4.3.1 Calibration . . . . . . . . . . . . . . . . . . . . .

4.3.2 Stabilization . . . . . . . . . . . . . . . . . . . . .

5

Path Following Subsystem . . . . . . . . . . . . . . . . . .

ii

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

iv

v

1

4

4

4

5

6

6

7

7

8

8

9

9

11

12

12

13

14

14

14

15

17

18

19

19

19

20

23

5.1

5.1.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . .

6

Autopilot Subsystem . . . . . . . . . . . . . . . . . . . . . . . .

6.1 Feedback Linearization Autopilot Algorithms . . . . . . . .

6.1.1 Ground Track Heading Hold . . . . . . . . . . . . . . .

6.1.2 Altitude Hold . . . . . . . . . . . . . . . . . . . . . . .

6.1.3 Existence of Solutions and Corresponding Control Laws

6.1.4 State Measurement and Estimation . . . . . . . . . . .

6.1.5 High-Gain Observer for Flight Path Angle . . . . . . .

6.2 Output Feedback Simulations . . . . . . . . . . . . . . . .

6.3 Alternative Autopilot Design and Comparison . . . . . . .

6.3.1 Altitude Hold: PI Controller . . . . . . . . . . . . . . .

6.3.2 Ground Track Heading Hold: P Controller . . . . . . .

6.3.3 Simulation Results . . . . . . . . . . . . . . . . . . . .

7

GNC System Implementation . . . . . . . . . . . . . . . . . . . .

7.1 GNC System Hardware . . . . . . . . . . . . . . . . . . . .

7.2 Radio Modem . . . . . . . . . . . . . . . . . . . . . . . . .

7.3 Remote Pilot Override . . . . . . . . . . . . . . . . . . . .

7.4 Hardware in the Loop Simulations . . . . . . . . . . . . . .

8

Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

A

Trajectory Model . . . . . . . . . . . . . . . . . . . . . . . . . .

B

Simulation Code . . . . . . . . . . . . . . . . . . . . . . . . . . .

C

C Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

iii

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

24

24

27

27

27

28

29

30

31

33

33

35

35

35

40

40

41

42

43

45

46

48

52

73

List of Tables

2.1

2.2

WGS-84 Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . .

Model Nomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . .

5

10

5.1

25

7.1

43

iv

List of Figures

1.1

1.2

Subsystem Partitioning . . . . . . . . . . . . . . . . . . . . . . . . . .

2

2

2.1

2.2

Aircraft Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . .

5

7

3.1

3.2

Simulation Results: PID Inner Loops . . . . . . . . . . . . . . . . . .

15

15

4.1

4.2

4.3

4.4

Copilot System: Single Axis . . . . . . . .

Voltage-Inclination Curve, Vmax =3.0 Volts

Modified Co-pilot System: Single Axis . .

.

.

.

.

19

20

21

21

5.1

5.2

Waypoint Switching Look-Ahead, s . . . . . . . . . . . . . . . . . . .

23

26

6.1

6.2

6.3

6.4

6.5

6.6

6.7

6.8

6.9

Angle of Attack Estimation: . . . . . . . . . . . . . . . . . . . . . .

High Gain Observer: = 10, h = 20 . . . . . . . . . . . . . . . .

Output Feedback Controller: a =0.25, k1 =0.18, k2 =0.7 . . . . . . .

Proportional Ground Track Heading Hold Autopilot, 15o Step Input

Proportional Ground Track Heading Hold Autopilot, P = 1.5 . . .

Proportional Ground Track Heading Hold Autopilot, P = 0.25 . .

Altitude Hold: Tuned for 3 Second Rise Time . . . . . . . . . . . .

Altitude Hold: (t) = sin t . . . . . . . . . . . . . . . . . . . . . . .

.

.

.

.

.

.

.

.

.

31

32

34

34

36

37

37

38

38

7.1

7.2

7.3

7.4

DAC/Op-Amp Implementation: Single Channel

Telemetry Sentence Structure . . . . . . . . . .

Hardware in the Loop Simulation Results . . . .

.

.

.

.

41

42

42

44

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

.

Chapter 1

Introduction

Interest in unmanned and autonomous aerial vehicles in the Mechanical Engineering department at Texas Tech University began in 2006 with a team of undergraduate mechanical engineering students who constructed and flew an RC-sized electric,

flying wing aircraft via a standard RC remote pilot link. The following year, two

undergraduate mechanical engineering teams were tasked with implementing video

payload, autonomous navigation, and telemetry systems in hopes of entering an international university competition for undergraduate students. The system was desired

to autonomously follow a three dimensional path specified by a 2-D waypoints and a

desired altitude. The undergraduate teams purchased hardware and began algorithm

development with a simplified model. The scope of the project, however, proved too

broad for such a short term assignment, and the project ended with few deliverables

complete and limited success. This thesis details the development and implementation of the guidance, navigation, and control system components for an autonomous

system which satisfies the autonomous navigation requirements.

Guidance, Navigation and Control (GNC) system design can be naturally divided

(both conceptually and mathematically) in to several tractable sub-problems, the

solutions of which act together in a larger, nested structure. The output of the

higher level functions provides the input for the lower stages. A good example of this

layered architecture and its benefits are given in [9]. A autonomous GNC system

in this thesis is defined to be the system of systems which enables the vehicle to

follow an externally specified path and maintain altitude. This task is summarized

graphically in Figure 1.1. The available controls (A , E , R ) are the aileron, elevator

and rudder control surface deflections, respectively. In general, the GNC system

mission is to drive these controls in order to track the desired altitude and path.

Methods of autonomous path path planning, along with other higher-level functions

such as automatic target selection and cooperative formations are not considered

here, although [9] describes how the nested design structure is expandable for such

functions. We choose to partition the autonomous GNC system design into three

smaller systems as shown in Figure 1.2.

Given a group of ordered waypoints and the current position of the aircraft, the

Path Following Subsystem generates a desired ground track heading angle which

will drive the aircraft to the desired path. This desired ground track and a desired

altitude are inputs to the Autopilot Subsystem which generates pitch and bank angle

commands. The bank and pitch angle commands are in turn fed into the Attitude

Stabilization Subsystem which drives the aircraft control surfaces to track the bank

and pitch angle commands. Each level of the system is designed assuming perfect

performance (either instantaneous actuation or first order error dynamics) of the

other levels.

The necessary definitions and additional nomenclature for discussion of the aircraft dynamics are covered in Chapter 2. The nonlinear, 6-DOF aircraft model

developed and used for simulation is detailed in Chapter 3 to illustrate the complexities inherent in control system design and motivate the following chapters. The

selected solution for the Path Following Subsystem is described in Chapter 4, while

the selected design for the Attitude Stabilization Subsystem is described in Chapter

2

5. The main theoretical contribution of this thesis is given in Chapter 6, where original algorithms for the ground-track heading and altitude hold autopilots are derived

and compared via simulation to more traditional autopilot designs. The hardware

and software implementations of the selected algorithms are described in Chapter 7.

A brief conclusion and suggestions for future work are included in Chapter 8.

Chapter 2

Coordinate Frames and

Nomenclature Preliminaries

An accurate discussion of aircraft navigation and dynamics involves several different coordinate systems and many variables. The coordinate systems and symbols

used throughout this thesis are defined in this chapter.

2.1

source of this information consists of an array of satellites which orbit the earth, the

data is reported with respect to a Earth centered, non rectangular coordinate system.

This section defines that coordinate system and its relation to the local rectangular

system used for navigation. For more information on the navigational coordinate

frames, see [5, 4].

2.1.1

The three dimensional position data is reported by the GPS receiver in an Earth

Centered Earth Fixed, Geodetic (ECEF-g) coordinate system as latitude, longitude

and altitude above Mean Sea Level (MSL) (, , hM SL ) values. Application of

the ECEF-g requires a model for the Earths ellipsoid. The model used in GPS

measurements is the WGS-84 model, and its experimentally determined parameters

are listed in Table 2.1. The other quantities necessary to transform the ECEF-g

coordinates into the other coordinate systems can then be calculated. The ellipsoid

flatness, f , and eccentricity, e, are expressed in terms of the axis lengths.

f=

ab

a

= 3.3528107 103

e=

f (2 f )

4

(2.1)

Symbol

Quantity

Value

Units

a

Earth Semimajor Axis 6378137.0

m

b

Earth Semiminor Axis 6356752.3142 m

The normal distance, N , is the distance from the ellipsoid surface as shown in Figure

2.1 to the ellipsoid z-axis and is calculated as a function of latitude.

N () = q

2.1.2

a

1 e2 sin2 ()

(2.2)

(ECEF-r) system. This system is an orthogonal right-handed (ORH) axes system

whose origin sits at the Earth center. The ECEF-r z-axis points up through the North

pole, with the x-axis perpendicular to z and passing through the Prime Meridian and

SL

the y-axis directed to complete the ORH system. For a point P = (P , P , hM

),

P

the ECEF-r coordinates of P are given by

SL

xP = hM

+ N (P ) cos P cos P

P

SL

yP = hM

+ N (P ) cos P sin P

P

SL

zP = hM

+ (1 e2 ) N (P ) sin P

P

(2.3)

2.1.3

The Local Tangent Plane (LTP) is a plane parallel to the ellipsoid tangent at an

arbitrary reference point, x0 . The enu Coordinate System is centered at this point,

which is usually taken to be the origin of the flight. The 1-axis, denoted by e, points

in the true East direction, and the 2-axis, denoted by n, points in the true North

direction. The e n plane lies in the local tangent plane and the 3-axis, denoted by

u, points straight up, perpendicular to the LTP. The origin of the enu Coordinate

System can be expressed in ECEF-r coordinates.

xo =

x0

y0

z0

(2.4)

ECEF-r coordinates are given by

SL

+ N (P ) cos P cos P

xP = hM

P

SL

yP = hM

+ N (P ) cos P sin P

P

zP =

SL

hM

P

(2.5)

+ (1 e ) N (P ) sin P

and the point can be expressed in enu coordinates via one translation and two

rotations.

The coordinates of P in the local tangent plane (eP ,nP ,uP ) can then be given by

one translation and two rotations:

2.2

eP

nP

uP

1

0

=

0 sin P

0 cos P

0

sin P

cos P

cos P

sin P

0

cos P

sin P

0

0 xP x0

0

y y0

P

1

zP z0

(2.6)

on the aircraft, are defined in various directions to ease the derivation of aircraft

equations of motion. For instance, the position of the aircrafts center of mass is

generally stated with respect to the origin of some inertial axes, the aerodynamic

forces are most easily described using the so-called wind axes, and on-board mea6

surements are easily referenced to the body axes directions. This section details the

pertinent coordinate axes systems for the analysis in the following chapters. For a

given frame, Q, we denote unit vectors in the directions of the xq , yq , and zq axes

as iq , jq , and kq , respectively. The five aircraft axes systems defined in this chapter

and used throughout this thesis are depicted graphically, along with the translations

or rotations that separate them, in Figure 2.2. For a more detailed derivation and

explanation of the aircraft coordinate systems, see [12, 3].

2.2.1

The Earth axes frame, denoted E, is formed by pure rotation from the LTP, in

order to invert the z-axis. The unit vectors for this system are denoted (ie , je , ke )

and point in the true North, true East, and down directions, respectively. The Earth

frame is assumed to be an inertial frame. Consequently, the Earth axes frame will

often be referred to as simply the inertial frame.

2.2.2

system whose origin is located at the aircraft center of gravity. This frame is not

an inertial frame, since the origin translates with the aircraft center of gravity. The

7

coordinate axes of the vehicle navigation frame, however, remain aligned with the

inertial ie , je and ke directions, regardless of the vehicles orientation. The vehicle

navigation frame will also be referred to as simply the navigation frame.

2.2.3

Body Axes

The body frame is again an ORH system whose origin is located at the aircraft

c.g. Its coordinate axes, however, are defined as follows. The body frame 1-axis, xb ,

points directly out the nose of the aircraft. The 2-axis, yb , points out the right wing,

and the 3-axis, zb , completes the right-handed, orthogonal system by pointing out

the belly of the aircraft. As the body rotates, so do the body frame coordinate axes

- the xb zb plane remains in the plane of symmetry of the airframe.

The body frame axes can be obtained from the navigation frame axes by a 3-2-1,

body-fixed rotation which defines the Body Euler Angles, , , and .

ie

ie

ib

cc

sc

s

cc + sss cs

je [Re2b ] je (2.7)

jb = sc + css

ke

ke

kb

ss + csc cs + ssc cc

Inside the rotation matrix, s denotes sin () and c denotes cos (). The former

compact notation will be used throughout the remaining pages unless the latter is

necessary for clarity. Given a vector expressed in Y coordinates, the rotation matrix

that transforms this vector into X axes coordinates will be written as Ry2x , as seen

in Equation 2.7.

2.2.4

Wind Axes

The wind frame is an ORH system whose origin is again located at the aircraft

c.g. The xw axis points in the direction of the relative velocity vector with respect

to the surrounding airmass. This relative velocity vector is denoted as Vcm/w . The

wind axes are obtained from the body axes by two body-fixed rotations. The first

is a positive 3-rotation by , and the second is a negative 2-rotation by . This

iw cc

= cs

j

w

s

kw

2.2.5

s

c

0

ib

ib

sc

ss

jb [Rb2w ] jb

kb

kb

c

(2.8)

Path Axes

The path axes frame is an ORH system whose origin is located at the aircrafts

c.g. The xp axis points in the direction of the aircrafts inertial velocity vector, Vcm/e .

The path axes can be obtained from the inertial axes by two successive body-fixed

rotations. The first is a 3-rotation by the instantaneous flight path heading angle, ,

and the second is a 2-rotation by the instantaneous flight path angle, . These two

rotations are expressed mathematically by the following rotation matrix.

ip cc

jp = s

kp

cs

sc

c

ss

s

n

0 e [Re2p ] e

d

d

c

(2.9)

The path axes frame is useful in describing the inertial velocity of the vehicle,

while the wind axes frame allows for easy quantification of the vehicles relative

velocity (with respect to the airmass). The velocity vector of the airmass with

respect to inertial is denoted Vw/e and is nonzero, in general. The following general

vector equation is a consequence of the definitions so far in this chapter.

Vcm/e = Vcm/w + Vw/e

2.3

(2.10)

Nomenclature

The 6-DOF simulation model, detailed in the following chapter, contains all the

quantities relevant to a discussion of aircraft dynamics. These quantities and their

symbols are summarized in Table 2.2. The same symbols are used in the discussion

of all algorithms and the trajectory model in Appendix A.

Symbol

Quantity

X

Aircraft COM Position Component (meters North)

Y

Aircraft COM Position Component (meters East)

Z

Aircraft COM Position Component (meters Down)

P

Angular Rate about body x-axis (rad/s)

Q

Angular Rate about body y-axis (rad/s)

R

Angular Rate about body z-axis (rad/s)

U

Inertial Velocity along body x-axis (m/s)

V

Inertial Velocity along body y-axis (m/s)

W

Inertial Velocity along body z-axis (m/s)

Wx

Wind Component in North Direction (m/s)

Wy

Wind Component in East Direction (m/s)

Wz

Wind Component in Down Direction (m/s)

U0

Relative Velocity along body x-axis (m/s)

V0

Relative Velocity along body y-axis (m/s)

0

W

Relative Velocity along body z-axis (m/s)

VT

Magnitude of Relative Velocity (m/s)

Vin

Magnitude of Inertial Velocity (m/s)

T

Thrust Force (Newtons)

a

Aileron Deflection (radians)

e

Elevator Deflection (radians)

r

Rudder Deflection (radians)

Latitude

Longitude

hM SL

Altitude above Mean Sea Level

10

Comments

State

State

State

State

State

State

State

State

State

State

State

State

State

State

State

Output

Output

Output

Output

Output

Output

Output

Output

Output

Input

Input

Input

Input

GPS output

GPS output

GPS output

Chapter 3

Simulation Model

This chapter details the simulation model used to test and validate the GNC

algorithms derived in later chapters. The details of the model development are

included to better qualify the simulation results given in the following chapters and

to introduce the aircraft nomenclature in a more complete fashion. It is also hoped

that this chapter will serve as a brief tutorial for future users of the simulation model

code.

For simulations, a flat-earth, nonlinear 6-DOF aircraft model is used. This model

was implemented in MATLAB following [12] and includes the often neglected terms

for wind disturbances, sideslip and sideforce. For simplicity, this model uses the

standard 3-2-1, yaw-pitch-roll Euler angle sequence for vehicle attitude representation. It is well known that the kinematic differential equation for such an attitude

parameterization exhibits a singularity at some points. The singularity for a 3-2-1

sequence occurs as the pitch angle, , approaches 2 , as can be seen from Equation

3.2. However, vehicle attitudes in which the nose points straight up or straight down

are outside the space of desired attitudes for most path following problems. Large

positive pitch angles can have the tendency to stall the aircraft. Also, large angles

in pitch and roll can cause problems for the selected attitude measurement and stabilization system[14]. For these reasons, the commanded pitch and bank angles will

be confined to a reasonable range well within the limits imposed by this singularity.

The three Euler angles for the body axes, the position of the aircraft in a Local

Tangent Plane, the body rates, and the inertial body velocities form the 12 basic

states in 6-DOF state-space model. Also included as (unchanging) states are the

wind velocity components in the inertial axis directions (WN , WE , and WD ). The

inputs to the simulation model are a thrust force, T, and control surface deflections

for the ailerons, elevator, and rudder (a , e , and r , respectively). Other quantities

of interest (outputs) are calculated as function of the states after integration. These

include the relative body velocities with respect to the airmass (U 0 , V 0 , and W 0 ),

11

aerodynamic angles ( and ), and flight path angles ( and ). The nomenclature

used in the model and within the remainder of this paper is summarized in Table

2.2.

3.1

State Equations

cc

sc

s 1 U

X

Y = sc + css

cc + sss cs

V

W

ss + csc cs + ssc cc

Z

(3.1)

P

= 0

cos

sin Q

sin

cos

0

R

cos

cos

(3.2)

2

Jxz (Jx Jy +Jz )

P Q Jz (Jz Jy )+Jxz QR + Jz l + Jxz n

x)

Q = (JzJJ

P R JJxzy (P 2 R2 ) + J1y m

y

2

(Jx Jy )Jx +Jxz

y +Jz )

P Q Jxz (Jx J

QR + Jxz l + Jx n

(3.3)

P =

R =

U

T

D

0

U

i

h

1

= (1/m) 0 + [Rb2w ] C + [Re2b ] 0 B/E V

W

0

L

g

(3.4)

The cross-product matrix, B/E , corresponds to the body fixed angular velocity

vector, B/E = [P Q R]T , and is shown below.

3.2

B/E

0

R Q

= R

0 P

Q P

0

(3.5)

Given initial conditions on the state, x, the aerodynamic forces and moments

must be calculated to be inserted into the right hand side of the state equations.

x = [X, Y, Z, , , , P, Q, R, U, V, W ]T

12

(3.6)

3.2.1

The aerodynamic forces are, by definition, applied in the wind frame and, consequently, are naturally written in wind axes coordinates:

w

b

FA =

C = [Rb2w ] FA

L

(3.7)

b

1

FA = [Rb2w ] C

(3.8)

The aerodynamic force components are modeled via the component build-up method

[12], with any dependence on neglected. The lift force is approximated as

CL = CL0

L = qSCL

+ CL + CLe e +

c

2VT

CLQ Q

(3.9)

D = qSCD

CD = CD0 +

(CL CL0 )2

ARe

(3.10)

in which the first two terms will typically dominate. A simple model for the sideforce,

C is given by

C = qSCY

CY = CY + CYr r + CYa a +

b

2VT

[CYP P + CYR R]

(3.11)

T = Tib

13

(3.12)

3.2.2

Aerodynamic Moments

The aerodynamic moments are modeled similarly, using the component build-up

method.

l

bCl

(3.13)

MbA [Rb2w ]1

m = [Rb2w ] qS cCm

bCn

n

Rolling Moment

The rolling moment coefficient is modeled as

Cl = Cl + Cla a + Clr r +

b

[ClP P + ClR R]

2VT

(3.14)

Pitching Moment

The pitching moment coefficient is modeled with no dependence on as

Cm = Cm (, Tc ) + Cme e +

i

c h

Cm q Q

2VT

(3.15)

Yawing Moment

Similarly, the yawing moment coefficient is modeled as

Cn = Cn + Cnr r + Cna a

3.2.3

(3.16)

Aerodynamic Coefficients

UAV[6] as summarized in the Aerosim toolset[15], distributed by Unmanned Dynamics.

3.3

In order to mimic the assumed inner loop attitude stabilization and actuation

system, PID controllers are implemented for roll and pitch angle holds in the simulations, as depicted for the roll axis in Figure 3.1. These controllers are tuned for

reasonable settling times as evidenced by simulation for static inputs of desired pitch

14

and roll angles. Typical simulation results for a step response in desired pitch and

roll angles are shown in Figure 3.2.

Desired Roll +

Roll Error

PID Controller

Aileron

Deflection

UAV Dynamics

Actual Roll

Angle

3.4

Outputs

The output quantities are functions of the states and are computed at each

timestep after integration. Given the inertial velocity components in the body axes

directions, the wind velocity components in the inertial axis directions, and the rotation matrix that transforms vectors between the body and inertial axes systems,

15

we can calculate the relative velocity of the vehicle with respect to the surrounding

airmass. U 0 , V 0 and W 0 are the components of the relative velocity in the body axes

directions.

0

U

WN

U

0

V = V [Re2b ] WE

(3.17)

W0

W

WD

The magnitude of the relative velocity, VT , and the dynamic pressure, q, can then

be calculated.

q

VT = (U 0 )2 + (V 0 )2 + (W 0 )2

(3.18)

q = 21 VT2

The aerodynamic and flight path angles can also be calculated from the inertial and

relative velocity components.

0

)

= tan1 ( W

U0

0

= sin1 ( VVT )

Y

= tan1 ( X

)

Z

)

= tan1 (

2 2

X +Y

16

(3.19)

Chapter 4

Attitude Stabilization System

The lowest level of the GNC system is the attitude stabilization system. The attitude stabilization system drives the aileron and elevator control surfaces in order to

stabilize the aircraft orientation at desired bank and pitch angles, as commanded by

the higher level systems. Control for automatic attitude stabilization is a well-known

problem which has been solved numerous times for full-size aircraft and spacecraft.

The main complicating factor for small UAVs, which typically have only a fraction

of the size, weight and power of their full-size aircraft counterparts, is the method of

measuring or estimating the attitude for feedback in the control system design.

A common solution for full-size aircraft attitude estimation involves expensive

and often large inertial measurement units (IMU), which use gyroscopic measurements to estimate the aircrafts orientation with respect to the inertial frame. With

the miniaturization provided by recent advances in MEMS accelerometer and gyro

technologies, similar approaches have been successfully applied to small UAVs. These

solutions, however, involve multiple measurements from relatively expensive hardware and complex nonlinear filtering methods[1, 13].

Another attitude estimation technique of recent interest for small UAVs is horizon sensing via image processing. In this technique, an on-board camera generates

images of the views along the pitch and roll axes (yb and xb ). The images are

then processed digitally to estimate the orientation of the horizon with respect to

the aircraft body axes and the pitch and roll angles can be estimated. Although

methods with reduced computational loads have been proposed, the algorithms and

software/hardware implementations are often complex.

Another method of horizon sensing, however, offers a cheaper, simpler solution

to the attitude estimation problem. Infrared (IR) horizon sensors have been used

successfully in spacecraft attitude control systems for over 40 years and their use

has more recently been extended to small UAVs[14]. The relatively low size, weight

and cost of thermopile sensors, along with the commercially available stabilization

17

a natural choice for a first effort in UAV development. For these reasons, we choose

to modify an existing commercial IR stabilization system for our purposes. It should

be noted that the higher level GNC systems, however, are independent of the chosen

method of attitude stabilization, provided that the performance is acceptable.

4.1

(VMC) with a quick glance out the cockpit by referencing the visible horizon. Digital

image processing allows an automatic control system to accurately mimic this behavior, but at significant computational cost. A simple method of stabilizing an RC-sized

aircraft in straight and level attitude (zero degrees in both pitch and roll) is marketed to beginner RC pilots in the Futaba Pilot Assist FA-2 Copilot devices. These

devices use light-sensitive sensors to estimate the midpoint between the dark ground

and the light sky, but are especially sensitive to clouds or very bright sunshine[14].

Egan and Taylor[2] show that these sensitivities can be minimized by using sensors

with bandwidths in the infrared portion of the spectrum. Since the ground and

sky have different apparent infrared temperatures, an infrared sensor with a large

field of view, say 100o , can be thought of as giving an average temperature of the

sky/ground combination in view of the sensor as depicted in Figure 4.1. Arranged

in a differential pair, two sensors facing 180o apart can be placed along the pitch or

roll axis to output voltages related directly to the inclination of the body about a

perpendicular axis.

Testing by Egan and Taylor[14] showed that the output of a differential pair

approximates a sinusoidal function of the inclination angle for modest deviations

(+/- 45o ) about zero. As expected, the output curve flattens out as each sensors

field of view becomes filled with all sky or all ground. Since we prefer our system

to maintain modest pitch and bank angles, the sinusoidal approximation suits our

systems needs.

18

4.2

Like the Futaba PA-2 Pilot Assist device, the FS-8 Co-pilot, manufactured and

distributed by FMA Direct, is an off-the-shelf device designed to assist beginner-level

RC pilots restore straight and level flight from precarious attitudes. The FS-8 Copilot, however, consists of a standard RC receiver coupled with a PIC microcontroller

and three orthogonal axes of amplified and offset differential thermopiles. The offset

voltage for the FS-8 is 1.6 Volts, but we will refer to it in general as Vof f set . After

proper calibration, the PIC samples the output from the differential pairs along

the pitch and roll axes, then determines the control surface deflections necessary to

drive the differential readings to the neutral offset value. This system is depicted

graphically in Figure 4.2. While the FS-8 Co-pilot is able to stabilize the aircraft at

zero pitch and roll angles, this device is unable to stabilize the aircraft at arbitrary

attitudes, as required by the higher level control loops, without further modification.

4.3

4.3.1

Calibration

In order to modify the commercial FS-8 device for our needs, we employ an

electrical bias technique as suggested by [14]. The Co-pilot device is mounted with

19

the two horizontal sensor axes aligned with the body pitch and roll axes. The third

vertical sensor axis is aligned with the aircraft yaw axis. Immediately following the

short calibration procedure described in the FS-8 Users Manual, the vertical axis

sensor pair is sampled several times and averaged by our flight controller. Since the

aircraft is still in the horizontal attitude dictated by the FS-8 calibration procedure,

the vertical sensors see all ground or all sky and the differential voltage measured

gives the maximum value of the Voltage-Inclination relationship, which we denote

with the symbol Vmax . By assuming that the thermopiles exhibit identical responses

to the same stimuli, we know that differential output will reach the neutral offset

value, Vof f set when the inclination of its axis reaches 0o and 180o . Together with the

Vmax calibration reading at an inclination of 90o , this allows us to fully parameterize

the sinusoidal Voltage-Inclination curve with the relationship in Equation 4.1. This

relationship is shown graphically in Figure 4.3 for Vmax = 3.0 Volts.

VIR = (Vof f set Vmax ) sin(x) + Vof f set

(4.1)

4.3.2

Stabilization

The FS-8 Co-pilot is designed to stabilize the aircraft about the IR reading given

when x=0, or VIR = Vof f set . We desire however, to stabilize about arbitrary values

of x. Rather than feeding the direct differential voltage VIR to the FS-8 controller,

we utilize digital to analog converters (DACs) and an additonal summing op-amp.

One axis of the modified Co-pilot system is shown in Figure 4.4. By adding (or

20

subtracting) some bias voltage, VDAC , to the differential IR voltage VIR we aim to

trick the FS-8 controller into thinking it is flying level when, in fact, it is flying at

some arbitrarily selected inclination angle. The voltage at the end of the modified

op-amp scheme, which is fed into the FS-8 is denoted VF S8 , and its value is given by

Equation 4.2.

VF S8 = VIR + VDAC

(4.2)

Given a desired inclination angle, xd , we first calculate the output of the differ-

21

VIR,d = (Vof f set Vmax ) sin(xd ) + Vof f set

(4.3)

Knowing that we wish VF S8 to be equal to Vof f set at xd , we then solve Equations 4.2

and 4.3 for the desired VDAC .

VDAC,d = Vof f set VIR,d = (Vmax Vof f set ) sin(xd )

22

(4.4)

Chapter 5

Path Following Subsystem

Given a set of ordered waypoints specifying a desired flight path, a higher level

algorithm is required to generate desired flight trajectories to drive the vehicle to its

path. This chapter details the Path Following Subsystem design, which solves this

guidance problem.

Many useful paths can be formed by using linear segments alone. Search patterns, rectangular perimeter orbits, and simple point-to-point paths are some of the

simplest examples, as shown in Figure 5.1. More advanced examples of linear paths

might include following streets in urban neighborhoods. The paths considered in

this thesis are limited to those which can be constructed by linear segments. These

paths are defined by an ordered series of two-dimensional (X,Y) waypoints in a local

tangent plane coordinate system. Our system also considers an altitude hold system independently of the lateral guidance algorithms, rather than following linear

trajectories in three dimensions.

With the path specified by an array of ordered waypoints, there are two general

approaches to the 2-D tracking problem. The first approach, commonly referred to

as trajectory tracking, drives the vehicle to follow a reference point which travels

along the path at some velocity. This implicitly requires that the vehicle be in a

certain position at a certain time, which can cause problems for small UAVs, where

wind speeds can approach 50 or 60 percent of the aircrafts airspeed. For example,

23

if the aircraft is traveling into the wind with constant thrust or constant airspeed,

and the reference points velocity is not reduced accordingly, the position error for

the trajectory tracking system will continually increase.

The second approach, which is used in this system, is a path following approach.

The objective in path following is that the vehicle be on the path, rather than in a

certain place at a certain time. The path following algorithms used are adapted from

the work in [11].

5.1

The path following algorithms used in this system are based on a vector field construction that represents the desired ground track heading of the aircraft. Given the

current position and the waypoint path, both specified in the planar coordinates, the

vector field is essentially painted on the ground around the path. This field specifies

the desired ground track headings as a function of vehicle position, groundspeed, and

ground track. By directly specifying a desired ground-track heading (rather than a

body-fixed heading), the algorithms in this subsystem guarantee path convergence in

the presence of constant wind disturbances, provided that the aircraft can maintain

a positive groundspeed. The algorithms for the vector field construction for linear

paths are summarized here.

5.1.1

Algorithm

A list of variables used for the vector field path following algorithm is given in

Table 5.1. The design parameters, , e , k, and s are tunable, and change the

convergence characteristics of the cross-track error.

First, the course between the two currently selected waypoints is calculated.

f

= tan

w2Y w1Y

w2X w1X

(5.1)

Then the position of the aircraft in the inertial coordinates is transformed into coordinates along and orthogonal to the path (s ,) and the side of the path which the

24

Symbol

i

f

s

s

wi , wiX , wiY

z = (X, Y )T

c

e

k

Vg

Description

Current waypoint index (initialized to 0)

Heading from waypoint i to waypoint i + 1

Aircraft progress along path, s [0,1]

Lateral tracking error

Waypoint switching look-ahead distance (0 < s 1)

Transition region boundary distance

ith waypoint, its north and east components

The side of the path the aircraft is on, having a value of 1

Current location of the aircraft

Commanded heading

Entry heading angle (0 < e < 2 )

Transition gain, k > 1

Current aircraft groundspeed

Current aircraft ground-track heading

(zw1 )T (w2 w1 )

|w2 w1 |2

(s (w2 w1 ) +

s =

= kz

w1 )k

= sign [(w2 w1 ) (z w1 )]

(5.2)

Once the position of the aircraft relative to the current waypoints (w1 , w2 ) is determined, we must determine whether it is time to switch to the next set of waypoints.

If s > s , then we switch to the next waypoint. w2 becomes w1 and the next waypoint in the ordered array becomes w2 . We use s = 1 in order to drive our aircraft

as close to w2 as possible, but smaller values can be used to switch waypoints sooner

and reduce overshoot in tight turns, as shown in Figure 5.2. The cross-track error,

, is then given a signed value.

=

(5.3)

If the magnitude of the cross track error is greater than the threshold value (|| > ),

then the commanded heading to the autopilot is set using the entry angle.

c = f e

25

(5.4)

Otherwise, an intermediate vector field heading, d , is calculated and the heading

commanded to the autopilot, c , is compensated for the instantaneous groundspeed

and ground track heading by Equation 5.6.

d = f (e )

k

ke Vg k1

sin

=

k

(5.5)

(5.6)

This algorithm is repeated each time a new position measurement is acquired and

the commanded ground-track heading, given either by Equation 5.4 or 5.6 is passed

on to the autopilot subsystem, which is described in the next chapter.

26

Chapter 6

Autopilot Subsystem

With the commanded ground-track heading angle given by the subsystem described in Chapter 5, a desired altitude specified, and the ability to actuate pitch

and bank angles via the subsystem described in Chapter 4, we then require algorithms which map the desired ground-track and altitude quantities into pitch and

bank commands. This task is accomplished with the autopilot subsystem described

in this chapter. The autopilot system is further subdivided into two controllers: a

ground track heading-hold autopilot which commands to drive , and an altitude

hold autopilot which commands to drive h.

When first considered with a simple model, the autopilot controller problems

lent themselves naturally to a feedback linearization design process. The model naturally took an affine-in-control structure with globally invertible nonlinearities and

a feedback linearization control law was found to be quite successful in early simulations. Similar controllers were derived based on the full 6-DOF model and found to

maintain the performance seen in the simpler model. These feedback linearization

controllers were then compared to a linear autopilot formulation and it was found

that comparable performance can be attained in the full GNC system with simpler,

easier to implement P and PI autopilot algorithms. This chapter details the derivation of the feedback linearization autopilot algorithms, the formulation of the linear

autopilot, and simulation results comparing performance of both.

6.1

6.1.1

Ground Track Heading Hold

coupling between states and a non-intuitive mapping between inputs and outputs.

While linearization and decoupling is certainly a design option, we instead choose

27

form we call the Trajectory Model. The derivation of these equations is covered

in Appendix A, and the final results given there depend only on an assumption of

zero sideslip. Another simplifying assumption can be made if the wind speeds are

assumed to be low, as compared to the inertial velocity of the aircraft. Under these

two assumptions, then , and the dynamics from Equation A.14 simplify

greatly.

=

L

sin

mVin cos

(6.1)

Intuitively, we wish to determine the necessary control, , which will exponentially drive the vehicle to the desired ground track angle, d , as commanded by the

path tracking algorithm. That is, we desire:

(t) = d exp(a t)

(6.2)

for some positive time constant, a . This is equivalent to desiring that the dynamics

take the form

= a ( d )

(6.3)

Simply equating the desired in Equation 6.3 to the actual from Equation 6.1 yields

a trigonometric equation in .

L

sin = a ( d )

mVin cos

6.1.2

(6.4)

Altitude Hold

subsystem. The control selected to maintain altitude during maneuvers and make

small altitude changes is the commanded pitch angle, d . We again choose a nonlinear

approach based on the trajectory model and a feedback linearization technique to

design the longitudinal controller.

We begin by defining the output we wish to stabilize. Let hd be the desired

altitude.

z1 = h hd

(6.5)

We differentiate, assuming that the desired altitude hd is constant. The aircraft

28

is given via geometry of the flight path trajectory in the vertical plane.

climb rate, h,

z1 = Vin sin

(6.6)

Since the control has not appeared, we differentiate again using the chain rule and

substituting from Equation A.12.

z1 = V in sin + V

in cos

= (V in ) sin + (Vin )(

cos )

(LfLx +DfDx +T fTx mg sin ) sin +(LfLz +DfDz +T fTz +mg cos )( cos )

=

m

(6.7)

Substitution from Equation A.13 allows for several cancellations and yields

z1 =

T Dc + Ls

Ds + Lc

sin +

cos cos g

m

m

(6.8)

We wish to use the control to reshape the z1 dynamics into a linear system,

z1 = z2

(6.9)

z2 = k1 z1 k2 z2 v(z1 , z2 )

(6.10)

solve the nonlinear equation

Ds + Lc

T Dc + Ls

sin +

cos cos g = v(z1 , z1 )

(6.11)

m

m

for . The autopilot task is then to simultaneously solve (or approximate a solution

to) equations (6.4) and (6.11).

6.1.3

The system of equations (6.4, 6.11) provided the available aerodynamic force

maintains a certain balance with the desired response characteristics. Mathematically, this condition is described by the following inequalities:

q

mV Lcos

in

|a ( d )|

29

(6.12)

This shows mathematically that the desired speed of the altitude response must be

balanced by the available aerodynamic forces. The direct algebraic solution, given

by (6.13) is used when equations (6.12) are satisfied for a complete inversion.

1

= sin

m(v+g)

(T Dc+Ls)2 +(Ds+Lc)2 c2

sin1 a (dL)mVin cos

(6.13)

0 = sin1

(Ds+Lc)c

(T Dc+Ls)2 +((Ds+Lc)c)2

0 ; T Dc + Ls 0

+

; T Dc + Ls < 0

(6.14)

( = 0, = 0).

Regardless of whether the (, ) commands are determined by equations (6.13)

or their linearized equivalents, both commands are passed through a saturation filter

before relay to the attitude stabilization system. The bank commands are saturated

at 45o while the pitch commands are saturated at a more modest 20o .

6.1.4

The lateral and longitudinal control laws given above assume full state feedback.

For implementation, we must either directly measure or estimate the ground-track

heading and vertical flight path angles (, ), the inertial velocity (Vin ), altitude (h),

and the angle of attack (). The instantaneous values of the thrust, lift, and drag

forces must also be estimated.

It is assumed that the thrust force, T , is known. Common static or wind tunnel

testing techniques are available to generate models for the thrust force as a function

of motor speed and airspeed. It is further assumed that the lift and drag forces are

known functions of airspeed and the angle of attack. The quantities which require

further estimation are and . We choose a two-stage estimation scheme in which

an estimate of angle of attack is generated based on the available measurements, and

the estimate of the vertical flight path angle depends on these measurements and the

estimate. This sequenced approach is depicted graphically in Figure 6.1.

30

Angle of Attack Estimation

Consider the relationship between relative and absolute velocities given by Equation 2.10. Writing the ze component of this vector equation, assuming no vertical

wind component (WD =0), we have

h = Vrel (scc cs)

(6.15)

filter to reduce high frequency noise), we can have a direct reading for the climb rate,

Since and are directly measured, we can directly solve Equation 6.15 for .

h.

1

= sin

h

2 2

Vrel s s + s2

!

1

sin

s

2 2

c c + s2

(6.16)

Simulation results for a highly maneuvering aircraft ((t) = 45o sin t , (t) = 12o cos t)

the estimator performance is shown in Figure 6.2. Although the estimator performance is hardly asymptotic, it works acceptably well in the closed loop system as

evidenced by the simulation results that follow.

6.1.5

If we again make the approximation = , the flight path angle and altitude

dynamics simplify.

[Lcs(T D)s]+[Lcc+(T D)smg]

(mVin )

[Lcs(T D)s]

D)smg]

+

(sin ) + [Lcc+(T

(mVin )

(mVin )

31

(cos 1)

(6.17)

(6.18)

15

Actual

Estimated

10

10

15

10

Time (s)

15

20

These dynamics, which have a nearly linear form as the flight path angle approaches

zero, are purposely separated into the linear portions and perturbation terms for

nonzero . For compactness we denote the perturbation terms as x .

=

[Lcs(T D)s]

mVin

D)smg]

(sin ) + [Lcc+(T

(cos 1)

mVin

h = Vin (sin )

(6.19)

In level flight, the flight path angle, , is zero. Since the longitudinal autopilot

uses pitch angle as a control and is designed only to hold altitude and make small

corrections, the resulting flight path angles during normal operation should be small.

Since both the and h terms will approach zero as does, we neglect these terms

formulate the observer as

=

[Lcs+(T D)s]

[Lcc+(T D)smg]

mVin

= Vin + h h h

h

+ h h

(6.20)

(6.21)

x = A

x + Bu

32

(6.22)

with

"

x = ,

h

"

#

1 0

B=

,

0 1

mVin

A=

Vin

h

#

" Lcc+(T D)smg

+ h

mVin

u=

h h

(6.23)

"

e

xe e =

h

hh

(6.24)

e =

[Lcs+(T D)s]

e

mVin

e +

h

e = (V ) h

e +

h

in

h

h

(6.25)

control the convergence of the observer to the true state by driving the poles of the

error dynamics (6.25) further into the left half plane. The high gain observer suffers

from the so-called peaking phenomenon, which is known to cause instability in the

closed loop feedback system. Our control laws, however, include saturation, which

reduces the risk of instability[8]. Simulation results for the high-gain observer are

shown in Figure 6.3.

6.2

The closed loop system uses direct measurements and estimates based on these

measurements to generate the feedback terms. Output feedback simulation results

in response to simultaneous step inputs in both desired altitude and desired ground

track are shown in Figure 6.4.

6.3

In this chapter, more traditional autopilot control laws are presented. While the

autopilot laws derived via feedback linearization work well in simulation, their implementation requires an impressive sensor suite and a two-stage estimation scheme

33

15

Actual

Estimated

10

5

0

5

10

15

10

Time (s)

15

20

Altitude (m)

220

Actual

Estimated

200

180

160

140

10

Time (s)

15

20

40

30

20

10

Desired

Actual

0

0

10

15

20

Time (s)

25

30

35

40

Altitude (m)

215

210

205

Desired

Actual

200

195

10

15

20

Time (s)

25

30

35

40

which makes for a complex software implementation. The question is whether the

practically feasible performance improvements warrant such a complicated controller

implementation.

Several commercial and academic UAV autopilots implement relatively simple P,

34

PI, or PID controllers for the heading-hold and altitude-hold functions. Encouraged

by their successes and our own experiences in simulation, we choose to formulate a

PI loop for altitude hold and a simpler proportional controller for heading hold.

6.3.1

Given a desired altitude, hd , we can calculate an altitude error signal from our

altitude measurement.

eh = h hd

(6.26)

The control law for the altitude hold autopilot is implemented as

= P eh + I

6.3.2

Z t

0

eh dt

(6.27)

The ground track heading hold autopilot is configured with a simple proportional

controller. With a desired ground track specified as d and a measurement of the

current ground track heading, , the lateral error signal can be calculated.

e = d

(6.28)

= P e

(6.29)

Additionally, both the ground track and altitude hold controllers include saturation blocks ( 45o in roll, 20o in pitch) to eliminate illogical commands in cases

of large error signals.

6.3.3

Simulation Results

The simpler, linear autopilot designs were tuned and tested using the 6-DOF

simulation model for a variety of conditions. The proportional ground track heading

hold autopilot was successful, as expected, for small step inputs in desired ground

track commands. Figure 6.5 shows typical response in ground track and bank angle

for a 15o change in desired ground track. The ground track heading hold autopilot is

35

Figure 6.5 Proportional Ground Track Heading Hold Autopilot, 15o Step Input

required to drive the ground track dynamics to a form approximating Equation 6.3,

since this is assumed by the Path Following algorithms. Moreover, the first order time

constant a should be quantifiable and constant over a wide range of operation. The

proportional controller is able to maintain the same response characteristics until the

right hand side of Equation 6.29 is large enough to saturate the bank angle command

at 45o . Performance, as measured by rise time, degrades for larger step inputs, but

stability is maintained. This effect is shown in Figure 6.6 where the time response of

the ground track heading dynamics, normalized by the magnitude of the step input,

are shown for several step input sizes and a constant proportional gain, P . The

transient response curves are tightly grouped for step input magnitudes between 15

and 45 degrees, but performance degrades for higher values. This phenomenon is

alleviated for smaller gains, as seen in Figure 6.7, at the cost of a constant but larger

time constant, a .

A comparison between the feedback linearization and PI altitude hold autopilots

also demonstrates the relatively small difference in performance between the two

designs. Both the feedback linearization (FL) and proportional plus integral (PI)

altitude hold controllers can be tuned for well-damped responses to a step input in

desired altitude as seen in Figure 6.8. This tuning is done with wings-level flight

( = 0). For a maneuvering vehicle, however, 6= 0, and the performance of both

36

FL and PI altitude hold controllers exhibit small altitude losses (and gains). This

behavior can be seen a maneuvering aircraft with (t) = 45o sin t in Figure 6.9.

The oscillations in the feedback linearization controller response are damped by the

explicit inclusion of in the control law of Equation 6.13, but the approximations

made in the FL controller derivation (namely, = 0) are not well satisfied during

aggressive maneuvers. Even without a derivative term or a lateral coupling term, the

PI controller is able to maintain performance comparable to that of the FL controller

37

in simulation. The magnitude of the altitude oscillations for both controller cases

are less than three feet. 6.8.

After evaluation comparison of numerous simulation studies, all with results comparable to those presented in this chapter, it was determined that the feedback linearization autopilot algorithms demonstrated only slightly improved performance

over the (P, PI) autopilot design in only a few situations. Accordingly,the increased

hardware complexity, i.e. the requirements for additional sensors for airspeed and

38

body-fixed heading, are not warranted and the simpler proportional, proportionalintegral controllers are used for the autopilot subsystem in implementation.

39

Chapter 7

GNC System Implementation

Hardware and software implementations of the selected path following, autopilot

and attitude stabilization systems are presented in this chapter. The hardware developed to accomplish the basic GNC functions includes a flight computer, sensors

and the DAC/op-amp scheme designed to complete the modified Co-pilot attitude

stabilization system. Additional developments detailed in this chapter are a wireless

telemetry downlink and a microcontroller-based pilot override system.

7.1

The flight computer is composed of an MSP430F149 microcontroller, manufactured by Texas Instruments. The MSP430 family is used in several courses in Texas

Tech Universitys Mechanical and Electrical Engineering Departments. The F149

variant offers two hardware USART ports, an 8 channel, 12-bit Analog to Digital

Converter (ADC) and is programmable in C. For the GNC system designed in the

previous chapters, using the simpler P, PI autopilot formulation, the only required

sensor is a GPS receiver which gives measurements of 3-D position, groundspeed and

ground-track heading. Four 12-bit digital to analog converters (DAC) are combined

with two summing operational amplifiers to provide the attitude stabilization bias

voltages, VDAC , for the two pitch and roll channels, as described in Chapter 4. The

hardware prototype for this GNC system is depicted graphically in Figure 7.1.

The Garmin 15L GPS receiver is configured to output the NMEA 0183 standard

GPGGA and GPVTG sentences at 9600 baud once a second. The flight computer

receives these sentences on UART0, which is configured for asynchronous serial communication (USART mode). The GPGGA sentence is parsed for position (latitude,

longitude, height aMSL) and the GPVTG sentence is parsed for ground-track heading

(course) and groundspeed. Additionally, the un-biased infrared readings, VIR , from

both the pitch and bank channels are sampled at a user-defined rate for transmission

40

to the ground station.

The position, ground-track, and groundspeed measurements are used to evaluate

the Path Following and Autopilot algorithms to generate the pitch and bank angle

commands. These commands are converted to a desired DAC voltage (either positive

or negative) using equation 4.4. This voltage is actuated by the flight computer via

a custom SPI library written to interface with the DAC7512 chip, manufactured by

Texas Instruments. Both the pitch and roll channels utilize two DAC modules a

piece, for a total of four modules. One of the channels DAC modules represents an

addition voltage, while the other represents a subtraction voltage, as shown for a

single channel (either pitch or roll) in Figure 7.2.

If the result of Equation 4.4 is greater than (or equal to) zero, then VDAC+ is set

to this result and VDAC is set to zero. If the result of Equation 4.4 is negative, then

VDAC+ is set to zero and VDAC is set to this result. In this way, the full 12 bits of

each DAC is devoted to the positive voltage range [0,3.3V], effectively doubling the

resolution and eliminating the need for negative voltage references.

7.2

Radio Modem

A 900 MHz radio link provides a wireless telemetry downlink to the ground station

software. The Maxstream 9XTend module is operated at a regulated 5.0 Volts with a

9600 baud rate using the Tx channel of UART0 on the flight computer. The 9XTend

module has a maximum transmit power of 1 Watt with software selectable power

ranges of 1, 10, 100, 500, or 1,000 mW. The current implementation uses 500 mW

41

for transmission to alleviate interference problems with the 72 MHz on-board RC

receiver. After parsing a set of valid GPS sentences and setting the DAC array to

actuate the commanded pitch and bank angles, the Flight Computer transmits an

ASCII sentence to the ground station for flight visualization and debugging purposes.

The structure and contents of the telemetry sentence are shown in Figure 7.3 and

Table 7.1.

7.3

A safety pilot override is required before any flight testing can begin. If the

aircraft adopts an unstable attitude or an undesirable flight path, the safety pilot

must be able to regain control and bring the vehicle home to ensure the integrity of

the electronics payload and the safety of any observers.

The pilot override consists of a second MSP430 microcontroller. This microcontroller is an F2012 variant, which has a smaller pinout, less I/O capabilities and is

cheaper. The F2012 monitors a PWM signal from the on-board 72 MHz RC receiver. This channel is configured to a switch on the RC pilots controls. When

42

Symbol Format Quantity

J

N/A

Denotes Beginning of Telemetry Sentence

<1>

xx

Latitude Degrees North

<2>

xx.xxxx Latitude Minutes North

<3>

xxx

Longitude Degrees West

<4>

xx.xxxx Longitude Minutes West

<5>

xxxxx.x GPS height in Meters Above/Below Mean Sea Level

<6>

xxx.xx Pressure Airspeed (m/s) Not Currently Implemented

<7>

xxx

GPS Ground Track Heading: 000-359 degrees CW from North

<8>

xxxx.x GPS Ground Speed: km/h

<9>

xxxx

IR ADC Reading, Roll Axis: 0-4095 corr. to 0-3.3 Volts

< 10 >

xxxx

IR ADC Reading, Pitch Axis: 0-4095 corr. to 0-3.3 Volts

< 11 >

xxxx

IR ADC Reading, Yaw Axis: 0-4095 corr. to 0-3.3 Volts

< 12 > xxxxxx GPS Time of Fix: hhmmss Format

< 13 >

xxx.x

Calculated Pitch Angle Command (degrees)

< 14 >

xxx.x

Calculated Roll Angle Command (degrees)

xx

Denotes End of Telemetry Sentence

< CR >

x

Carriage Return character

< NL >

x

New Line character

the pulse width on the PWM signal raises above a certain threshold (corresponding

to an ON control switch), the F2012 raises an digital signal high. This high signal

is then read by the Flight Computer and autonomous operation is enabled. If the

pulse width drops below the threshold (corresponding to an OFF control switch),

the F2012 drops the digital signal low. This triggers an interrupt on the Flight Computer which disables the telemetry downlink and sets all DAC bias voltages to zero

for remote pilot flight.

7.4

In the absence of a reliable aircraft system, a hardware-in-the-loop (HIL) simulation environment was developed to test, debug and validate the hardware/software

implementations of the GNC system design. In these simulations, the nonlinear,

6-DOF equations of motion are integrated in software. The output position and

velocity quantities are then converted to the form of expected GPS measurements:

43

are used to construct simulated GPS readings using the NMEA 0183 ASCII format

and passed to the Flight Computer through an RS-232 interface at a 1 Hz frequency

to simulate the actual GPS environment. The simulated pitch and bank angles are

converted to simulated infrared voltages and passed through two analog outputs to

the Flight Computers ADC.

The Flight Computer is then able to perform the same functions as it would if

it were on an actual flying aircraft. The Flight Computer runs the navigation and

actuation routines, and the DAC bias voltages are read by the simulation computer

as commanded pitch and bank angles. The simulation computer then uses these as

inputs to integrate the equations of motion and the HIL simulation cycle continues.

The Flight Computer also sends the telemetry data through the wireless modem to

a laptop running the ground station software.

Figure 7.4 shows typical Hardware in the Loop simulation results from the simulation computer.

44

Chapter 8

Conclusion

This thesis presents a complete Guidance, Navigation and Control system design

for an autonomous air vehicle. Flight tested algorithms for air vehicle path following

are summarized. These algorithms are combined with a modified commercial attitude

stabilization system based on horizon sensing using infrared thermopiles. An original autopilot design using feedback linearization is also presented alongside a more

traditional proportional and proportional plus integral autopilot design. Simulations

show that the qualitative performance of both autopilot designs is comparable, and

it is concluded that the current complexity of the physical implementation of the

feedback linearization control strategy does not warrant further investigation at this

time and a simpler P, PI autopilot is selected for implementation. The path following, autopilot, and attitude stabilization system designs demonstrate acceptable

performance in simulation and these three subsystem designs are implemented in

hardware and tested using Hardware in the Loop simulations.

Future work to experimentally validate the entire GNC system design is required.

Although the basic hardware implementations have been validating using HIL simulation, these simulations assume satisfactory performance of the modified commercial

attitude stabilization system, which has yet to be demonstrated for the custom design. Additionally, while the simple P, PI autopilot designs require fewer sensors

and less software to implement, their gains have proved cumbersome to tune in

simulation. An investigation of the actual tuning requirements of a P,PI autopilot

implementation compare to that of a feedback linearization implementation.

45

Bibliography

[1] Markley, F.L., Yang, C., and Crassidis, J.L. Nonlinear Attitude Filtering Methods Proceedings of the AIAA Guidance, Navigation, and Control Conference

and Exhibit, San Francisco, 15-18 August 2005.

[2] Egan, G. and Taylor, B. Characterisation of Infrared Sensors for Absolute Unmanned Aerial Vehicle Attitude Determination, Technical Report MECSE-22007, Monash University, 2007.

[3] Etkin, B. Dynamics of Atmospheric Flight. New York: John Wiley and Sons,

1972.

[4] Farrell, J.A. and Barth, M. The Global Positioning System and Inertial Navigation. New York: McGraw-Hill, 1999.

[5] Farrell, J.A. Aided Navigation: GPS with High Rate Sensors. New York: McGraw Hill, 2008.

[6] Holland, G. The Aerosonde Robotic Aircraft: A New Paradigm for Environmental Observations, Bulletin of the American Meteorological Society, vol. 82

part 5, pp. 889-902, 2001.

[7] Khalil, H.K., High-gain observers in nonlinear feedback control, Proceedings

of the International Conference on Control, Automation and Systems, 14-17

October 2008.

[8] Khalil, H.K. Nonlinear Systems. Upper Saddle River, NJ: Prentice Hall, 2002.

[9] Kingston, D. Implementation Issues of Real-Time Trajectory Generation on

Small UAVs. Masters thesis, Brigham Young University, April 2004.

[10] Miele, A. Flight Mechanics, Volume One: Theory of Flight Paths. Reading, MA:

Addison-Wesley, 1962.

[11] Nelson, D.R., Barber, D.B., McLain, T.W., and Beard, R.W. Vector Field

Path Following for Small Unmanned Air Vehicles, Proceedings of the American

Control Conference, Minneapolis, 14-16 June 2006.

[12] Stevens, B.L. and Lewis, F.L. Aircraft Control and Simulation. Hoboken, NJ:

John Wiley and Sons, 2003.

46

[13] Tayebi, A., McGilvray, S., Roberts, A., and Moallem, M. Attitude Estimation

and Stabilization of a Rigid Body Using Low-Cost Sensors, Proceedings of the

46th IEEE Conference on Decision and Control, New Orleans, 12-14 December

2007.

[14] Taylor, B., Bil, C., Watkins, S., and Egan, G. Horizon Sensing Attitude Stabilisation: A VMC Autopilot, Proceedings of the 18th International UAV Systems

Conference, Bristol, UK, 2003.

[15] Aerosim Blockset, Unmanned Dynamics. [Online]. Available: http://www.udynamics.com/aerosim/

47

Appendix A

Trajectory Model

The 6-DOF, body axes equations of motion involve complicated aerodynamic

moments, nonlinear coupling between the angular rates and other undesirable mathematical structures for control system design. The trajectory tracking system, described in Chapter 5, however, maps desired the desired aircraft kinematics (groundtrack heading and altitude) into desired pitch and roll angles, and must therefore

consider the effects of the relevant forces acting on the aircraft. This chapter presents

a more intuitive model of the aircrafts flight path that is used in Chapter 5 to formulate a nonlinear solution to the trajectory tracking problem.

To simplify control design, we first assume that the full orientation of the aircraft

is measurable and/or controllable at any given time. The quantities , , and

are assumed to be time varying, but known. The angles and are assumed to

be controllable, via some relatively tight attitude stabilization loop. We can then

model the aircraft as a particle and apply Newtons law to generate an alternative

but equivalent state space model, with a reduced number of state equations.

Newtons law for the aircraft center of mass (COM) is given as

T + A + mg = ma0

(A.1)

where T, A, and a0 are the thrust force, aerodynamic force and total acceleration

vectors, respectively.

The path axes frame, as defined in Chapter 2, is centered at the aircraft COM,

but can rotate with respect to the inertial frame. The total acceleration, a0 , term

must account for this angular rate, which is nonzero in general, using a correction

term for a rotating frame.

48

d

d

Vcm/e = p Vcm/e +

p/e Vcm/e

(A.2)

dt

dt

Since the xp axis points in the direction of the inertial velocity vector, Vcm/e can

easily be expressed in path coordinates.

a0 = e

p

Vcm/e

= Vinip

(A.3)

p

The scalar quantity Vin is the magnitude of Vcm/e

. The angular velocity vector of

the path axes with respect to inertial axes is

p/e = ( sin ) ip + ()

jp + ( cos ) k

(A.4)

The total acceleration (A.2) can then be expressed in path axes coordinates.

p

in cos ) jp (V

in ) k

a0 = V in ip + (V

(A.5)

Tp + Ap + mgp = m

V in

V

in cos

V

in

(A.6)

The right superscripts denote the vector quantities written in path axes coordinates.

In order to complete the three scalar equations of motion, we must express the thrust,

aerodynamic, and gravitational forces in path axes coordinates.

Forces

Gravity

The gravitational force resolved into path axes is given by

mgp = [Re2p ]

0

0

mg

49

mg sin

0

mg cos

(A.7)

Thrust Force

We assume the thrust force, T, is directed along the bodys xb axis and resolve

this force into the path axes directions using rotation matrices.

Tp = [Re2p ] [Rb2e ]

T

0

0

(A.8)

Expanded, the thrust force in the path axes coordinate directions is given by

sin sin + sin sin cos cos + cos cos cos cos

T

cos sin cos sin cos cos

sin sin cos sin + cos cos cos sin sin cos

(A.9)

where T is the magnitude of the thrust force generated by the motor-prop combination.

Aerodynamic Forces

The aerodynamic forces (lift, drag, and sideforce) are applied in the wind frame.

Aw =

C

L

(A.10)

The sideforce, C, is primarily caused by sideslip and rudder deflection. For an aircraft

with fixed rudder and a suitable vertical tail fin, sideslip is induced only when the

aircraft banks and is quickly damped by the moment generated by the vertical tail.

Although sideforce is modeled in simulation, we neglect this force in the Trajectory

Model, which is used only for control design. With C = 0, the aerodynamic forces

can then be expressed in the path axes directions.

p

p

p

(A.11)

Ignoring sideslip (setting =0 in Rw2b ), the total external force, expressed in path

axes coordinates, can be expressed as the sum of the gravitational, thrust and aero-

50

dynamic forces.

p

p

p

T + A + mg =

LfLy + DfDy + T fTy

(A.12)

In equation A.12, L, D, and T are the magnitudes of the lift, drag, and thrust forces,

and the fQi are the direction cosines of the Q force in the path axes i direction. The

direction cosines are shown below as a function of the aerodynamic, attitude, and

flight path angles.

fLx = (ss + ccc) s + (sc csc) c cos( ) + (csc) sin( )

fDx = (scc cs) s (ssc + cc)c cos( ) + (ssc) sin( )

fLy = (csc sc) sin( ) + cs cos( )

fDy = (scs + cc) sin( ) + ss cos( )

fLz = (sc csc) s cos( ) + css sin( ) (ss + ccc) c

fDz = (cc + scs) s cos( ) + sss sin( ) + (cs scc) c

fTx = cc cos( ) + ss

fTy = c sin( )

fTz = cs cos( ) sc

(A.13)

Substituting A.12 into the LHS of Equation A.6 yields scalar state equations for Vin ,

, and .

mV in = LfLx + DfDx + T fTx mg sin

mV

in cos = LfLy + DfDy + T fTy

mV

in = LfLz + DfDz + T fTz + mg cos

(A.14)

These equations form a basis for the derivation the nonlinear autopilot designs

and are referred to collectively as the trajectory model. Together with the assumption

that vehicle orientation can be instantaneously measured (, , , and are known

inputs), zero sideslip, and a model of lift and drag forces, these three equations

describe the same path dynamics as the full 6-DOF body axes equations given in

Chapter 3.

51

Appendix B

Simulation Code

This appendix contains the MATLAB files used to simulate the GNC system. The

main script (mainScript.m) sets initial conditions, runs the simulation, and interprets

and plots the output. The RHS (rhs6DOFcontrolled.m) function generates the state

equations at each timestep. Each of the three GNC subsystems is contained in its

own MATLAB function (pathFollowing.m, autopilot.m and attitudeStabilization.m).

Additional subroutines are used for canned functions such as generating waypoint

data, rotation matrices, and output calculations. These files must all be included in

a common directory.

Main Script

The main script (mainScript.m) sets initial conditions, runs the simulation, and

interprets and plots the output.

% Main Simulation Script

% mainScript.m

clear all; close all; clc

waypointgen; % Run waypoint generation script (waypointgen.m)

WN0=0; WE0=0; WD0=0; VcI = 27.88; GcI = 0; ChiCi = 0;

gamHatInitial = 15*pi/180; % Observer

hHatInitial = 205; % Observer

tspan = [0,130]; % Time Span

IC = [0,0,-200,0,.00682,0,27.88,0,0,0,0,0,WN0,WE0,WD0,...

.006820,0,27.88,200,0,0,0,0,gamHatInitial,hHatInitial,...

VcI, ChiCi,GcI];

count=0;

fprintf(Simulating...\n)

[t,state] = ode45(@rhs6DOFPIDinnerLoops,tspan,IC,[],count);

52

fprintf(Simulation Complete.\n)

%keyboard

[X,Y,Z,phi,theta,psi,U,V,W,P,Q,R,W_N,W_E,W_D,gammaCalc1,VCalc1,...

h2,x2,y2...,gamHat,hHat,chiCalc,gamCalc,V_T,q,aoa,bet,gam,...

chi,V_in,Lift,Drag,Sideforce,A,B,C,v,phi_d,theta_d,aoaEst,...

LiftEst,DragEst,chi_d]= calcOutputs(t,state);

% Calculate outputs

% State Estimation

subplot(3,1,1)

h=-Z;

plot(t,h,t,hHat)

xlabel(t (seconds))

ylabel(Altitude (m))

subplot(3,1,2)

plot(t,gam*180/pi,t,gamHat*180/pi)

ylim([-40,40]);

xlabel(t (seconds))

ylabel(Flight Path Angle (deg))

subplot(3,1,3)

plot(t,(aoa)*180/pi)

title(AOA (deg))

% Autopilot

figure;

subplot(3,1,1)

plot(t,chi*180/pi)

title(Flight Path Heading Angle)

subplot(3,1,2)

plot(t,phi*180/pi)

title(Bank Angle)

subplot(3,1,3)

plot(t,theta*180/pi)

title(Pitch Angle)

% Aerodynamic Forces

plotAeroForces;

% Control Time Series

plotControls;

53

%%%%

figure;

subplot(3,1,1)

plot(t,aoa*180/pi,t,aoaEst*180/pi)

legend(Actual,Est.)

ylabel(AOA (deg))

subplot(3,1,2)

plot(t,Lift,t,LiftEst);

legend(Actual,Est.)

ylabel(Lift (N))

subplot(3,1,3)

plot(t,Drag,t,DragEst);

ylabel(Drag (N))

legend(Actual,Est.)

% Birds-eye View

figure;

plot(Y,X,b,y1,x1,ro)

RHS Function

The right hand side (RHS) function generates the state equations at each timestep.

This file must be included in the local directory.

function stateDots = rhs6DOFcontrolled(t,state,co)

% RHS of 6DOF model

% t - current time

% state - state vector: [ X, Y, Z, phi, theta, psi, U, V, W,...

%

P, Q, R ,... W_N , W_E , W_D,...

%

augmented states... ]

% Give the states nicer names

X = state(1); Y = state(2); Z = state(3);

phi = state(4); theta = state(5);

psi = state(6);

U = state(7); V = state(8); W = state(9); P = state(10); Q = state(11);

R = state(12); W_N = state(13); W_E = state(14); W_D = state(15);

gammaCalc=state(16); chiCalc=state(17); VCalc = state(18);

gamHat = state(24); hHat = state(25); Vcalc = state(26);

chiCalc = state(27); gamCalc = state(28);

h=-Z;

g=9.81;

54

% Wing span

b = 2.8956; % m

% Wing area

S = 0.55; % m^2

AR = b^2/S;

c = .189941;

% Gross Moments of Inertia kg*m^2

Jx = .8244;

Jy = 1.135;

Jz = 1.759;

Jxz = .1204;

% ALL aerodynamics derivatives are per radian:

%%% Lift coefficient %%%

% Zero-alpha lift

C_L0 = 0.23;

% alpha derivative

C_Lalpha = 5.6106;

% Pitch control (elevator) derivative

C_LdeltaE = 0.13;

% alpha-dot derivative

C_Lalphadot = 1.9724;

% Pitch rate derivative

C_LQ = 7.9543;

%%% Drag coefficient %%%

% Lift at minimum drag

CLmind = 0.23;

% Minimum drag

C_D0 = 0.0434;

% Pitch control (elevator) derivative

C_DdeltaE = 0.0135;

% Roll control (aileron) derivative

C_DdeltaA = 0.0302;

% Yaw control (rudder) derivative

C_DdeltaR = 0.0303;

% Oswalds coefficient

e = 0.75;

%%% Side force coefficient %%%

% Sideslip derivative

C_Ybeta = -0.83;

55

C_YdeltaA = -0.075;

% Yaw control derivative

C_YdeltaR = 0.1914;

% Roll rate derivative

C_YP = 0;

% Yaw rate derivative

C_YR = 0;

%%% Pitch moment coefficient %%%

% Zero-alpha pitch

C_m0 = 0.135;

% alpha derivative

C_malpha = -2.7397;

% Pitch control derivative

C_mdeltaE = -0.9918;

% alpha_dot derivative

%C_malphaDot = -10.3796;

% Pitch rate derivative

C_mQ = -38.2067;

%%% Roll moment coefficient %%%

% Sideslip derivative

C_lbeta = -0.13;

% Roll control derivative

C_ldeltaA = -0.1695;

% Yaw control derivative

C_ldeltaR = 0.0024;

% Roll rate derivative

C_lP = -0.5051;

% Yaw rate derivative

C_lR = 0.2519;

%%% Yaw moment coefficient %%%

% Sideslip derivative

C_nbeta = 0.0726;

% Roll control derivative

C_ndeltaA = 0.0108;

% Yaw control derivative

C_ndeltaR = -0.0693;

% Roll rate derivative

C_nP = -0.069;

% Yaw rate derivative

56

C_nR = -0.0946;

mass = 13.5; % kg (my estimate)

Weight=mass*g;

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

Thrust = 10;

% Trim 21.4066

airframeProps = [mass*9.81,S,C_D0,e,AR,C_Lalpha,C_L0,Thrust];

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

% Model for air density

rho = 1.121;

% Inertia Matrix

InertiaMatrix = [Jx , 0 , -Jxz; 0, Jy , 0; -Jxz , 0 , Jz];

% Rotation from tangent plane to body frame

C_bn = [ cos(psi)*cos(theta) , sin(psi)*cos(theta)...

, -sin(theta); -sin(psi)*cos(phi)+cos(psi)*...

sin(theta)*sin(phi) , cos(psi)*cos(phi)...

+sin(psi)*sin(theta)*sin(phi) , cos(theta)*sin(phi) ;...

sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi), ...

-cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi) , ...

cos(theta)*cos(phi) ];

% Rotation from body frame to tangent plane

C_nb = C_bn;

% Cross Product Matrix for omega x ...

OmegaCrossProdMatrix = [0 , -R , Q; R , 0 , -P ; -Q, P, 0];

% Calculate relative velocities (w.r.t moving atmosphere (wind) )

[c1,Rb2e,c2] = calcRotationMatrices(psi,theta,phi,0,0,0,0);

primes = [U ; V ; W] - Rb2e*[W_N ; W_E ; W_D];

Uprime = primes(1);

Vprime = primes(2);

Wprime = primes(3);

57

V_T = norm([Uprime; Vprime ; Wprime]);

q = .5*rho*(V_T)^2;

positionDots = Rb2e*[U ; V ; W ];

Xdot = positionDots(1);

Ydot = positionDots(2);

Zdot = positionDots(3);

V_in = sqrt(Xdot^2+Ydot^2+Zdot^2);

gam = atan2(-Zdot,sqrt(Xdot^2+Ydot^2));

chi = atan2(Ydot,Xdot);

aoa = atan2(Wprime,Uprime);

bet = asin(Vprime/V_T);

[Re2p,Rb2e,Rw2b] = calcRotationMatrices(psi,theta,...

phi,bet,aoa,chi,gam);

% Calculate Aerodynamic Forces

C_L = C_L0 + C_Lalpha*aoa;

Lift = q*S*C_L;

Drag = q*S*C_D;

[chi_d] = pathFollowing(X,Y,chi,V_T,S);

%% My Controller

[phi_desired,theta_desired,h_d,k1,k2] = ...

controllerPIDinnerLoops(t,state,airframeProps,Lift,Drag,chi_d);

pitchError = theta-theta_desired;

pitchErrorDerivative = cos(phi)*Q-sin(phi)*R;

pitchErrorIntegral = state(22);

pitchErrorIntegralDot = pitchError;

bankError = phi-phi_desired;

bankErrorDerivative = P+tan(theta)*sin(phi)*Q+tan(theta)*cos(phi)*R;

58

bankErrorIntegral = state(23);

bankErrorIntegralDot = bankError;

% Simulate PID inner loops for att. stabilization

[deltaA,deltaE,deltaR] = attitudeStabilization(t,state,...

bankError,bankErrorDerivative,bankErrorIntegral,pitchError,...

pitchErrorDerivative,pitchErrorIntegral);

C_Y = C_Ybeta*bet + C_YdeltaR*deltaR + C_YdeltaA*deltaA +...

b/(2*V_T)*(C_YP*P + C_YR*R);

Sideforce = q*S*C_Y;

% Calculate Aerodynamic Moments

% Rolling Moment

C_l = C_lbeta*bet + C_ldeltaA*deltaA + C_ldeltaR*deltaR +...

b/(2*V_T)*(C_lP*P+C_lR*R);

l = q*S*b*C_l;

% Pitching Moment

C_m = C_m0 + C_malpha*aoa + C_mdeltaE*deltaE+ ...

c/(2*V_T)*(C_mQ*Q); % C_malphaDot*alphaDot

m = q*S*c*C_m;

% Yawing Moment

C_n = C_nbeta*bet + C_ndeltaA*deltaA + C_ndeltaR*deltaR +...

b/(2*V_T)*(C_nR*R+C_nP*P);

n = q*S*b*C_n;

% Begin State Equations:

positionDots = Rb2e*[U ; V ; W ];

Xdot = positionDots(1);

Ydot = positionDots(2);

Zdot = positionDots(3);

% phidot ; thetadot ; psidot

eulerdots = [1 , tan(theta)*sin(phi) , tan(theta)*cos(phi) ;...

0, cos(phi) , -sin(phi);...

0, sin(phi)/cos(theta) , cos(phi)/cos(theta)]*[P;Q;R];

psidot = eulerdots(3);

thetadot = eulerdots(2);

phidot = eulerdots(1);

59

velocitydots = (1/mass)*(Rw2b*[-Drag;-Sideforce;-Lift] +...

[Thrust ;0 ;0]) + Rb2e*[0;0;9.81]...

- OmegaCrossProdMatrix*[U;V;W];

% Pdot ; Qdot ; Rdot

angularRatedots = inv(InertiaMatrix)*...

(Rw2b*[ l ; m ; n ]-OmegaCrossProdMatrix*...

InertiaMatrix*[P ; Q ; R]);

%keyboard;

Pdot = angularRatedots(1);

Qdot = angularRatedots(2);

Rdot = angularRatedots(3);

% Path calculations for debugging

Drag_pathAxes = Re2p*Rb2e*Rw2b*[-Drag;0;0];

Lift_pathAxes = Re2p*Rb2e*Rw2b*[0;0;-Lift];

Sideforce_pathAxes = Re2p*Rb2e*Rw2b*[0;-Sideforce;0];

Thrust_pathAxes = Re2p*Rb2e*[Thrust;0;0];

F_pathAxes = Drag_pathAxes+...

Lift_pathAxes+Thrust_pathAxes+Sideforce_pathAxes;

mg_pathAxes=Re2p*[0;0;mass*g];

gamDot = -1/(mass*V_in)*(F_pathAxes(3)+mg_pathAxes(3));

Vdot=g*(Thrust/(mass*g) -Drag/(mass*g) -sin(gam));

chiDot = 1/(mass*V_in*cos(gam))*...

(F_pathAxes(2)+mg_pathAxes(2));

% Observer Formulation

epsilon1 = 0.05;

epsilonGamma = 1/epsilon1;

epsilonH = 1/epsilon1;

gamHatDot = -((-Lift*cos(phi)*sin(theta) +...

(Thrust-Drag)*sin(theta))*gamHat - ...

(Lift*cos(phi)*cos(theta) +...

(Thrust-Drag)*sin(phi)-mass*g ))/(mass*V_in) ...

+ epsilonGamma*(h-hHat);

hHatDot = V_in*gamHat + epsilonH*(h-hHat);

stateDots = [Xdot ; Ydot ; Zdot ; phidot; thetadot ; psidot ;...

velocitydots ; Pdot ; Qdot ; Rdot ; 0 ; 0 ; 0 ; gamDot; ...

chiDot;Vdot; h2Dot;x2Dot; y2Dot; pitchErrorIntegralDot; ...

bankErrorIntegralDot ; gamHatDot ; hHatDot; Vdot; ...

chiDot; gamDot];

60

Subroutines

These subroutines perform repeated tasks and must also be included in the local

simulation directory.

function [chiC] = pathFollowing(x,y,chi,V_T,S)

% Path Following subroutine

% %Design parameters%

tau = 35;

% set transition boundary distance

% on either side of path

xe=pi/5;

% set entry angle heading

k=2;

% Transition Gain

deltaS = 1.0; % Waypoint Switching look-ahead

a = 1;

g = 9.81;

cnt=0;

% initialize iterative counting index

z = [x;y];

d = getpts;

% grabs waypoints from data file

% Check to see if current position is past any waypoints

sprime=5;

% Make first condition check true

while sprime>deltaS

cnt=cnt+1;

sprime = ( ((z-d(1:2,cnt)))*( d(1:2,(cnt+1))-...

d(1:2,cnt)))./ ((norm( d(1:2,(cnt+1))-d(1:2,cnt)))^2);

end

sprime = ( ((z-d(1:2,cnt)))*( d(1:2,(cnt+1))-d(1:2,cnt)))...

./ ((norm( d(1:2,(cnt+1))-d(1:2,cnt)))^2);

% Calculate current waypoint ground track heading

xf = atan2( (d(2,cnt+1)-d(2,cnt)),(d(1,cnt+1)-d(1,cnt)));

% Calculate error from path

epsilon = norm(z - ((sprime*( d(1:2,(cnt+1))-d(1:2,cnt)))+...

d(1:2,cnt)));

% Assign variable names to the currently applicable waypoints

% w1 and w2 are 3 dimensional vectors to use cross function

w1= [d(1:2,cnt);0];

w2= [d(1:2,cnt+1);0];

znow = [z;0]; % add third dimension to current position

61

% p is either +1 or -1

p = sign ( cross( (w2-w1), (znow-w1) ) );

p = p(3,1);

% Give error a sign

epsilon = p*epsilon;

% Calculate Commanded Heading Angle

if abs(epsilon)>tau % Position is far away

xd = xf -(p*xe);

xc = xd;

else

% Position is inside transition zone

xd = xf - p*xe*((epsilon/tau)^k);

xc = xd - (k*xe*S/(a*tau^k))*(epsilon^(k-1))*sin(chi);

end

chiC = xc;

% Remove waypoints already passed, write new waypoint file

% not removing most recently passed waypoint

dnew = d(1:2,cnt:length(d));

writepts(dnew);

end

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

function [deltaA,deltaE,deltaR] = ...

attitudeStabilization(t,state,bankError,...

bankErrorDerivative,bankErrorIntegral,pitchError,...

pitchErrorDerivative,pitchErrorIntegral)

% Simulates PID loops for attitude stabilization

psi = state(6);

theta = state(5);

phi = state(4);

K_thetaP

K_thetaI

K_thetaD

K_phiP =

K_phiI =

K_phiD =

= 10;

= 1;

= 1;

1;

.5;

0;

62

K_phiD*bankErrorDerivative;

deltaE = (K_thetaP*pitchError+K_thetaI*pitchErrorIntegral+...

K_thetaD*pitchErrorDerivative);%Trim -3deg @.00682,,27.88

deltaR = 0; % Fixed Rudder

% Saturation

if deltaE > 60*pi/180

deltaE = 60*pi/180;

% fprintf(Sat Elev. Pos.\n)

elseif deltaE < -60*pi/180

deltaE = -60*pi/180;

% fprintf(Sat Elev. Neg.\n)

end

if deltaA > 45*pi/180

deltaA = 45*pi/180;

%fprintf(Sat Ail. Pos.\n)

elseif deltaA < -45*pi/180

deltaA = -45*pi/180;

%fprintf(Sat Ail. Neg.\n)

end

end

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

function [bank,pitch,h_d,k1,k2] = ...

autopilot(t,state,airframeProps,Lift,Drag,chi_d)

% Feedback Linearization Autopilot Algorithms

%% Nice names for states, etc.

X = state(1); Y = state(2); Z = state(3); phi = state(4);

theta=state(5);psi = state(6); U = state(7); V = state(8);

W = state(9); P = state(10); Q = state(11); R = state(12);

W_N = state(13); W_E = state(14); W_D = state(15); g=9.81;

rho = 1.121; h = 0-Z;Weight = airframeProps(1);

S = airframeProps(2); CD_0 = airframeProps(3);

e = airframeProps(4); AR= airframeProps(5);

CL_alpha = airframeProps(6);CL_0= airframeProps(7);

Thrust = airframeProps(8); mass=Weight/g;

h_d = 200; % Altitude Setpoint

63

% Design parameters

aChi=.465;

k1=10;

k2=10;

% Calculate Airspeed, dynamic pressure, aerodynamic angles

C_bn = [ cos(psi)*cos(theta) , sin(psi)*cos(theta) , ...

-sin(theta); -sin(psi)*cos(phi)+cos(psi)...

*sin(theta)*sin(phi) , ...

cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi) , ...

cos(theta)*sin(phi) ; sin(psi)*sin(phi) + ...

cos(psi)*sin(theta)*cos(phi), -cos(psi)*sin(phi) +...

sin(psi)*sin(theta)*cos(phi) , cos(theta)*cos(phi) ];

C_nb = C_bn;

primes = [U ; V ; W] - C_bn*[W_N ; W_E ; W_D];

Uprime = primes(1);Vprime = primes(2);Wprime = primes(3);

V_T = norm([Uprime; Vprime ; Wprime]);

q = .5*rho*(V_T)^2;

aoa = atan2(Wprime,Uprime);

bet = asin(Vprime/V_T);

% Calculate Gamma: (or grab estimate)

positionDots = C_nb*[U ; V ; W ];

%Xdot = positionDots(1); Ydot = positionDots(2);

%Zdot = positionDots(3);

%V_in = sqrt(Xdot^2+Ydot^2+Zdot^2);

%gam = atan2(-Zdot,sqrt(Xdot^2+Ydot^2));

gam = state(24); % Estimated

chi = atan2(Ydot,Xdot); % Measurement

%% Measured Airspeed

V_a = Uprime; % w.r.t. rel. wind. & measured along body x axis

x1 = h-h_d;

x2=V_in*sin(gam);

v = -k1*x1-k2*x2;

% Wrap the difference mod. 2pi

dif = (chi-chi_d);

if dif < -pi

dif = dif + 2*pi;

else if dif > pi

dif = dif-2*pi;

end

64

end

Thrust_est = Thrust;

Drag_est = Drag;

Lift_est = Lift;

bank = (asin( (-aChi*(chi-chi_d)*mass*V_in*cos(gam))...

/Lift_est));

% Saturate

if bank > maxPhi

bank = maxPhi;

elseif bank<-maxPhi

bank = -maxPhi;

end

A1 = Thrust_est - Drag_est*cos(aoa)+Lift_est*sin(aoa);

B1 = Drag_est*sin(aoa)*cos(phi)+Lift_est*cos(aoa)*cos(phi);

% Linearization Solution:

pitch = (mass*v+mass*g-B1)/A1;

% Find exact solution if it exists

if (sqrt(A1^2+B1^2)>=abs( mass*v + mass*g))

theta0 = asin(B1/sqrt(A1^2+B1^2));

if A1<0

theta0=theta0+pi;

end

tcom = asin((mass*v +mass*g)/sqrt(A1^2+B1^2))-theta0;

tquad = (-A1 + sqrt(A1^2+2*B1*(B1-mass*(v+g))))/-B1;

pitch = tcom;

else

end

if pitch>pi/14

pitch = pi/14;

elseif pitch <-pi/14

pitch = -pi/14;

end

end

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

65

function [X,Y,Z,phi,theta,psi,U,V,W,P,Q,R,W_N,W_E,W_D,...

gammaCalc,VCalc,h2,x2,y2,gamHat,hHat,chiCalc,...

gamCalc,V_T,q,aoa,bet,gam,chi,V_in,...

Lift,Drag,Sideforce,A,B,C,v,phi_desired,...

theta_desired,aoaEst,...

LiftEst,DragEst,chi_d]= calcOutputs(t,state)

% Calculate Ouputs

% Nice Names again:

X = state(:,1); Y = state(:,2); Z = state(:,3);

phi = state(:,4); theta = state(:,5);psi = state(:,6);

U = state(:,7); V = state(:,8); W = state(:,9);

P = state(:,10); Q = state(:,11); R = state(:,12);

W_N = state(:,13); W_E = state(:,14); W_D = state(:,15);

gammaCalc=state(:,16); chiCalc=state(:,17);

VCalc = state(:,18);h2 = state(:,19); x2 = state(:,20);

y2=state(:,21);gamHat = state(:,24);hHat = state(:,25);

Vcalc = state(:,26); chiCalc = state(:,27);

gamCalc = state(:,28);pitchErrorIntegral = state(:,22);

bankErrorIntegral = state(:,23);

h = -Z;

%Initialize for calcuation

V_T = zeros(size(X)); q = zeros(size(X));

bet = zeros(size(X)); gam = zeros(size(X));

V_in = zeros(size(X)); rho = 1.121;

Lift = zeros(size(X)); Drag = zeros(size(X));

phi_desired = zeros(size(X)); theta_desired = zeros(size(X));

deltaA = zeros(size(X)); deltaE = zeros(size(X));

aoa = zeros(size(X)); chi = zeros(size(X));

Sideforce = zeros(size(X));deltaR = zeros(size(X));

% Airframe Props

% Wing span

b = 2.8956; % m

% Wing area

S = 0.55; % m^2

AR = b^2/S;

c = .189941;

% Gross Moments of Inertia kg*m^2

Jx = .8244;

Jy = 1.135;

Jz = 1.759;

Jxz = .1204;

66

%%% Lift coefficient %%%

% Zero-alpha lift

C_L0 = 0.23;

% alpha derivative

C_Lalpha = 5.6106;

% Pitch control (elevator) derivative

C_LdeltaE = 0.13;

% alpha-dot derivative

C_Lalphadot = 1.9724;

% Pitch rate derivative

C_LQ = 7.9543;

%%% Drag coefficient %%%

% Lift at minimum drag

CLmind = 0.23;

% Minimum drag

C_D0 = 0.0434;

% Pitch control (elevator) derivative

C_DdeltaE = 0.0135;

% Roll control (aileron) derivative

C_DdeltaA = 0.0302;

% Yaw control (rudder) derivative

C_DdeltaR = 0.0303;

% Oswalds coefficient

e = 0.75;

%%% Side force coefficient %%%

% Sideslip derivative

C_Ybeta = -0.83;

% Roll control derivative

C_YdeltaA = -0.075;

% Yaw control derivative

C_YdeltaR = 0.1914;

% Roll rate derivative

C_YP = 0;

% Yaw rate derivative

C_YR = 0;

%%% Pitch moment coefficient %%%

% Zero-alpha pitch

67

C_m0 = 0.135;

% alpha derivative

C_malpha = -2.7397;

% Pitch control derivative

C_mdeltaE = -0.9918;

% alpha_dot derivative

%C_malphaDot = -10.3796;

% Pitch rate derivative

C_mQ = -38.2067;

%%% Roll moment coefficient %%%

% Sideslip derivative

C_lbeta = -0.13;

% Roll control derivative

C_ldeltaA = -0.1695;

% Yaw control derivative

C_ldeltaR = 0.0024;

% Roll rate derivative

C_lP = -0.5051;

% Yaw rate derivative

C_lR = 0.2519;

%%% Yaw moment coefficient %%%

% Sideslip derivative

C_nbeta = 0.0726;

% Roll control derivative

C_ndeltaA = 0.0108;

% Yaw control derivative

C_ndeltaR = -0.0693;

% Roll rate derivative

C_nP = -0.069;

% Yaw rate derivative

C_nR = -0.0946;

g = 9.81;

mass = 13.5; % kg (my estimate)

Weight=mass*g;

Thrust = 10;

% Trim 21.4066

[numRows,numCols] = size(state);

airframeProps = [mass*9.81,S,C_D0,e,AR,C_Lalpha,C_L0,Thrust];

A = zeros(size(X)); B = zeros(size(X)); h_d = zeros(size(X));

v=zeros(size(X));

Thrust = 21.4066*ones(size(X));

aoaEst=zeros(size(X)); LiftEst=zeros(size(X)); ...

68

DragEst = zeros(size(X));

chi_d=zeros(size(X));

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

counter1=0;

fprintf(Calculating Outputs: )

waypointgen;

for i = 1:length(X)

C_bn = [ cos(psi(i))*cos(theta(i)) , sin(psi(i))...

*cos(theta(i)) ,-sin(theta(i)); -sin(psi(i))*...

cos(phi(i))+cos(psi(i))*sin(theta(i))...

*sin(phi(i)) , cos(psi(i))*cos(phi(i))+...

sin(psi(i))*sin(theta(i))*sin(phi(i)) ,...

cos(theta(i))*sin(phi(i)) ; sin(psi(i))...

*sin(phi(i)) + cos(psi(i))*sin(theta(i))...

*cos(phi(i)), -cos(psi(i))*sin(phi(i))...

+ sin(psi(i))*sin(theta(i))*cos(phi(i)) ,...

cos(theta(i))*cos(phi(i)) ];

C_nb = C_bn;

primes = [U(i) ; V(i) ; W(i)] - C_bn*[W_N(i) ...

; W_E(i) ; W_D(i)];

Uprime = primes(1);

Vprime = primes(2);

Wprime = primes(3);

V_T(i) = norm([Uprime; Vprime ; Wprime]);

q(i) = .5*rho*(V_T(i))^2;

aoa(i) = atan2(Wprime,Uprime);

bet(i) = asin(Vprime/V_T(i));

positionDots = C_nb*[U(i) ; V(i) ; W(i) ];

Xdot = positionDots(1);

Ydot = positionDots(2);

Zdot = positionDots(3);

gam(i) = atan2(-Zdot,sqrt(Xdot^2+Ydot^2));

chi(i) = atan2(Ydot,Xdot);

V_in(i) = sqrt(Zdot^2+Xdot^2+Ydot^2);

C_L = C_L0 + C_Lalpha*aoa(i);

Lift(i) = q(i)*S*C_L;

C_D = C_D0 + (C_L-C_L0)^2/(pi*AR*e);

Drag(i) = q(i)*S*C_D;

chi_d(i)=trajectoryGenerator(X(i),Y(i),chi(i),V_T(i),S);

[phi_desired(i),theta_desired(i),h_d(i),k1,k2] = ...

autopilot(t(i),state(i,:),...

airframeProps,Lift(i),Drag(i),chi_d(i));

pitchError = theta(i)-theta_desired(i);

69

pitchErrorDerivative = cos(phi(i))...

*Q(i)-sin(phi(i))*R(i);

bankError = phi(i)-phi_desired(i);

bankErrorDerivative = P(i)+...

tan(theta(i))*sin(phi(i))*Q(i)...

+tan(theta(i))*cos(phi(i))*R(i);

[deltaA(i),deltaE(i),deltaR(i)] = ...

attitudeStabilization(t(i),state(i,:),bankError,...

bankErrorDerivative,bankErrorIntegral(i),...

pitchError, pitchErrorDerivative,...

pitchErrorIntegral(i));

C_Y = C_Ybeta*bet(i) + C_YdeltaR*deltaR(i) +...

C_YdeltaA*deltaA(i)...

+ b/(2*V_T(i))*(C_YP*P(i) + C_YR*R(i));

Sideforce(i) = q(i)*S*C_Y;

A(i) = Thrust(i) - Drag(i)*cos(aoa(i))+Lift(i)*sin(aoa(i));

B(i) = Drag(i)*sin(aoa(i))*cos(phi(i))+...

Lift(i)*cos(aoa(i))*cos(phi(i));

v(i) = -k1*(h(i)-h_d(i)) - k2*(V_in(i)*sin(gam(i)));

C(i) = mass*v(i)+mass*g;

aoaEst(i) = asin(-V_in(i)*sin(gam(i))/...

(V_T(i)*sqrt(cos(phi(i))^2*...

cos(theta(i))^2+sin(theta(i))^2))-...

asin(-sin(theta(i)/sqrt(cos(phi(i))^2....

*cos(theta(i))^2+sin(theta(i))))));

C_LEst = C_L0 + C_Lalpha*aoaEst(i);

LiftEst(i) = q(i)*S*C_LEst;

C_DEst = C_D0 + (C_LEst-C_L0)^2/(pi*AR*e);

DragEst(i) = q(i)*S*C_DEst;

if ((i>numRows/10)&(counter1<1))

fprintf(10%%..)

counter1 = 1;

elseif ((i>numRows/5)&(counter1<2))

fprintf(20%%..)

counter1 = 2;

elseif ((i>numRows/2)&(counter1<5))

fprintf(50%%..)

counter1 = 5;

elseif((i>numRows*.8)&(counter1<8))

fprintf(80%%..\n)

70

counter1 = 8;

end

end

end

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

function [Re2p,Rb2e,Rw2b] = ...

calcRotationMatrices(psi,theta,phi,bet,aoa,chi,gam)

% Calcuate Rotation Matrices

Rpsi = rotate3(psi);

Rtheta=rotate2(theta);

Rphi=rotate1(phi);

Re2b=Rphi*Rtheta*Rpsi;

Rb2e = Re2b;

Rbet=rotate3(bet);

Raoa=rotate2(aoa);

Rb2w = Raoa*Rbet;

Rb2w = [ cos(aoa)*cos(bet), sin(bet), sin(aoa)*cos(bet); ...

-cos(aoa)*sin(bet),cos(bet),-sin(aoa)*sin(bet);...

-sin(aoa),0,cos(aoa)];

Rw2b = Rb2w;

Rchi = rotate3(chi);

Rgam = rotate2(gam);

Re2p = Rgam*Rchi;

end

function rotMat3=rotate3(ang)

rotMat3 = [cos(ang),sin(ang),0; -sin(ang),cos(ang),0;0,0,1];

end

function rotMat2=rotate2(ang)

rotMat2 = [cos(ang),0,-sin(ang);0,1,0;sin(ang),0,cos(ang)];

end

function rotMat1 = rotate1(ang)

rotMat1 = [1,0,0;0,cos(ang),sin(ang);0,-sin(ang),cos(ang)];

end

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

function genpts(x,y)

% Waypoint File Generation

if size(y)~=size(x)

error(size of x and y differ);

end

fout = fopen(waypoints.dat,w);

71

for k=1:length(x);

fprintf(fout, %f %f\n,x(k),y(k));

end

fclose(fout);

end

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

function d = getpts

% Waypoint file grabbing

fin = fopen(waypoints.dat,r);

d = fscanf(fin,%f);

fclose(fin);

l = length(d);

d = reshape(d,[2 (l/2)]);

end

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

function writepts(d)

% Write to waypoint file

fout = fopen(waypoints.dat,w);

ld=length(d);

for k=1:ld;

fprintf(fout, %f %f\n,d(1,k),d(2,k));

end

fclose(fout);

end

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

% Waypoint Generation Script:

x1=4*[0 200

400 400 200 200];

y1=4*[0 0 0 200 200 0]

genpts(x1,y1);

fprintf(Waypoint Pairs Generated: %i.\n,length(x1));

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

72

Appendix C

C Code

This appendix contains the C code used to program the flight computer for the

Hardware in the Loop simulations. The code was compiled and loaded on the the

MSP430F149 using the IAR Embedded Workbench IDE.

// HIL Simulation Code

#include<msp430x14x.h>

#include<stdlib.h>

#include<string.h>

#include<stdio.h>

#include<math.h>

#define SIZECHARS 6

// 6 for at least one decimal digit, including neg. numbers

// UDF Prototypes:

void configUART(void);

void configSPI1(void);

void resetSlave(void);

void sendOutput2Ground(void);

void parseGPS(void);

void constructOutput(void);

double deg2rad(double);

double rad2deg(double);

void addADCtoOutput(void);

void itoa(unsigned int n, char s[]);

void clearStr(char st[]);

void configADCwithTimerA(void);

void sendSlaveAnalogPacket(void);

void constructAnalogPacket(void);

void convertFloat2String(float num, char s[]);

void addCommandstoOutput(void);

void controller(void);

float saturate(float num, float max);

void voltageOut(float, int);

73

void actuatePitch(float);

void actuateBank(float);

void delay(int);

void geodecCurrent2currentECEFr(void);

void currentECEFr2localENU(void);

void geodecOrigin2originECEFr(void);

float vectorField(void);

void initWaypoints(void);

int numWypts;

// Global Variables

unsigned int q;

int i;

char c;

int firstRun;

char output[98]; //92;

char chPitchDeg[SIZECHARS];

char chRollDeg[SIZECHARS];

/////// GPS Variables ////////

int GPSreceive;

char sentenceType[5];

char check = F;

char GGAsentence[68];

char VTGsentence[68];

int count = 0;

int it;

// ADC Variables //////

int rollADC=15678;

int pitchADC=333;

int zAxisADC=1;

int pressureAlt;

char s[5];

int jk=0;

int analogReady;

int calibrationZaxis;

int calibrationZarray[5];

// Struct Type Definitions

struct gpsChars

{

char chLatDeg[3];

char chLongDeg[4];

char chLongMin[8];

74

char chLatMin[8];

char chTime[7];

char latDir;

char longDir;

char chTrueCourse[4];

char chGrdSpeed[7];

char chHeightAMSL[8]; //meters above MSL

};

struct gpsData

{

int latitudeDeg;

int longitudeDeg;

float latitudeMin;

float longitudeMin;

float latitude;

float longitude;

float trueCourse;

float grdSpeed;

float heightAMSL;

unsigned long int gpsTime;

};

struct geodecticCoord

{

float altitude; //meters

float lambda;

// RADIANS !!!

float phi;

// RADIANS!!!

};

struct ECEFr

{

float Norm;

float x;

float y;

float z;

};

struct enu

{

float e;

float n;

float u;

};

struct attCommand

{

75

float pitch;

float roll;

};

struct northEastWaypoint

{

float north;

float east;

};

//// More Globals

static char airspeedTest[]={"12.345"};

struct gpsChars c_chars;

struct gpsData current;

struct northEastWaypoint NEwaypoint[15];

int currWpt=0;

struct geodecticCoord geodecOrigin;

struct geodecticCoord geodecCurrent;

struct attCommand toFMA;

struct ECEFr originECEFr;

struct ECEFr currentECEFr;

struct enu localENU;

float PI=3.1415;

float altErrorIntegral=0.0;

float lastAltError=0.0;

float altError;

void main(void)

{

WDTCTL = WDTPW + WDTHOLD; // Stop watchdog

volatile unsigned int i;

geodecOrigin.altitude = 0.0;

geodecOrigin.lambda = 33.5647*PI/180.0; // Hardcode

geodecOrigin.phi = 101.878*PI/180.0;

// Hardcode

initWaypoints();

geodecOrigin2originECEFr();

firstRun=1;

//enable first run boolean

GPSreceive = 0;

analogReady=0;

P1OUT = 0x03;

// P1.0 setup for LED output,

P1DIR |= 0x03;

// Enable P1.0, P1.1 high

76

P1OUT = 0x01;

// Drop P1.1 low for slave reset

for (i = 0xAFF; i > 0; i--);

P1OUT = 0x03;

// Raise P1.1 back high to enable slave

configUART();

configADCwithTimerA();

_EINT();

IE1 |= URXIE0; // enable GPS receiving

configSPI1();

while(1)

{

if (GPSreceive==1)

{

sendOutput2Ground();//TransmitGPS sentence

GPSreceive =0; // reset boolean

}

}

}

__interrupt void usart0_rx (void)

{

if(RXBUF0 == $)

{

check = T;

count = 0;

it=0;

}

else if(check == T)

{

if(it<5)

// Finish filling in sentenceType

{

sentenceType[it]=RXBUF0;

it = it+1;

}

else if((sentenceType[2]==G)|(sentenceType[3]==G)...

|(sentenceType[4]==A))

{ GGAsentence[5+count++] = RXBUF0;

if(RXBUF0 == *)

{

77

check = F;

_BIS_SR(LPM3_EXIT);

}

}

else if((sentenceType[2]==V)|(sentenceType[3]==T)...

|(sentenceType[4]==G))

{

VTGsentence[5+count++] = RXBUF0;

if(RXBUF0==*)

{

it = 0;

P1OUT=0x00; // Turn LED on

parseGPS();

constructOutput();

addADCtoOutput();

controller();

GPSreceive=1;

P1OUT=0x01; // Turn LED off

}

}

}

}

void addADCtoOutput(void)

{

char o;

itoa(rollADC,s);

for(o = 0; o < 5; o++)

{ output[55-o]=s[o];}

itoa(pitchADC,s);

for(o = 0; o < 5; o++)

{ output[61-o]=s[o];}

itoa(zAxisADC,s);

for(o = 0; o < 5; o++)

{ output[67-o]=s[o];}

itoa(pressureAlt,s);

for(o = 0; o < 5; o++)

{ output[94-o]=s[o];}

78

void configUART(void)

{

UCTL0 |= SWRST;

P3SEL |= 0x30;

ME1 |= URXE0 + UTXE0;

UCTL0 |= CHAR;

UTCTL0 |= SSEL0;

UBR00 = 0x03;

UBR10 = 0x00;

UMCTL0 = 0x4A;

UCTL0 &= ~SWRST;

}

// = USART0 TXD/RXD

//Enable USART1 RXD/TXD

//8-bit character

// UCLK = ACLK

// 32k/9600 - 3.41

//

// Modulation

// Set

void resetSlave(void)

{

P1OUT &= ~0x02;

//for (i = 0xFF; i > 0; i--);

P1OUT |=0x02;

}

void constructOutput(void)

{

int o;

// Construct Output for GPS data TX to Grd Station

for(o = 0; o < 2; o++)

{ output[o+2]=c_chars.chLatDeg[o]; }

for(o = 0; o < 7; o++)

{ output[o+5]=c_chars.chLatMin[o]; }

for(o = 0; o < 3; o++)

{ output[o+13]=c_chars.chLongDeg[o];}

for(o = 0; o < 7; o++)

{ output[17+o]=c_chars.chLongMin[o];}

for(o = 0; o < 7; o++)

{output[25+o]=c_chars.chHeightAMSL[o];}

for(o = 0; o < 6; o++)

{ output[33+o]=airspeedTest[o]; }

for(o = 0; o < 3; o++)

{ output[40+o]=c_chars.chTrueCourse[o];}

79

{ output[44+o]=c_chars.chGrdSpeed[o];}

for(o=0; o < 6; o++)

{ output[69+o]=c_chars.chTime[o];}

}

void parseGPS(void)

{

int j;

char ch1;

for(i = 0; i < 3; i++)

{

c_chars.chLatDeg[i] = GGAsentence[13+i];

}

for(i = 0; i < 4; i++)

{

c_chars.chLongDeg[i] = GGAsentence[25+i];

}

for(i = 0; i < 8; i++)

{

c_chars.chLongMin[i] = GGAsentence[28+i];

c_chars.chLatMin[i] = GGAsentence[15+i];

}

for (i =0; i<6; i++)

{

c_chars.chTime[i] = GGAsentence[6+i];

}

i=43; //Beginning of dilution of precision

ch1 = GGAsentence[i];

while (ch1!=,) // Go through Dilution of Precision

{

ch1 = GGAsentence[i++];

}

ch1 = GGAsentence[i++];

j=0;

while(ch1!=,) // Collect True Course

{

c_chars.chHeightAMSL[j] = ch1;

ch1=GGAsentence[i++];

j = j+1;

}

ch1=VTGsentence[i++]; //Skip Comma

80

i = 0;

j = 0;

ch1= VTGsentence[i];

////////

while(ch1!=,) // Go through $GPVTG

{

ch1=VTGsentence[i++]; // Go to next character

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

///////

while(ch1!=,) // Collect True Course

{

c_chars.chTrueCourse[j] = ch1;

ch1=VTGsentence[i++];

j = j+1;

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

///////

while(ch1!=,) // Go through T

{

ch1=VTGsentence[i++];

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

/////

while(ch1!=,) // Collect Magnetic Heading

{

//c_chars.magneticHeading = ch1;

ch1=VTGsentence[i++];

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

///////

while(ch1!=,) // Go through M

{

ch1=VTGsentence[i++];

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

//////

81

{

//Collect here

ch1=VTGsentence[i++];

j = j+1;

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

/////

{

ch1=VTGsentence[i++];

}

ch1=VTGsentence[i++]; //Skip Comma

j=0;

/////

while(ch1!=,) // Collect groundspeed (km/h)

{

c_chars.chGrdSpeed[j]=ch1;

ch1=VTGsentence[i++];

j = j+1;

}

/////

// Add \0 char to signal end of string

c_chars.chLatDeg[2] = \0;

c_chars.chLongDeg[3] = \0;

c_chars.chLatMin[7] = \0;

c_chars.chLongMin[7] = \0;

c_chars.chTime[6] = \0;

c_chars.chTrueCourse[3] = \0;

c_chars.chGrdSpeed[6] = \0;

c_chars.chHeightAMSL[7]=\0;

current.latitudeDeg = atoi(c_chars.chLatDeg);

current.longitudeDeg = atoi(c_chars.chLongDeg);

current.latitudeMin = atof(c_chars.chLatMin);

current.longitudeMin = atof(c_chars.chLongMin);

current.gpsTime = atol(c_chars.chTime);

current.trueCourse = atof(c_chars.chTrueCourse);

current.grdSpeed = atof(c_chars.chGrdSpeed);

current.heightAMSL = atof(c_chars.chHeightAMSL);

current.latitude = current.latitudeDeg + ...

82

(current.latitudeMin/60.0);

current.longitude = current.longitudeDeg + ...

(current.longitudeMin/60.0);

//geodecCurrent.altitude = current.altitude;

geodecCurrent.lambda = deg2rad(current.latitude);

geodecCurrent.phi

= deg2rad(current.longitude);

}

void sendOutput2Ground(void)

{

int outCounter=0;

while (outCounter<sizeof output)

{

while (!(IFG1 & UTXIFG0));

for (i = 2; i > 0; i--); //Short Delay

TXBUF0=output[outCounter];

outCounter=outCounter+1;

}

}

double deg2rad(double degrees)

{

// In Progress...

// Will convert deg to radians

double radians;

radians = degrees*PI/180;

return radians;

}

double rad2deg(double radians)

{

return radians*180/PI;

}

void itoa(unsigned int n, char st[])

{

// Convert int to string for Tx

clearStr(st);

int i;

i = 0;

do {

/* generate digits in reverse order */

st[i++] = n % 10 + 0;

/* get next digit */

83

/* delete it */

}

void clearStr(char st[])

{

int i;

for(i=0;i<5;i++)

st[i]= ;

}

void convertFloat2String(float value, char s[])

{

// Convert float to string

int result;

result = snprintf(s, SIZECHARS, "%f", value);

}

#pragma vector=ADC12_VECTOR

__interrupt void ADC12ISR (void)

{

// ADC Interrupt Service Routine

if(firstRun==1)

{

calibrationZarray[jk]=ADC12MEM3;

jk=jk+1;

if(jk==4)

{

calibrationZaxis = ...

(calibrationZarray[0]+calibrationZarray[1]+...

calibrationZarray[2]+calibrationZarray[3])/4;

itoa(calibrationZaxis,s);

// Tx Cal. packet to Ground:

while (!(IFG1 & UTXIFG0));

TXBUF0=C;

while (!(IFG1 & UTXIFG0));

TXBUF0=,;

while (!(IFG1 & UTXIFG0));

TXBUF0=s[4];

while (!(IFG1 & UTXIFG0));

TXBUF0=s[3];

while (!(IFG1 & UTXIFG0));

TXBUF0=s[2];

while (!(IFG1 & UTXIFG0));

84

TXBUF0=s[1];

while (!(IFG1 & UTXIFG0));

TXBUF0=s[0];

while (!(IFG1 & UTXIFG0));

TXBUF0=*;

while (!(IFG1 & UTXIFG0));

TXBUF0=\r;

while (!(IFG1 & UTXIFG0));

TXBUF0=\n;

jk=0; // reset counter

firstRun=0;

// disable calibration run

}

ADC12CTL0 &= (~ENC); // Toggle ENC bit to allow for next

ADC12CTL0 |= ENC;

// sample (see Users Manual)

}

else

{

pressureAlt = ADC12MEM0;

rollADC = ADC12MEM1;

pitchADC = ADC12MEM2;

zAxisADC = ADC12MEM3;

jk = jk+1;

if(jk == 10)

{

//P1OUT^=0x01;

// For debugging, to check frequency

jk=0;

}

ADC12CTL0 |= ENC;

// sample (see Users Manual)

}

}

void configADCwithTimerA(void)

{

P6SEL |=0xF;

ADC12CTL1 = SHS_1 + SHP + CONSEQ_1;

// TA trig., single seq. conv.

ADC12MCTL0 = SREF_1 + INCH_0 ;

// Channel A0, Vref+

ADC12MCTL1 = INCH_1 ;

85

ADC12MCTL2 = INCH_2 ;

// Channel A2, 3.3V pos. ref

ADC12MCTL3 = INCH_3 + EOS;

// Channel A3, 3.3V pos. ref, end sequence

ADC12IE = 0x08;

// Enable ADC12IFG.3

ADC12CTL0 = SHT0_8 + REF2_5V + REFON + ...

ADC12ON + MSC + ENC; // Config ADC12

TACCTL1 = OUTMOD_4;

// Toggle on EQU1 (TAR = 0)

TACCR0 = 1638;

// 1/2 of sampling rate (in ACLK ticks)

TACTL = TASSEL_1 + MC_1;

// ACLK, up-mode

}

void controller(void)

{

float kProll = 3*0.5;

float kPpitch = 0.025;//0.005;

float kIpitch = 0.003;

float headingError;

int maxBank = 45;

int maxPitch = 10;

geodecCurrent2currentECEFr();

currentECEFr2localENU();

int h_d = 992;

current.trueCourse = deg2rad(current.trueCourse);

float chi_d = vectorField();

headingError = chi_d-current.trueCourse;

if (headingError <-PI)

headingError = headingError +2*PI;

else if (headingError > PI)

headingError = headingError-2*PI;

altError = h_d-current.heightAMSL;

altErrorIntegral += lastAltError;

altErrorIntegral += ((altError-lastAltError)*0.5);

// Save last value in global lastAltError

86

lastAltError = altError;

toFMA.roll = kProll*headingError; // Radians

toFMA.pitch = kPpitch*altError + ...

kIpitch*altErrorIntegral; // Radians

toFMA.roll = saturate(toFMA.roll,deg2rad(maxBank));

toFMA.pitch = saturate(toFMA.pitch,deg2rad(maxPitch));

actuateBank(toFMA.roll);

actuatePitch(toFMA.pitch);

convertFloat2String(rad2deg(toFMA.pitch),chPitchDeg);

convertFloat2String(rad2deg(toFMA.roll),chRollDeg);

addCommandstoOutput();

}

float saturate(float num, float max)

{

if(num>max)

num=max;

else if(num<-max)

num=-max;

return(num);

}

void addCommandstoOutput(void)

{

int o;

for(o = 0; o < 6; o++)

{ output[76+o]=chPitchDeg[o];}

for(o = 0; o < 6; o++)

{ output[83+o]=chRollDeg[o];}

}

void configSPI1(void)

{

P5SEL |= 0x0E;

// P5.1,2,3 SPI option select

P5OUT |= 0xF0;

87

P5DIR |= 0xF0;

//Hook pin 5.4 to SSelect

U1CTL = CHAR + SYNC + MM + SWRST;

// 8-bit, SPI, Master

U1TCTL = CKPL + SSEL1 + STC; // Polarity, SMCLK, 3-wire

U1BR0 = 0x02;

// SPICLK = SMCLK/2

U1BR1 = 0x00;

U1MCTL = 0x00;

ME2 |= USPIE1;

// Module enable

U1CTL &= ~SWRST; // SPI enable

}

{

// Acutate DAC for voltage out

P5OUT=~0x00;

if(channel == 4)

P5OUT = ~0x10;

else if(channel ==5)

P5OUT = ~0x20;

else if(channel ==6)

P5OUT = ~0x40;

else if(channel ==7)

P5OUT = ~(0x80);

q=q+.02;

int vOut;

int MSBout;

vOut = q/3.3*4095;

MSBout = vOut>>8;

TXBUF1 = MSBout;

//BUF1 = 0xFFF-(MSBout<<8);

delay(8);

TXBUF1 = vOut;

delay(4);

P5OUT = ~0x00;

}

void delay(int q)

{

for (q = 3; q > 0; q--);

}

88

{ // Pitch: Positive-4, Negative - 6

float Vmax=3.0;

float Voffset = 1.65;

float VDAC = (Vmax-Voffset)*sin(pitch);

if(VDAC<0)

{

voltageOut(0,4);

voltageOut(-VDAC,6);

}

else

{

voltageOut(0,6);

voltageOut(VDAC,4);

}

}

void actuateBank(float bank)

{

float Vmax=3.0;

float Voffset = 1.65;

// Bank: Positive-5, Negative - 7

float VDAC = (Vmax-Voffset)*sin(bank);

if(VDAC<0)

{

voltageOut(0,5);

voltageOut(-VDAC,7);

}

else

{

voltageOut(0,7);

voltageOut(VDAC,5);

}

}

void geodecCurrent2currentECEFr(void)

{

float a = 6378137.0; // earth semimajor axis in meters

float f = 1/298.257223563; // reciprocal flattening

float e2 = 2*f -f*f; // eccentricity squared

float chi = sqrt(1-e2*(sin(geodecCurrent.lambda))...

*(sin(geodecCurrent.lambda)));

89

cos(geodecCurrent.lambda)*cos(-geodecCurrent.phi);

currentECEFr.y = (a/chi +current.heightAMSL)...

*cos(geodecCurrent.lambda)*sin(-geodecCurrent.phi);

currentECEFr.z = (a*(1-e2)/chi + ...

current.heightAMSL)*sin(geodecCurrent.lambda);

}

void currentECEFr2localENU(void)

{

float phiP = atan2(originECEFr.z,...

sqrt(originECEFr.x*originECEFr.x ...

+ originECEFr.y*originECEFr.y));

float lambda = atan2(originECEFr.y,originECEFr.x);

localENU.e = -sin(lambda)*(currentECEFr.x-originECEFr.x)...

+ cos(lambda)*(currentECEFr.y-originECEFr.y);

localENU.n = -sin(phiP)*cos(lambda)...

*(currentECEFr.x-originECEFr.x)- sin(phiP)...

*sin(lambda)*(currentECEFr.y-originECEFr.y)...

+ cos(phiP)*(currentECEFr.z-originECEFr.z);

localENU.u = cos(phiP)*cos(lambda)*...

(currentECEFr.x-originECEFr.x)+ cos(phiP)*sin(lambda)...

*(currentECEFr.y-originECEFr.y)...

+ sin(phiP)*(currentECEFr.z-originECEFr.z);

}

void geodecOrigin2originECEFr(void)

{

float a = 6378137.0; // earth semimajor axis in meters

float f = 1/298.257223563; // reciprocal flattening

float e2 = 2*f -f*f; // eccentricity squared

float chi = sqrt(1-e2*(sin(geodecOrigin.lambda))...

*(sin(geodecOrigin.lambda)));

originECEFr.x = (a/chi +geodecOrigin.altitude)...

*cos(geodecOrigin.lambda)*cos(-geodecOrigin.phi);

originECEFr.y = (a/chi +geodecOrigin.altitude)...

*cos(geodecOrigin.lambda)*sin(-geodecOrigin.phi);

originECEFr.z = (a*(1-e2)/chi + geodecOrigin.altitude)...

*sin(geodecOrigin.lambda);

}

float vectorField(void)

90

{

// Path Following Subroutine

float p;

float sprime;

float w1x;

float w2x;

float w1y;

float w2y;

float xf;

float epsilon;

float dif;

float xd;

float crossProduct;

int tau = 20;

float xe = 45*PI/180;

int kp = 5;

w1x = NEwaypoint[currWpt].north;

w1y = NEwaypoint[currWpt].east;

w2x = NEwaypoint[currWpt+1].north;

w2y = NEwaypoint[currWpt+1].east;

// Check to see if waypoint passed

sprime = ((localENU.n-w1x)*(w2x-w1x)+(localENU.e-w1y)...

*(w2y-w1y))/((w2x-w1x)*...

(w2x-w1x)+(w2y-w1y)*(w2y-w1y));

if(sprime>0.8)

{

//while(sprime>1.0)

//{

currWpt = currWpt+1;

w1x = NEwaypoint[currWpt].north;

w1y = NEwaypoint[currWpt].east;

w2x = NEwaypoint[currWpt+1].north;

w2y = NEwaypoint[currWpt+1].east;

sprime = ((localENU.n-w1x)*(w2x-w1x)+...

(localENU.e-w1y)*(w2y-w1y))/((w2x-w1x)...

*(w2x-w1x)+(w2y-w1y)*(w2y-w1y));

//}

}

xf = atan2 ( (w2y-w1y),(w2x-w1x) );

epsilon = sqrt ( (localENU.n-sprime*(w2x-w1x)-w1x)*...

(localENU.n-sprime*(w2x-w1x)-w1x)+...

91

(localENU.e-sprime*(w2y-w1y)-w1y)...

*(localENU.e-sprime*(w2y-w1y)-w1y) );

////////////////////////////

// Obtain sign of cross product

crossProduct = (w2x-w1x)*(localENU.e-w1y)-...

(w2y-w1y)*(localENU.n-w1x);

if (crossProduct >= 0.01)

p=1.0;

else

p=-1.0;

///////////////////////////

if ( (epsilon>tau) || (epsilon<-tau))

// Outside Transition Zone

{

xd = xf-p*xe;

dif = xd-current.trueCourse;

if (dif <-PI)

dif = dif +2*PI;

else if (dif > PI)

dif = dif-2*PI;

}

else // Inside transition zone

{

xd = xf-p*xe*(pow((epsilon/tau),kp));

dif = xd-current.trueCourse;

if (dif < -PI)

dif = dif + 2*PI;

if (dif > PI)

dif = dif-2*PI;

}

return xd;

}

void initWaypoints(void)

{

// Square Pattern

NEwaypoint[0].north = 0.0;

NEwaypoint[0].east = 0.0;

NEwaypoint[1].north = 400.0;

92

NEwaypoint[1].east = 0.0;

NEwaypoint[2].north = 400.0;

NEwaypoint[2].east = 400.0;

NEwaypoint[3].north = 0.0;

NEwaypoint[3].east = 400.0;

NEwaypoint[4].north = 0.0;

NEwaypoint[4].east = 0.0;

NEwaypoint[5].north = 400.0;

NEwaypoint[5].east = 0.0;

NEwaypoint[6].north = 400.0;

NEwaypoint[6].east = 400.0;

NEwaypoint[7].north = 0.0;

NEwaypoint[7].east = 400.0;

NEwaypoint[8].north = 0.0;

NEwaypoint[8].east = 0.0;

numWypts = 3;

}

93

PERMISSION TO COPY

In presenting this thesis in partial fulfillment of the requirements for a masters

degree at Texas Tech University or Texas Tech University Health Sciences Center,

I agree that the Library and my major department shall make it freely available

for research purposes. Permission to copy this thesis for scholarly purposes may

be granted by the Director of the Library or my major professor. It is understood

that any copying or publication of this thesis for financial gain shall not be allowed

without my further written permission and that any user may be liable for copyright

infringement.

Agree (Permission is granted.)

Adam Ufford

Student Signature

5/25/2009

Date

Student Signature

Date

- Design and Flight Control of Miniature Air VehicleDiunggah olehSunil Kumar
- USARTL-TR-79-22A Aircraft Crash Survival Design GuideDiunggah olehsbondar
- AIAA-1970-533-510Diunggah olehDaoud Tarek
- Adams 2dof Car TutorialDiunggah olehShashank Katiyar
- Avenues Math PackDiunggah olehBhavik Lodha
- GPS TutorialDiunggah olehNik Aiman
- Sem 6 SyllabusDiunggah olehRangan Srinivasan
- Airplane Upset Recovery by Boeing (Part 2)(1)Diunggah olehwarp9
- gfilippiniDiunggah olehRafael Luque
- Gasdynamics GuidanceDiunggah olehLeiser Hartbeck
- etdDiunggah olehPaul Dupont
- Multi Variable Sliding Mode Control for AUV Steering and DivingDiunggah olehlauralally
- Siemens 840D AlarmsDiunggah olehPaul Lifton
- ORION FE Analysis of FoundationsDiunggah olehdarealboy
- math literature lesson planDiunggah olehapi-302995213
- JOK_2016SIBILSKIKOWALSKI.docxDiunggah olehAnonymous mE6MEje0
- C1 2008 JuneDiunggah olehHashan Godakanda
- L2V4bGlicmlzL2R0bC9kM18xL2FwYWNoZV9tZWRpYS83MjIxDiunggah olehShiva Krishnan
- SchMeiPap05Diunggah olehnavdin
- Alg GraphingDiunggah olehkushwah_770145566
- Suspension DefinitionsDiunggah olehrameshsagapariya
- Exercise 1.docxDiunggah olehAdika Bagaskara
- 2008onlinemathuheduAP AB Version1 Questions 17Diunggah olehteachopensource
- Cartesian Coordinate SystemDiunggah olehtvaprasad
- Kerala Preboard XII QuestionsDiunggah olehApoorva Iyer
- Math Long QuizDiunggah olehLeigh Yah
- pop cycle 1 lesson plan finalDiunggah olehapi-402307836
- math1010 project 1Diunggah olehapi-410590664
- Rev-Ex (2)(Full Permission)Diunggah olehAnonymous AtyZD9DS1m
- EDU_CAT_E_ID1_UF_V5R8Diunggah olehrazvandanut

- Spacecraft Propulsion SystemsDiunggah olehMarcus Skookumchuck Vannini
- Design of a Robust Controller for the VEGA TVCDiunggah olehAdrian Toader
- Adaptive Sliding Mode Spacecraft Attitude ControlDiunggah olehnab05
- Space Environment - Juno SpacecraftDiunggah olehnab05
- Face Recognition System Hidden Markov ModelDiunggah olehnab05
- Evolution in IslamDiunggah olehnab05
- Attitude Control of Satellites With Flexible Appendages_ a Structured H_ Control DesignDiunggah olehnab05
- Cerebral Palsy StrengtheningDiunggah olehnab05
- Fine Pointing of Military SpacecraftDiunggah olehnab05
- Robust Attitude Control Using Mu-synthesis for TheDiunggah olehnab05
- Matlab GUI-Based Simulation Platform Design of Flexible SatelliteDiunggah olehnab05
- Mu-Analysis and Synthesis ToolboxDiunggah olehnab05
- MSc Thesis Comparison and Analysis of Attitude Control Systems of a Satellite Using Reaction Wheel ActuatorsDiunggah olehnab05
- Robust Adaptive Variable Structure Control of a Flexible SpacecraftDiunggah olehnab05
- Robust Attitude Control for Spacecraft Based on Normal Matrix and Pattern SearchDiunggah olehnab05
- Arcile Attitude Control Using Analytical Solutions to Approximations of the Hamilton-Jacobi EquationsDiunggah olehnab05
- Attitude Control and Determination System for DTUsat1Diunggah olehnab05
- Acs TheoryDiunggah olehnab05
- Lecure NotesDiunggah olehnab05
- ESPA Separation SystemDiunggah olehnab05
- Electrical Power SubsystemDiunggah olehnab05
- 37-100001-E Facility Handbook 2016-1-25Diunggah olehnab05
- Introduction to Numerical AnalysisDiunggah olehnab05
- LANDSAT Products Description DocumentDiunggah olehnab05
- Fengyun 2 – Spacecraft & SatellitesDiunggah olehnab05
- MSL Landing Site – MSL – Mars Science LaboratoryDiunggah olehnab05
- Falcon Heavy – RocketsDiunggah olehnab05
- Phd Thesis Nonlinear Trajectory NavigationDiunggah olehnab05

- 78491010 Fault Listing ManualDiunggah olehvalles_1168
- 67-08-121 Underground Utilities Survey Standard.RCN-D12^23434826Diunggah olehsrsureshrajan
- World Port IndexDiunggah olehkylden
- SMS_BOUSS2D.pdfDiunggah olehOentoeng Kartono
- Geoinformatics in applied geomorphology.pdfDiunggah olehmanuel_rayano
- Pre-Processing and Geocoding of ALOS PALSAR Data Over Queensland, AustraliaDiunggah olehVladimirodipostov
- DO 027 S2019stream flow manual.pdfDiunggah olehNowell Penonia
- SGEM 2014 Tiberiu Rus Section 9 Geodesy and Mine SurveyingDiunggah olehBenciu Florin Valentin
- How to Read a Nautical Chart- Course PresentationDiunggah olehapi-26189277
- PropagationDiunggah olehAikit Lim
- Pugh, 2004. Tides, Surges and Mean Sea-LevelDiunggah olehAndreea Savu
- 3 - Terrain DataDiunggah olehLenin Federico Ruiz Lagos
- Remotesensing 07 01021 v2Diunggah olehMunajat Nursaputra
- Ernstsen_et_al_2010_RCEM_Tide-controlledvariationsof.pdfDiunggah olehAisyah Ekanofani
- Wspro ManualDiunggah olehYam Balaoing
- GN.pdfDiunggah olehSR Monicasree
- TNC 430 HeidenhainDiunggah olehCao Thanh
- Geographic Coordinate SystemDiunggah olehBertrand Tchuente
- J-SHIS File format rulesDiunggah olehDelgersaihan Tuya
- 2015ValdezHarborPCS.pdfDiunggah olehmizonex
- ICAO Annex 15.pdfDiunggah olehPhuong Trang
- Geologic Mapping n DocumentationDiunggah olehAndyra Jaiz Baddu
- Chap 3 Geographic Coordinates Systems and practical use in Rwanda.pdfDiunggah olehBizimenyera Zenza Theoneste
- 2 Plateia 2011 ENG LayoutDiunggah olehMadalina Cercel
- Sms Procedurs Important READMEDiunggah olehjskourtis
- Heidenhain FK-Programming TNC 530iDiunggah olehtonybullough
- Skysafe BrochureDiunggah olehAnonymous gMgeQl1Snd
- 0420 Oil ReportDiunggah olehGreg Jaklewicz
- GII-06 Training MaterialDiunggah olehalaminm77072
- Geodesy and CartographyDiunggah olehGiniux Preez