CALIFORNIA STATE UNIVERSITY FRESNO UNMANNED AERIAL VEHICLE 

PROJECT SENSORS AND VECTOR FUNCTION SPRING 2009 REPORT

DEPARTMENT OF ELECTRICAL AND COMPUTER ENGINEERING

CALIFORNIA STATE UNIVERSITY FRESNO
FRESNO, CALIFORNIA
JONATHAN FLERCHINGER

JULY 2009
 TABLE OF CONTENTS
Section       Page
1. Introduction..............................................................................................................1
2. Sensors Packages......................................................................................................1
1. Proof of Concept Sensors...................................................................................1
2. Demonstration Sensors......................................................................................5
3. Theory of Operation.................................................................................................6
4. The Vector Function.................................................................................................6
1. Frames of Reference and Coordinate Systems...................................................6
2. Calculation of Position.......................................................................................7
3. Calculation of Attitude.......................................................................................9
4. Calculation of Final Gimbal Angles.................................................................10
Appendix............................................................................................................................11
 1. INTRODUCTION

The California State University Fresno Unmanned Aerial Vehicle (CSUF UAV) project is 
a joint venture between the Mechanical and Electrical Engineering departments at CSUF 
and is sponsored as an educational exercise through Edwards Air Force Base (EAFB). 
The project has two phases: Phase 1 involves a remotely piloted UAV that will acquire a 
known Global Positioning System (GPS) target and laser designate it with a targeting 
gimbal. Phase 2 involves making the UAV autonomous, such that the aircraft would fly 
itself without direct human interaction. This paper will discuss the sensors package of the 
aircraft being used in Phase One and the algorithms used to collect the sensor data and 
output the gimbal angles.

2. SENSORS PACKAGES

The Sensors subsystem of the aircraft was broken into two separate systems, a proof­of­
concept (POC) grade system that was integrated from several cheap off the shelf 
integrated circuits (ICs) and a final demonstration grade system purchased as an all in one 
unit. We decided to do a POC primarily to avoid purchasing an expensive system that 
wouldn't suit our needs, but also as an academic exercise to further increase our 
knowledge and understanding of how our final package would work. 

2.1 Proof of Concept Sensors

The POC system comprises a complete attitude, heading, and reference system (AHRS). 
This means that we can expect to know the roll, pitch, and yaw of the aircraft at all times. 
Along with a GPS the entire picture for attitude and position of the aircraft will be 
known.  In addition, the different sensors each have different error characteristics that 
allow us to implement a type of Kalman filter to increase overall system accuracy. The 

1
POC system is composed of: 3 Analog Devices ADXRS401 MEMS gyroscopes. A 
Freescale MMA7260Q 3­axis MEMS accelerometer. 2 VTI Technologies SCP1000­D01 
MEMS barometers. A PNI MicroMag3 3­axis magnetometer. A Locosys LS20031 GPS 
receiver.

The  Analog Devices ADXRS401 is a 
MEMS package that outputs a voltage level 
corresponding to the rotation rate it 
experiences about its normal axis. By 
including 3 of them, mounted orthogonally 
from each other, and integrating the rotation 
rate versus time, we can determine the roll, 
pitch, and yaw of the aircraft. The maximum 
rotation rate the ADXRS401 can detect is 75 
Figure 1: ADXRS401 Gyroscope
°/s. This is barely adequate for our purposes, 
but suitable for a POC system. The  ADXRS401 is a 5V system and outputs a 2.5V 
reference pin for more accurate integration. We use a voltage divider to bring the outputs 
into the 3.3V window that the 10­bit analog to digital converters  (ADC) can accept. This 
corresponds to a minimum resolution of 0.08 °/bit. 

−165∗G
r new = r old 
 512∗SPS 
Equation 1
The instantaneous rotational position is defined by the constants given in Equation 1, 
where rold is the unupdated rotational position and G is the current gyroscope reading in 
bits, and SPS is the number of samples per second.

2
The Freescale MMA7260Q 3­axis accelerometer is a 
MEMS package that outputs a voltage level 
corresponding to the accelerations it is experiencing 
on each of its orthogonal axes. It can be configured to 
measure a maximum acceleration of 1.5g, 2g, 4g, or 
6g. We have configured it to its most sensitive at 1.5g 
max. This corresponds to 800mV/g on the output pins 
with a 3.3V power supply. With the 10­bit ADC that 
we have in our PIC32 our resolution is 4mg. The  Figure 2: MMA7260Q 3­axis  
primary function of the accelerometers is to help  accelerometer
correct the pitch and roll data we calculate from the gyroscopes. Accelerometers can also 
be double integrated with respect to time to get short term positioning data for use in an 
extended Kalman filter, but we have not implemented that function.

pitch = sin−1
 xa
∣x a∣∣y a∣∣z a∣  roll = sin−1
 ya

∣x a∣∣y a∣∣z a∣
Equation 3
With equation 3 the static orientation can be calculated from the acceleration values 
along the x, y, and z axis. xa, ya, and za respectively. The units are unimportant as long as 
they are all identical.

 The VTI Technologies SCP1000­D01 barometer 
is a MEMS package that outputs pressure and 
temperature as 2 words in SPI protocol. The 
absolute pressure is represented by 19­bits and 
each bit corresponds to .25 Pascals. The 
temperature is represented by 14­bits and each 
bit corresponds to .05 °C. We measure the static 

Figure 3: SCP1000 Barometric Sensor
3
air pressure inside the aircraft and on the ground and use the difference to get a more 
accurate above ground level (AGL) altitude than by GPS alone.

[ ]
R
T0 P  −dT ∗ 
h= ∗ 1− dh g  
−dT P0
dh
Equation 4
The altitude of one barometer over another is calculated with Equation 1. The altitude of 
one barometer above another h is a function of the temperature on the ground T0 (K), the 
standard atmospheric temperature gradient dT/dh (K/m), the pressures on the plane P 
(Pa), and the ground P0  (Pa), the R value of air (m2/s2/K), and gravity (m/s2). Substituting 
standard values for dT/dh and R gives:

[ ]
0.19
P
  h = T 0∗153.8∗ 1− P
0

Equation 5

The PNI MicroMag3 3­axis magnetometer 
is a MEMS package that outputs three 16­
bit SPI words corresponding to the 
magnetic field along the three principle 
axes. It has a maximum reading of 1.1 mT 
and a minimum resolution of 17 nT. The 
primary function of the magnetometer is to 
help correct yaw data from the gyroscopes.

= 2 −atan2 y m , x m  Figure 4: Micromag3 3­Axis Magnetometer
Equation 6
Heading Ɵ as a function of the magnetic reading along the x and y axes of the 
magnetometer. 

4
The Locosys LS20031 is an integrated GPS 
receiver and antenna module. The receiver is a 
MediaTek MT3318 which is L2 band and 
supports up to 32 channels and can output at up 
to 5 Hz. The output format is TTL serial data in 
NMEA format. The receiver supports GPS only 
or SBAS support for increased accuracy via 
DGPS. The output format of the GPS string is 
decimal minutes to four decimal places. This 
gives a resolution in the centimeter range 
Figure 5: LS20031 5Hz GPS Receiver
depending upon latitude. 

2.2 Demonstration Sensors

The demonstration grade AHRS system is a 
self contained unit manufactured by SBG­
Systems. The IG­500n contains its own 
MEMS gyroscopes, accelerometers,, 
barometer, magnetometer, and GPS unit. It 
also contains its own microprocessor to collect 
the inputs, run them through an extended 
Kalman filter, and output its best estimate of 
the aircraft's attitude and position in real time  Figure 6: SBG IG­500n AHRS
at a 100 Hz refresh rate. The IG­500n is capable of 0.5 ° accuracy for pitch and roll, and 
1.0 ° accuracy for yaw, with double floating point precision. The position output accuracy 
is less than 2.0m for latitude, longitude, and altitude. The IG­500n outputs serial RS232 
data with position and attitude represented by double precision floating point numbers. 

5
The barometer is configured to work with standard definition atmospheric pressure 
absolute altitude estimation, or with a ground sample pressure for relative altitude. 
3. THEORY OF OPERATION

The POC system differs from the demonstration grade system in that all sensor inputs are 
collected and then calculations must be performed to determine the attitude of the 
aircraft. The gyroscopic input is integrated to give roll, pitch, and yaw. However, since the 
integration is subject to to increasing error over time, the other sensors are necessary to 
help correct this error. The accelerometers can give accurate roll and pitch if the aircraft 
is not accelerating or rotating. The magnetometer is used to correct the yaw. When used 
together, the gyroscope, accelerometer, and magnetometer give us an approximate 
accuracy of 5 °RMS. The GPS gives reasonable accuracy for latitude and longitude 
(<3.0m), but altitude accuracy is terrible (~10m). The barometer is subject to daily 
atmospheric pressure changes, but when used in tandem with one on the ground it can 
give much better relative altitude accuracy (~1.0m).

4.0 THE VECTOR FUNCTION

The vector function is meant to be a self contained C function within the PIC that takes 
the gathered sensor data and outputs two angles to drive the gimbal to point at the target 
on the ground. The vector function works with whatever data is available to it, from either 
the POC system or the SBG system, and subject to user selectable input masking. The 
outputs of the vector function are 2 signed integer angles, in degrees, between ­180° and 
180°. 

4.1 Frames of Reference and Coordinate Systems

6
 In order to understand what the vector function 
does, it is first necessary to define a coordinate 
convention for the aircraft, the target, and the 
gimbal. Our targeting system works on a line of 
sight basis, at an altitude of less than 1000m, so 
it is useful to consider the ground a flat plane. 
We define the target as the origin of a Cartesian 
coordinate system with East as the positive x­
axis, North as the positive y­axis, and the 
Figure 7: East North Up coordinate 
system positive z­axis is up (ENU coordinate system). 
For the aircraft, we define a Cartesian coordinate system with the origin at the aircraft's 
center of mass. The aircraft's positive x­axis is out the 
nose, the positive y­axis is out the left wing, and the 
positive z­axis is up. All rotational axes are right hand 
rule with roll, pitch, and yaw corresponding to x, y, 
and z, respectively. The gimbal has two degrees of 
freedom, tilt and pan. These correspond directly to phi 
and theta of a spherical coordinate system with the z­
axis up, and the rotational plane parallel to the x­y 
Figure 8: Roll, Pitch, and Yaw  
plane of the 
axes
aircraft where a 0° theta is out the nose of the aircraft. 
The distance to target corresponds directly to the 
radius of this spherical coordinate system.

4.2 Calculation of Position

The first task for the vector function is to take the 
GPS coordinates and altitude of the aircraft and target 
Figure 9: Geocentric Coordinate 
System
7
and convert them to a local Cartesian coordinate system. The GPS coordinate system that 
our receivers output is actually the WGS84 standard that models the earth as a geode. We 
convert the geodetic coordinate system to a geocentric coordinate system that models the 
Earth in a Cartesian coordinate frame with the center of the Earth as the origin with the 
z­axis through the north pole, the x­axis through the equator at 0° longitude and the y­
axis through 90° E longitude. This conversion is done to account for the eccentricity of 
the earth as an ellipsoid. We then take a standard rotational matrix to convert those 
coordinates to a local Earth level system with the origin at the target. These conversions 
lose less than a millimeter accuracy when done with double precision floating point 
numbers and give us a much more convenient reference frame with which to work.

a
N=
1−e 2 sin2 
2  a −b 
2 2
e = a=6378137mb=6356752.3142m
a2
x=N h cos cos 
y= Nh cos sin 
z=N 1−e 2h
Equation 1
Equation 1 shows the calculations necessary to transform the geodetic coordinate system 
(latitude and longitude) to the geocentric coordinate system. ɸ is the latitude, λ is 
longitude, h is the altitude above mean sea level. The semimajor axis of the earth a, along 
with the semiminor axis is used to define the eccentricity of the earth's ellipsoid e. This 
calculation needs to be performed twice, for the aircraft and the target.

8
 [ ]
−sin  cos  0
M = −sin cos  −sin sin cos 
cos  cos  cos sin  sin 

[ ] []
x p −x 0 x
P= y p− y 0 y = M⋅P
z p −z 0 z
Equation 2
Equation 2 shows the rotation matrix used to make the final calculation to a local earth 
level coordinate system. xp, yp, and zp are the aircraft's geocentric coordinates and x0, y0, 
and z0 are the target's geocentric coordinates.  ɸ and  λ are the latitude and longitude of 
the target. The final x, y, and z are the local earth level cartesian coordinates of the plane 
with the target at the origin and x, y, z co­aligned with East North and Up respectively.

4.3 Calculation of Attitude
The second task for the vector function is to determine the aircraft's attitude. Of course 
with the SBG system, the attitude is precomputed, but the POC system outputs are raw 
sensor readings and attitude must be calculated from them. The POC system feeds the 
vector function barometric pressure readings from the aircraft and the basestation, if they 
are available. The barometric readings are then used to calculate an altitude above ground 
level which is an order of magnitude more accurate than the altitude that can be 
calculated by GPS readings alone. The vector function chooses the accelerometer 
corrected roll and pitch from the gyroscopes and the magnetometer corrected yaw from 
the gyroscopes if all those sensors are available. Otherwise it will choose the 
accelerometer for pitch and roll and magnetometer for yaw if the gyroscopes are not 
available. If no sensors data is available with which to calculate the pitch and roll, the 
vector function will assume the plane is level and give an output based upon the position 
and heading, or position only . The vector function can still drive a useful 2D vector with 
just magnetometer and GPS readings.

9
4.4 Calculation of Gimbal Angles

The final task for the vector function is to use the calculated 3D position and attitude 
information to actually calculate the spherical coordinate vector to point back from the 
gimbal at the target. The only way to do this is with a matrix transforms method that takes 
all of the rotational and position axes into account at the same time.

[ ]
−cos ∗cos 
= atan2 y , x = atan2 z ,   x 2y 2  d = −cos ∗sin 
−sin 
Equation 3
Equation 3 shows how the positional matrix is formed before final gimbal angles are 
calculated. x, y, and z are the local earth level cartesian coordinates for the aircraft. 

[ ]
cos ∗cos  sin ∗sin ∗cos −cos ∗sin  cos ∗sin ∗cos −sin ∗sin 
M = cos ∗sin  sin ∗sin ∗sin cos ∗cos  cos ∗sin ∗sin −sin ∗cos 
−sin  sin ∗cos  cos ∗cos 
Equation 4
Equation 4 shows how the rotational matrix is populated. α, β, and Ɣ correspond to the 
aircraft's roll, pitch, and yaw, respectively.

A = M −1⋅d  =−atan2 −A2 ,− A1   = atan2  A3, A1 /cos−

Equation 5
Equation 5 shows how the final gimbal angles are populated. The rotational matrix M is 
inverted A is the dot product of M inverse and d, the positional matrix. Θ and Φ 
correspond to the pan and tilt axes of the gimbal, respectively.

10
 APPENDIX A: 
Vector Function and Related Code
// GPS type struct
typedef struct _GPS
{
unsigned long UTC;
struct _Lat
{
double decimal;
unsigned char deg;
unsigned char min;
unsigned short sec;
unsigned char dir; // usually either 'N' or 'S'
} Lat;
struct _Long
{
double decimal;
unsigned char deg;
unsigned char min;
unsigned short sec;
unsigned char dir; // usually either 'E' or 'W'
} Long;

struct _Time
{
unsigned char hour;
unsigned char min;
unsigned short sec;
unsigned short ms;
} Time;
unsigned char fix;
   unsigned char sat;
int altitude;   // 551 = 55.1 meters
unsigned short velocity; // 
int heading;  //

} GPS_type;

typedef struct _Gimbal
{
int bSetTiltStatic;
int bSetPanStatic;
int theta;
int phi;
int DTT;
int x;
int y;
int z;

11
}Gimbal_type;

typedef struct _SBG
{
struct _GPS_
{
unsigned int time;
unsigned char fix; // 4 states: none, time only, 2D fix, 3D fix
unsigned char sats;
double altitude;
double Lat;
double Long;
} GPS;

float roll;
float pitch;
float yaw;
} SBG_type;

#include "utils.h"
#include "main.h"
#include "math.h"
#include "stdlib.h"
#include "stdio.h"

int quadrant(double angle) //given an angle in radians, this function returns the quadrant
{
int degangle = (int)(angle * _180_div_PI) % 360;
if ((degangle >= 0) && (degangle < 90)) return 1;
if ((degangle >= 90) && (degangle < 180)) return 2;
if ((degangle >= 180) && (degangle < 270)) return 3;
else return 4;
// return (int(((int(angle*180/3.1415926))%360)/90)+1); //should give the same value, in one line.
}

void invert(double a[3],double b[3],double c[3], double X[3][3])
{
int i, j;
double x, n=0.0, A[3][3], B[3][3], C[3][3];
A[0][0] = a[0]; 
A[0][1] = a[1];
A[0][2] = a[2];
A[1][0] = b[0]; 
A[1][1] = b[1];
A[1][2] = b[2];
A[2][0] = c[0]; 
A[2][1] = c[1];

12
 A[2][2] = c[2];
for(i=0,j=0;j<3;j++)
{     
if(j==2) n+=A[i][j]*A[i+1][0]*A[i+2][1];
else if(j==1) n+=A[i][j]*A[i+1][j+1]*A[i+2][0];
else n+=A[i][j]*A[i+1][j+1]*A[i+2][j+2];
}
for(i=2,j=0;j<3;j++)
{     
if(j==2) n­=A[i][j]*A[i­1][0]*A[i­2][1];
else if(j==1) n­=A[i][j]*A[i­1][j+1]*A[i­2][0];
else n­=A[i][j]*A[i­1][j+1]*A[i­2][j+2];
}//n now holds det(A);

//n=A[0][0]*(A[1][1]*A[2][2]­A[1][2]*A[2][1])­A[1][0]*(A[0][1]*A[2][2]­A[0][2]*A[2][1])
+A[2][0]*(A[0][1]*A[1][2]­A[0][2]*A[2][1]);
if (n!=0) x = 1.0/n;
else x = 99999999999.0; //if we are passed a singular matrix, return results that don't make sense, 
not crash
for(i=0;i<3;i++) for(j=0;j<3;j++) B[i][j]=A[j][i];//B now holds transpose(A)
C[0][0]=B[1][1]*B[2][2]­(B[2][1]*B[1][2]);
C[0][1]=(­1)*(B[1][0]*B[2][2]­(B[2][0]*B[1][2]));
C[0][2]=B[1][0]*B[2][1]­(B[2][0]*B[1][1]);

C[1][0]=(­1)*(B[0][1]*B[2][2]­B[2][1]*B[0][2]);
C[1][1]=B[0][0]*B[2][2]­B[2][0]*B[0][2];
C[1][2]=(­1)*(B[0][0]*B[2][1]­B[2][0]*B[0][1]);

C[2][0]=B[0][1]*B[1][2]­B[1][1]*B[0][2];
C[2][1]=(­1)*(B[0][0]*B[1][2]­B[1][0]*B[0][2]);
C[2][2]=B[0][0]*B[1][1]­B[1][0]*B[0][1];//C now holds adjunct(B)
for(i=0;i<3;i++) for(j=0;j<3;j++) X[j][i]=C[i][j]*x; //X now holds A^­1

Gimbal_type Vector()
{
// This is the Vector.
Gimbal_type vector; // create 
Gimbal_type entity. (this function returns this type)

// If the GPS sensor is selected its data will be used in the Vector calculations
// if the GPS unit is disabled in this function we will not calculate anything. The gimbal will be set 
to a control 
// position and DTT set to 0.

if (!(Sensor_Enable & SENSOR_ENABLE_GPS) && !(Sensor_Enable & 
SENSOR_ENABLE_SBG))
{
vector.theta= 0;
vector.phi= 0;

13
 vector.DTT=0;
return(vector);
}

// ****************************** Calculate the Vector 
*********************************************
// Collect needed data for static calculation.
vector_GPS = current_GPS; // create snapshot of plane's 
GPS for vector calculation.
vector_COMPASS = current_COMPASS; // create snapshot of COMPASS for 
vector calculation.
target_vector_GPS = target_GPS; // create snapshot of target GPS for 
vector calculation.
Gyro_type vector_GYRO = Current_Gyro; // create snapshot of GYRO for vector 
calculation.

double target_temp_altitude;
double pitch=0,roll=0,yaw=0;

/*
//Define the y­axis of the plane as positive off the left wing
pitch = Current_Gyro.Y.position * GYRO_SKEW; //in degrees
//Define the x­axis of the plane as positive out through the plane's nose
roll = Current_Gyro.X.position * GYRO_SKEW;
//Define the x­axis of the plane as positive out through the plane's nose
yaw = Current_Gyro.Z.position * GYRO_SKEW;
*/

// ********WARNING**********  using current_Gyro. instead of vector_Gyro. in the vector 
calcs
// can result in using data from different sample sets. Copy the data then use this static
// set of variables for the calculations. Freddy made the fix.

//Define the y­axis of the plane as positive off the left wing
if (Sensor_Enable & SENSOR_ENABLE_SBG)
{

vector_GPS.Lat.decimal = SBG.GPS.Lat;
vector_GPS.Long.decimal = SBG.GPS.Long;
Current_Altitude = SBG.GPS.altitude;
target_temp_altitude = (double)target_vector_GPS.altitude/10.0;
}
else
{
Current_Gyro.Y.angle = (int)((Current_Gyro.Y.position*­165/
(512*TOGGLES_PER_SEC_T4))%360); //165 is an experimentally determined constant based upon the 
sample rate, I think (Jon)
Current_Gyro.X.angle = (int)((Current_Gyro.X.position*­165/

14
(512*TOGGLES_PER_SEC_T4))%360);
Current_Gyro.Z.angle = (int)((Current_Gyro.Z.position*­165/
(512*TOGGLES_PER_SEC_T4))%360);
if (abs(Current_Gyro.Y.angle) > 180 ) Current_Gyro.Y.angle ­= (Current_Gyro.Y.angle/
(abs(Current_Gyro.Y.angle)))*360; //ensures that the angles are between +­pi
if (abs(Current_Gyro.X.angle) > 180 ) Current_Gyro.X.angle ­= (Current_Gyro.X.angle/
(abs(Current_Gyro.X.angle)))*360; 
if (abs(Current_Gyro.Z.angle) > 180 ) Current_Gyro.Z.angle ­= (Current_Gyro.Z.angle/
(abs(Current_Gyro.Z.angle)))*360; 

if (Sensor_Enable & SENSOR_ENABLE_GYRO_Y) pitch = Current_Gyro.Y.angle; //in 
degrees
//Define the x­axis of the plane as positive out through the plane's nose
if (Sensor_Enable & SENSOR_ENABLE_GYRO_X) roll =  Current_Gyro.X.angle;
//Define the z­axis of the plane as positive up from the pilot's position
if (Sensor_Enable & SENSOR_ENABLE_GYRO_Z) yaw =  Current_Gyro.Z.angle;
//prefer the use of the magnetometer for heading, so if it's enabled, use it.
if ((Sensor_Enable & SENSOR_ENABLE_MAG) && !(Sensor_Enable & 
SENSOR_ENABLE_GYRO_Z)) yaw = ­1*vector_COMPASS.heading/100 + 90;

double apitch,aroll;
apitch = SQRT2*_180_div_PI*asin((double)((double)(Current_Accel.X.raw)/((double)
(abs(Current_Accel.X.raw)) + (double)(abs(Current_Accel.Y.raw)) + (double)
(abs(Current_Accel.Z.raw)))));
aroll = SQRT2*_180_div_PI*asin((double)((double)(Current_Accel.Y.raw)/((double)
(abs(Current_Accel.X.raw)) + (double)(abs(Current_Accel.Y.raw)) + (double)
(abs(Current_Accel.Z.raw)))));

if ((Sensor_Enable & SENSOR_ENABLE_ACCEL_Y) && !(Sensor_Enable & 
SENSOR_ENABLE_GYRO_X)) pitch = apitch;
//Define the x­axis of the plane as positive out through the plane's nose
if ((Sensor_Enable & SENSOR_ENABLE_ACCEL_X) && !(Sensor_Enable & 
SENSOR_ENABLE_GYRO_Y)) roll = aroll;

// if Altitude is used in calculations for 3D vector.
if (Sensor_Enable & SENSOR_ENABLE_ALT)
{
// if Altitude ON and Barometer is Altitude source.
if (Sensor_Enable & SENSOR_ENABLE_ALT_BAR)
{
double pressure = current_BAR.pressure + Barometer_Offset; //these 
numbers are actually the pressure x 4, but they
double groundpressure = base_BAR.pressure; //are only used as a 
quotient, so we don't need to modify them
double temperature = current_BAR.temp/20.0 +273.15;
double groundtemp = base_BAR.temp/20.0 +273.15;
Current_Altitude = (groundtemp  / .0065) * (1 ­ 
pow(pressure/groundpressure , 0.19));
target_temp_altitude = 0;

15
 }
// if Altitude is ON and Barometer is OFF then GPS is Altitude source.
else if (Sensor_Enable & SENSOR_ENABLE_ALT_GPS)
{
Current_Altitude = (double)vector_GPS.altitude/10.0;
target_temp_altitude = (double)target_vector_GPS.altitude/10.0;
}
}
// if Altitude not used in vector calculation (2D control).
else
{
Current_Altitude = 0;
target_temp_altitude = 0;
}
// find quadrant (+) ,(­) for East West/ North South
if (vector_GPS.Lat.dir == 'S') vector_GPS.Lat.decimal = (vector_GPS.Lat.decimal)* ­1;
if (vector_GPS.Long.dir == 'W') vector_GPS.Long.decimal = (vector_GPS.Long.decimal)* ­1;
if (target_vector_GPS.Lat.dir == 'S') target_vector_GPS.Lat.decimal = 
(target_vector_GPS.Lat.decimal)* ­1;
if (vector_GPS.Long.dir == 'W') target_vector_GPS.Long.decimal = 
(target_vector_GPS.Long.decimal)* ­1;
}

vector_GPS.Lat.decimal = vector_GPS.Lat.decimal*PI_div_180; //convert gps coords to radians
vector_GPS.Long.decimal = vector_GPS.Long.decimal*PI_div_180;
target_vector_GPS.Lat.decimal = target_vector_GPS.Lat.decimal*PI_div_180;
target_vector_GPS.Long.decimal = target_vector_GPS.Long.decimal*PI_div_180;

double a = 6378137, b = 6356752.3142;
double esquared = (a*a­b*b)/(a*a); //earth constants, the eccentricity squared, approx .0067
double phi = vector_GPS.Lat.decimal; //% latitude in radians assume 90 degrees north of 
plane
double lambda = vector_GPS.Long.decimal; //% lon in radians assume positive to east of 
plane
double h = Current_Altitude; //% in meters assume altitude from mean sea level
double phi0 = target_vector_GPS.Lat.decimal; //% latitude in radians assume 90 degrees 
north of target
double lambda0 = target_vector_GPS.Long.decimal; //% lon in radians assume positive to east of 
target
double h0 = target_temp_altitude; //% in meters altitude of target above mean sea level
double sinphi = sin(phi);     // duh
double cosphi = cos(phi);     // duh
double sinlambda0 = sin(lambda0);
double coslambda0 = cos(lambda0);
double N  = a/(sqrt(1­(esquared*sinphi*sinphi)));   //normalization
double x = (N + h) * cosphi * cos(lambda); 
double y = (N + h) * cosphi * sin(lambda);
double z = (N*(1 ­ esquared) + h) * sinphi; //x, y, and z are now the geocentric coords of the plane
double sinphi0 = sin(phi0);     // duh

16
 double cosphi0 = cos(phi0);     // duh
double N0  = a / sqrt(1 ­ esquared * sinphi0*sinphi0);   //normalization
double x0 = (N0 + h0) * cosphi0 * coslambda0; 
double y0 = (N0 + h0) * cosphi0 * sinlambda0;
double z0 = (N0*(1 ­ esquared) + h0) * sinphi0; //x0, y0, and z0 are now the geocentric coords of 
the target
//Now that both coords are geocentric, a simple matrix rotation will produce local reference frame 
coords
double M[3][3];
M[0][0] = ­sinlambda0;
M[1][0] = coslambda0;
M[2][0] = 0;
M[0][1] = ­sinphi0*coslambda0;
M[1][1] = ­sinphi0*sinlambda0;
M[2][1] = cosphi0;
M[0][2] = cosphi0*coslambda0;
M[1][2] = cosphi0*sinlambda0;
M[2][2] = sinphi0;
double xlocal = (x­x0)*M[0][0]+(y­y0)*M[1][0]+(z­z0)*M[2][0];
double ylocal = (x­x0)*M[0][1]+(y­y0)*M[1][1]+(z­z0)*M[2][1];
double zlocal = (x­x0)*M[0][2]+(y­y0)*M[1][2]+(z­z0)*M[2][2];
double DTT = sqrt(xlocal*xlocal+ylocal*ylocal+zlocal*zlocal);
//begin non rpy method
double theta1 = atan2(ylocal, xlocal);
phi = atan2(zlocal, sqrt(xlocal*xlocal+ ylocal*ylocal)); //changed for rpy calcs

// begin transform method
roll*=PI_div_180;
pitch*=PI_div_180;
yaw*=PI_div_180;
double cosroll = cos(roll), sinroll = sin(roll), cospitch = cos(pitch), sinpitch = sin(pitch), cosyaw = 
cos(yaw), sinyaw = sin(yaw), costheta = cos(theta1), sintheta = sin(theta1);
cosphi = cos(phi);
sinphi = sin(phi);
double am[3],bm[3],c[3], d[3], A[3];
int i,j;
for (i=0;i<3;i++)
{
am[i]=0;
bm[i]=0;
c[i]=0;
d[i]=0;
A[i]=0;
}

c[2] = cosroll*cospitch;
bm[2] = sinroll*cospitch;
am[2] = ­sinpitch;
c[1] = cosroll*sinpitch*sinyaw ­ sinroll*cosyaw;

17
 bm[1]= sinroll*sinpitch*sinyaw + cosroll*cosyaw;
am[1] = cospitch*sinyaw;
c[0] = cosroll*sinpitch*cosyaw ­ sinroll*sinyaw;
bm[0] = sinroll*sinpitch*cosyaw ­ cosroll*sinyaw;
am[0] = cospitch*cosyaw;
d[0] = ­cosphi*costheta;
d[1] = ­cosphi*sintheta;
d[2] = ­sinphi;
//A[0,1,2] = inverse of matrix am,bm,cm * ­d[0,1,2]
double inv[3][3];
invert(am,bm,c,inv);

for (i = 0; i < 3; i++) for (j=0; j<3; j++) A[i]+=inv[i][j]*d[j]*­1.0;

theta1 = atan2(­A[1],­A[0]);
phi = atan2(A[2], (A[0]/cos(theta1­3.1415926)))* _180_div_PI;
// end transform method

theta1*=_180_div_PI;
vector.theta = (int)theta1; // theta becomes an int.

// "flip the sign" adjust for the fact that jon and shawn disagreed on the sign convention for theta
vector.theta = vector.theta*­1; 

// Adjust for heading. Then take the modulus of theta with 360
//Now theta will be between ­360 and +360
//this uses the compass for yaw, we must choose between compass and gyro for this 
reading
//to be decided later
//vector.theta = (vector.theta ­ vector_COMPASS.heading/100 + 90) % 360;
//vector.theta = (vector.theta + (int)(yaw*_180_div_PI)) % 360;  //commented for debug, 
maybe wrong coefficients?
// Adjust quadrant after heading shift, this is current theta for gimbal control 
//We wish theta to be between ­180 and +180, so if it is outside this range, we add or subtract 360
//as appropriate.
if (abs(vector.theta) > 180 ) //with our current matrix transforms, this should never happen
{
vector.theta ­= (vector.theta/(abs(vector.theta)))*360; 
}

// Phi
vector.phi=(int)(phi);

// (DTT) is the magnitude of the vector (in meters) from the plane to the target.
// this value is calculated here and then saved to current_Gimbal.DTT.
vector.DTT= (int)DTT;
vector.x = (int)xlocal;

18
 vector.y = (int)ylocal;
vector.z = (int)zlocal;
// This function returns a type_Gimbal .theta, .phi, .DTT. calculated here.
return vector;

19