Documente Academic
Documente Profesional
Documente Cultură
A
g
sV
Lateral Aircraft Model
K
A
s
Roll Angle
Sensor
Elevator
Deflection
E
-
A of A
Attitude
Sensor
s
1
K
L
Longitudinal Aircraft Model
s
K
E
s
1
V
1
h a
n
g
Normal
Accelerometer
h Altimeter
Flight Path Angle
Pitch Angle
Sensor
h Vertical Velocity
Angle of Attack
3
Figure 3
Roll Angle Control Loop
Figure 4
Pitch Angle Control Loop
4
Figure 5
Simulated Lateral Closed Loop Step Response
Figure 6
Simulated Longitudinal Closed Loop Step Response
5
Figure 7
Measured Closed Loop Step Response
Figure 8
Simulated Closed Loop Step Response
6
Figure 9
Heading Control Loop
Figure 10
Altitude Control Loop
Longitudinal
Stabilization
Lateral
Stabilization
7
Figure 11
Altitude VS Pressure
8
Figure 12
Roll Axis Thermopiles
Figure 13
Pitch Axis Thermopiles
9
Figure 14
PCB Front
Figure 15
PCB Back
10
Figure 16
Dashed Box Denotes Time When Control System Was Engaged
(The Aircraft Was in Manual Control Before and After)
275 280 285 290 295 300 305 310 315 320 325
80
100
120
140
160
180
200
220
240
Thermopile Data
AutoPilot Stability Test
Seconds
Pitch
Roll
11
EMBEDDED C CODE
/*
Autonomous Airplane Control System
Aaron Greer & Nick Smith
University of Utah, 2006-2007
*/
#include <hidef.h> /* for EnableInterrupts macro */
#include <math.h>
#include "derivative.h" /* include peripheral declarations */
#define BYTE unsigned char
#define BOOL unsigned char
#define WORD unsigned short
#define TIMER1_1MS 157
#define TIMER1_100MS 15625
// BAUDxxxx=(MAINCLKFREQ/(16*BAUDRATE))
#define BAUD1200 1041
#define BAUD9600 120
#define initSCI1C2 0x08 // %00001000 ;SCI1 Control Register 2 - RF
/* ||||||||
; |||||||+-SBK ----- not sending break
; ||||||+--RWU ----- receiver wakeup not on
; |||||+---RE ------ receiver disabled
; ||||+----TE ------ transmitter enabled
; |||+-----ILIE ---- idle line interrupt disabled
; ||+------RIE ----- Rx interrupts disabled
; |+-------TCIE ---- Tx complete interrupt disabled
; +--------TIE ----- Tx interrupt disabled (TDRE)
*/
#define initSCI2C2 0x04 // %00000100 ;SCI2 Control Register 2 - GPS
/* ||||||||
; |||||||+-SBK ----- not sending break
; ||||||+--RWU ----- receiver wakeup not on
; |||||+---RE ------ receiver enabled
; ||||+----TE ------ transmitter enabled
; |||+-----ILIE ---- idle line interrupt disabled
; ||+------RIE ----- Rx interrupts disabled
; |+-------TCIE ---- Tx complete interrupt disabled
; +--------TIE ----- Tx interrupt disabled (TDRE)
*/
// TSIP packet control bytes
#define DLE 0x10
#define ETX 0x03
#define GPSPOS 0x4A
#define GPSVEL 0x56
#define GPSHEALTH 0x46
#define GPSDGPSMODE 0x82
#define GPSSTATUS 0x4B
// telemetry packet ids for packets sent to PC
#define RF_ESC 0x0A
#define RF_PRESSURE 0x01
#define RF_ACCEL 0x02
#define RF_IR 0x03
#define RF_SPDHEAD 0x04 // spead and heading over ground
#define RF_LATLON 0x05 // current position data
#define RF_BEARDIST 0x06 // bearing and distance to target
12
#define RF_VEL 0x07 // East and North velocity vectors
#define RF_PRESSURE_HOME 0x08 // baseline pressure
#define RF_LATLON_HOME 0x09 // initial position
// GPS states
#define GPSWAIT 1 // waiting for a packet
#define GPSGOTFIRST 2 // got DLE
#define GPSGOTPOS 3 // got id for Single LLA Position (0x4A)
#define GPSGOTVEL 4 // got id for Single ENU Velocity (0x56)
#define GPSGOTCONFIG 5 // got id for Pos and Vel configuration (0x55)
#define GPSENDPACKET 6 // get DLE,ETX to end the packet
#define GPSSTOP 7 // use this state to park for debugging
#define GPSGOTHEALTH 8 // got the RX health packet
#define GPSGOTDGPSMODE 9 // got the DGPS mode packet
#define GPSGOTSTATUS 10
// radio trim settings
/*
rudder: center=1.58ms right=1.27ms left=1.89ms
elevator: center=1.53ms up=1.2ms down=1.88ms
throttle: closed=1.35ms open=1.84ms
aileron: center=1.48ms right=1.75ms left=1.22ms
*/
// servo PWM values: T=400nsec, 2500*T=1ms, 3750*T=1.5ms, 5000*T=2ms, 50000*T=20ms
// servo range: 2500-5000 = 2500 ticks
#define ONEMS 2500
#define ONEPFIVEMS 3750
#define ONEPSEVENFIVEMS 4375
#define TWOMS 5000
#define TWENTYMS 50000
#define AILERON_MIN 3050 // full left: 1.22ms
#define AILERON_CENTER 3700 // 1.48ms
#define AILERON_MAX 4375 // full right: 1.75ms
#define ELEVATOR_MIN 3000 // full up: 1.2ms
#define ELEVATOR_CENTER 3825 // 1.53ms
#define ELEVATOR_MAX 4700 // full down: 1.88ms
#define THROTTLE_MIN 3375 // full closed: 1.35ms
#define THROTTLE_CENTER 4450 // 4293 == too small // ~75% open: 1.71ms
#define THROTTLE_MAX 4600 // full open: 1.84ms
#define RUDDER_MIN 3175 // full right: 1.27ms
#define RUDDER_CENTER 3950 // 1.58ms
#define RUDDER_MAX 4725 // full left: 1.89ms
#define AILERON_VAL TPM2C0V // PWM compare value regs
#define ELEVATOR_VAL TPM2C1V
#define THROTTLE_VAL TPM2C2V
#define RUDDER_VAL TPM2C3V
// PID control values
#define AILERON_KP 8
#define AILERON_KI 0
#define AILERON_KD 0
#define ELEVATOR_KP 8 // 16== too much , might need to reduce the endpoints
#define ELEVATOR_KI 0
13
#define ELEVATOR_KD 0
// ************ Globals ************
//*
byte gGPSstate;
byte gGPSbytecount;
byte gGPSDLEcount;
struct position {
float Lat;
float Lon;
float Alt;
byte IsValid;
};
struct position gGPSPosition; // hold current position
struct position gGPSPositionTarget; // hold target position
struct position gGPSPositionHome; // initial position of the plane
byte *LatPtr; // use for loading floats from serial stream
byte *LonPtr;
byte *AltPtr;
struct velocity {
float ESpeed;
float NSpeed;
float USpeed;
byte IsValid;
};
struct velocity gGPSVelocity;
byte *ESpeedPtr;
byte *NSpeedPtr;
byte *USpeedPtr;
struct velocity_and_heading {
float Velocity;
float Heading;
byte IsValid;
};
struct velocity_and_heading gGPSVelocityAndHeading;
float gGPSHeadingToTarget;
float gCosLat;
const float gEarthRadiusMeters= 6378137.5275; // radius of the earth in meters
const float gRadToDeg= 180./_M_PI; // converts Radians to Degrees
const float gDegToRad= _M_PI/180.; // converts Degrees to Radians
const float gOneRadDist=6378137.5275; // distance between 1 Rad in meters
//*/
BYTE gALT_STATE;
enum ALTIMETER_STATES {
ALT_WAIT,
ALT_TX1,
ALT_TX2,
ALT_TX3,
ALT_TX4,
ALT_TX5
};
unsigned long gPressure;
14
unsigned long gPressureHome;
BYTE gPressureReadyForTx;
BYTE gATD_state;
enum ATD_STATES{
ATD_WAIT,
ATD_GET_IR1,
ATD_GET_IR2,
ATD_GET_Z,
ATD_GET_X,
ATD_GET_Y
};
WORD gIR1; // decide if these can be 8 bits (12.89mV resolution)
WORD gIR2;
WORD gZ;
WORD gX;
WORD gY;
BYTE gIRReadyForTx;
BYTE gXYZReadyForTx;
const BYTE gDoStabPID = 1; // turn on and off the stablization PID code
const BYTE gDoServos = 1; // turn on and off the servo code
signed short gRollRef = (130<<2); // nice right bank about 15 degrees = (118<<2); //
IR2 reference
signed short gPitchRef = (118<<2); // IR1 reference
// ************ end Globals ************
// ************ function prototypes ************
void ICG_init(void);
void TIMER1_init(void);
void TIMER1_delay_1ms(void);
interrupt Vtpm1ch1 void TIMER1_CH1_isr(void);
void TIMER1_CH1_isr_disable(void);
void TIMER1_CH1_isr_enable(void);
void SCI1_init(void);
void RF_init(void);
BOOL SCI1_HasRxData(void);
BOOL SCI1_ReadyToTx(void);
void SCI1_put(BYTE b);
BYTE SCI1_get(void);
void SCI1_encode_and_transmit(BYTE *Bptr,BYTE numBytes,BYTE dataType);
void SPI_init(void);
void ALTIMETER_init(void);
void ALTIMETER_write_indirect(BYTE address, BYTE data);
void ALTIMETER_write_direct(BYTE address,BYTE data);
WORD ALTIMETER_read_direct(BYTE address, BYTE num_bytes);
WORD ALTIMETER_read_indirect(BYTE address);
void ALTIMETER_select(void);
void ALTIMETER_deselect(void);
unsigned long ALTIMETER_RX_pressure(void);
BOOL SPI_HasRxData(void);
BYTE SPI_get(void);
BOOL SPI_ReadyToTx(void);
void SPI_put(BYTE b);
15
void ATD_init(void);
interrupt Vatd void ATD_isr(void);
void ATD_set_channel(BYTE channel);
void ATD_isr_enable(void);
void ATD_isr_disable(void);
void GPS_init(void);
void SCI2_init(void);
void SCI2_Rx_ISR_disable(void);
void SCI2_Rx_ISR_enable(void);
interrupt Vsci2rx void SCI2RDRFisr(void);
float GPS_GetBearing(float dx,float dy);
float GPS_GetDist(float dx,float dy);
void GPS_GetDxDy(float *dx, float *dy, float lat1, float lon1, float lat2, float
lon2);
float GPS_GetHeading(float ESpeed, float NSpeed);
float GPS_GetSpeed(float ESpeed, float NSpeed);
// ************ end function prototypes ************
// ************ ICG code ************
void ICG_init(void){
//FLL loop multiplier N=8, R=1, P=1 (because in low freq xtal mode)
// 5MHz*P*N/R -> 40MHz (/2 for bus rate) -> 20MHz bus/instruction clock
rate T=50nsec
ICGC2 = ICGC2_MFD1_MASK;
// select FLL engaged external, enable oscillator amplifier
ICGC1 = ICGC1_REFS_MASK|ICGC1_CLKS1_MASK|
ICGC1_CLKS0_MASK|ICGC1_RANGE_MASK|ICGC1_OSCSTEN_MASK;
while(ICGS1_LOCK==0); // loop until FLL locks
}
// ************ end ICG code ************
// ************ TIMER1 code ************
void TIMER1_init(void){
// setup timer 1 module
// use bus rate clock, prescale by 128 T= 50ns * 128 = 6.4us (20,000 ticks
= 1ms)
TPM1SC = TPM1SC_CLKSA_MASK|TPM1SC_PS2_MASK|TPM1SC_PS1_MASK|TPM1SC_PS0_MASK;
// setup timer 1 channel 0
// for output compare (used for delay subroutine)
// output compare mode, software only
TPM1C0SC = TPM1C0SC_MS0A_MASK;
TPM1C0SC_CH0F; // read flag CH0F
TPM1C0SC_CH0F = 0; // to clear flag, write zero to CH0F
// setup timer 1 channel 1
// for output compare with interrupt
// will trigger every 100ms to start a new round of ATD conversions
TPM1C1SC = TPM1C1SC_MS1A_MASK;
TPM1C1SC_CH1F; // read flag CH1F
TPM1C1SC_CH1F = 0; // to clear flag, write zero to CH1F
}
void TIMER1_CH1_isr_enable(void){
TPM1C1SC_CH1IE = 1;
}
void TIMER1_CH1_isr_disable(void){
16
TPM1C1SC_CH1IE = 0;
}
interrupt Vtpm1ch1 void TIMER1_CH1_isr(void){ // start an ATD sequence very 100ms
//PTAD &= ~1; // turn D2 on
TPM1C1SC_CH1F; // read flag CH1F
TPM1C1SC_CH1F = 0; // to clear flag, write zero to CH1F
// setup the new compare value for another 100ms
TPM1C1V = TPM1CNT + TIMER1_100MS;
// start the ATD conversion
ATD_set_channel(0);
ATD_isr_enable();
gATD_state = ATD_GET_IR1;
//PTAD |= 1; // turn D2 off
}
void TIMER1_delay_1ms(void){ // uses timer1 channel 0 for a delay of 1ms
TPM1C0V = TPM1CNT + TIMER1_1MS; // setup compare value for 1ms worth of ticks
while( TPM1C0SC_CH0F==0); // wait 1ms for CH0F to be set
TPM1C0SC_CH0F = 0; // to clear flag, write zero to CH0F
}
// ************ end TIMER1 code ************
// ************ TIMER2/servo code ************
void TIMER2_init(void){
// setup timer 2 module -- use to drive servo signals
// use bus rate clock, prescale by 8, T=400nsec
// servo values: 2500*T=1ms, 3750*T=1.5ms, 5000*T=2ms, 50000*T=20ms
// servo range: 2500-5000 = 2500 ticks
TPM2SC = TPM2SC_CLKSA_MASK|TPM2SC_PS1_MASK|TPM2SC_PS0_MASK;
TPM2MOD = TWENTYMS; // setup modulo for PWM period of 20ms
// setup channels 0-3 to drive servos
TPM2C0SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
TPM2C1SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
TPM2C2SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
TPM2C3SC = TPM2C0SC_MS0B_MASK|TPM2C0SC_ELS0B_MASK;
AILERON_VAL = ONEPFIVEMS; // initial pulse width is 1.5ms (SET TO PROPER
MIDPOINT LATER)
ELEVATOR_VAL = ONEPFIVEMS;
THROTTLE_VAL = ONEPFIVEMS;
RUDDER_VAL = ONEPFIVEMS;
}
// ************ end TIMER2/servo code ************
// ************ SCI1/RF code ************
void RF_init(void){
unsigned short i;
PTGDD |= (1<<6); // make the RF_reset pin an output
PTGD |= (1<<6); // make the RF module reset
SCI1_init();
PTGD &= ~(1<<6); // bring the RF module out of reset
for (i=0;i<60000;i++);
PTGD |= (1<<6); // make the RF module reset again
17
for (i=0;i<60000;i++);
for (i=0;i<60000;i++);
PTGD &= ~(1<<6); // bring the RF module out of reset again
for (i=0;i<60000;i++);
SCI1D = 0; // transmit a dummy byte
while (SCI1S1_TDRE!=1); // wait until serial port is finished txing
for (i=0;i<60000;i++);
for (i=0;i<60000;i++);
}
void SCI1_init(void){
SCI1BD = BAUD1200; // setup the serial port hardware for RF comm
SCI1C2 = initSCI1C2;
}
BOOL SCI1_HasRxData(void){
if (SCI1S1_RDRF==0)
return FALSE;
else
return TRUE;
}
BOOL SCI1_ReadyToTx(void){
if (SCI1S1_TDRE==0)
return FALSE;
else
return TRUE;
}
void SCI1_put(BYTE b){
SCI1D = b;
}
BYTE SCI1_get(void){
return SCI1D;
}
// packet format: {RF_ESC,ID,DATA} (RF_ESC is used to precede any esc values in data)
void SCI1_encode_and_transmit(BYTE *Bptr,BYTE numBytes,BYTE dataType){
while(!SCI1_ReadyToTx()); // wait until ready to TX again
SCI1_put(RF_ESC); // send the start byte
while(!SCI1_ReadyToTx()); // wait until ready to TX again
SCI1_put(dataType); // send the type identifier
while (numBytes>0){ // transmit the data section
while(!SCI1_ReadyToTx()); // wait until ready to TX again
if (*Bptr == RF_ESC){ // check if this data byte is an escape code
SCI1_put(RF_ESC); // precede data byte with an escape
while(!SCI1_ReadyToTx()); // wait until ready to TX again
}
// now transmit the actual data byte (MSB first, big endian)
SCI1_put(*Bptr);
Bptr++; // increment the data pointer
numBytes--;
}
}
// ************ end SCI1/RF code ************
// ************ SPI/KB/Altimeter code ************
void SPI_init(void){
// enable SPI as a master
SPI1C1 = SPI1C1_SPE_MASK|SPI1C1_MSTR_MASK;
//
SPI1C2 = 0;
// SPI baud rate: prescale by 5, divide by 8 -> 20MHz/(5*8) = 500KHz
18
SPI1BR = SPI1BR_SPPR2_MASK|SPI1BR_SPR1_MASK;
// make MOSI and SCK outputs
PTEDD |= (1<<4)|(1<<5);
// make MISO input
PTEDD &= ~(1<<3);
}
BOOL SPI_HasRxData(void){
if (SPI1S_SPRF==0)
return FALSE;
else
return TRUE;
}
BYTE SPI_get(void){
return SPI1D; }
BOOL SPI_ReadyToTx(void){
if (SPI1S_SPTEF==0)
return FALSE;
else
return TRUE;
}
void SPI_put(BYTE b){
SPI1D = b;
}
void SPI_RX_ISR_enable(void){
SPI1C1_SPIE = 1;
}
void SPI_RX_ISR_disable(void){
SPI1C1_SPIE = 0;
}
void SPI_TX_ISR_enable(void){
SPI1C1_SPTIE = 1;
}
void SPI_TX_ISR_disable(void){
SPI1C1_SPTIE = 0;
}
interrupt Vspi void SPI_isr(void){ // should only need to use the RX isr
functionality
SPI1S; // read the RX/TX flags
switch(gALT_STATE) {
case ALT_WAIT:
SPI_RX_ISR_disable(); // should never get here;
SPI_TX_ISR_disable();
break;
case ALT_TX1: // we just txed an address and rxed the first dummy byte
SPI_get(); // read the dummy byte to clear the rx buffer
SPI_put(0x00); // dummy transmit to get data back
gALT_STATE = ALT_TX2;
break;
case ALT_TX2: // we just txed the first dummy byte and read the 8-bit value
ALTIMETER_deselect(); // finished with the first transaction
gPressureReadyForTx = FALSE;
gPressure = (unsigned long)(SPI_get()); // read the 3MSbits of the pressure
value
gPressure <<= 16; // move the MSbits to the top
ALTIMETER_select(); // begin the second transaction for the 2 LSbytes
SPI_put(0x20<<2);
gALT_STATE = ALT_TX3;
break;
case ALT_TX3: // we just txed the address and rxed the first dummy byte
SPI_get(); // read the dummy byte to clear the rx buffer
19
SPI_put(0x00); // dummy transmit to get data back
gALT_STATE = ALT_TX4;
break;
case ALT_TX4: // we just txed the first dummy byte and read the upper of two
LSbytes
gPressure |= ((unsigned long)(SPI_get()<<8)); // read the upper of two LSbytes
SPI_put(0x00); // dummy transmit to get data back
gALT_STATE = ALT_TX5;
break;
case ALT_TX5: // we just txed the second dummy byte and rxed the lower of two
LSbytes
gPressure |= ((unsigned long)(SPI_get())); // read the lower of two LSbytes
ALTIMETER_deselect(); // finished with the second transaction
gPressure >>= 2; // divide by 4 to convert to units of Pascals
gALT_STATE = ALT_WAIT; // (later: go into a state to calculate new outputs)
gPressureReadyForTx = TRUE; // signal to 'main' that a pressure value can be
TXed
SPI_RX_ISR_disable(); // we have the pressure value now, so disable this
isr
break;
default:
gALT_STATE = ALT_WAIT;
break;
} // switch
//PTAD ^= 0xff; // toggle both LEDs
}
void KB_init(void){
PTAPE |= PTAPE_PTAPE5_MASK; // enable internal pull down resistors
KBI1SC = KBI1SC_KBEDG5_MASK|KBI1SC_KBACK_MASK; //rising edge (only) sensitive &
clear int flag
KBI1PE = KBI1PE_KBIPE5_MASK; // enable KB pin (PTA5) for KBI function
}
void KB_ISR_enable(void){
KBI1SC_KBIE=1;
}
void KB_ISR_disable(void){
KBI1SC_KBIE=0;
}
interrupt Vkeyboard void KB_isr(void){
KBI1SC |= KBI1SC_KBACK_MASK; // clear interrupt flag
// do this if SPI is ready and DTRDY is high
if ( SPI_ReadyToTx() && ((PTAD&(1<<5))==(1<<5)) ){
gALT_STATE=ALT_TX1;
ALTIMETER_select();
SPI_put( 0x1F<<2 ); // initiate the pressure transmission with the altimeter
SPI_RX_ISR_enable();
}
}
void ALTIMETER_init(void){
BYTE i,j;
gALT_STATE = ALT_WAIT;
gPressureReadyForTx = FALSE;
SPI_init();
KB_init();
// config the data ready pin as an input
// use a KB interrupt pin to trigger an interrupt
PTFDD &= ~(1<<7); // clear for input
20
// config chip select pin as output
ALTIMETER_deselect(); // de-select the altimeter
PTEDD |= (1<<7);
for (i=0;i<=160;i++) // POR delay of at least 150ms
TIMER1_delay_1ms();
// check startup status
for (i=0;i<7;i++){
if ( !(ALTIMETER_read_direct(0x07,1) & 0x0001) ){
break;
}
for (j=0;j<=160;j++) // delay 10ms
TIMER1_delay_1ms();
}
if (i==7)
while(1); // stop here if startup status is bad
// check EEPROM checksum
if ( !(ALTIMETER_read_direct(0x1F,1) & 0x0001) )
while(1); // stop here for back checksum
// LOW NOISE CONFIGURATION
ALTIMETER_write_indirect(0x2D, 0x03);
for (i=0;i<=110;i++) // delay of at least 100ms
TIMER1_delay_1ms();
ALTIMETER_write_direct(0x03, 0x00); // reset operation mode
for (i=0;i<=10;i++) // delay of at least 10ms
TIMER1_delay_1ms();
// start high resolution continuous sampling
// write 0x0A to OPERATION (0x03)
ALTIMETER_write_direct(0x03, 0x0A);
}
// writes an address to a pointer, and data to a temp,
// then the command to move the data to the reg
void ALTIMETER_write_indirect(BYTE address, BYTE data){
BYTE i;
ALTIMETER_write_direct(0x02, address);
ALTIMETER_write_direct(0x01, data);
ALTIMETER_write_direct(0x03, 0x02);
for (i=0;i<=50;i++) // delay of at least 50ms
TIMER1_delay_1ms();
}
// writes a data value directly to an addressable reg
void ALTIMETER_write_direct(BYTE address,BYTE data){
while( !SPI_ReadyToTx() ); // wait until ready to TX
ALTIMETER_select();
SPI_put( (address<<2)|(1<<1) ); // send address and write command
while (!SPI_HasRxData()); // wait until the first byte (dummy) is read
SPI_get(); // read the dummy byte to clear the rx buffer
while( !SPI_ReadyToTx() ); // wait until ready to TX
SPI_put( data ); // send the data
while (!SPI_HasRxData()); // wait until the second byte (dummy) is read
SPI_get(); // read the dummy byte to clear the rx buffer
ALTIMETER_deselect();
21
}
// directly reads one or two bytes
WORD ALTIMETER_read_direct(BYTE address, BYTE num_bytes){
WORD data=0;
while( !SPI_ReadyToTx() ); // wait until ready to TX
ALTIMETER_select();
SPI_put( address<<2 ); // send address and read command
while (!SPI_HasRxData()); // wait until the first byte (dummy) is read
SPI_get(); // read the dummy byte to clear the rx buffer
while(num_bytes > 0){
num_bytes--;
data <<= 8;
while( !SPI_ReadyToTx() ); // wait until ready to TX
SPI_put(0x00); // dummy transmit to get data back
while (!SPI_HasRxData()); // wait until the first byte (dummy) is read
data |= SPI_get(); // read the data from altimeter
}
ALTIMETER_deselect();
return data;
}
// indirectly read two bytes from DATARD16 (0x20)
WORD ALTIMETER_read_indirect(BYTE address){
BYTE i;
WORD data = 0;
ALTIMETER_write_direct(0x02, address); // setup read pointer
ALTIMETER_write_direct(0x03, 0x01); // command for indirect read
for (i=0;i<=10;i++) // delay of at least 10ms
TIMER1_delay_1ms();
data = ALTIMETER_read_direct(0x20, 0x02); // read two bytes from DATARD16 (0x20)
return data;
}
// clears the SS pin for the altimeter
void ALTIMETER_select(void){
PTED &= ~(1<<7); // select the altimeter
}
// sets the SS pin for the altimeter
void ALTIMETER_deselect(void){
PTED |= (1<<7); // de-select the altimeter
}
// pressure value is fixed point with 2 decimal places ie. divide by 4 for fract value
unsigned long ALTIMETER_RX_pressure(void){
unsigned long pressure=0;
while ( (PTFD & (1<<7)) != (1<<7) ) // wait until pressure data is ready
TIMER1_delay_1ms();
pressure = (unsigned long)(ALTIMETER_read_direct(0x1F,1)&0x07); // read first byte
w/ 3 MSbits
pressure <<= 16; // move MSbits to the top
pressure |= (unsigned long)ALTIMETER_read_direct(0x20,2); // read two LSbytes
pressure >>= 2; // divide by four to make units of Pascals
return pressure;
}
// ************ end SPI code ************
// ************ ATD code ************
void ATD_init(void){
// enabld ATD, right aligned, 10-bit, unsigned, prescale by 10 for 2MHz conversion
clock
22
ATD1C = ATD1C_ATDPU_MASK|ATD1C_DJM_MASK|ATD1C_PRS2_MASK;
// enable PB0 to PB4 for ATD input
ATD1PE = ATD1PE_ATDPE0_MASK|ATD1PE_ATDPE1_MASK|
ATD1PE_ATDPE2_MASK|ATD1PE_ATDPE3_MASK|
ATD1PE_ATDPE4_MASK;
// single conversion, start with CH0
ATD1SC = 0;
gATD_state = ATD_WAIT;
}
interrupt Vatd void ATD_isr(void) {
signed short Error;
WORD aileron_output,elevator_output;
WORD newATDvalue = (ATD1R & 0x03FF); // read 10-bit atd value and clear CCF
//PTAD &= ~1; // turn D2 on
switch(gATD_state){
case ATD_WAIT: // should not get here
ATD_isr_disable();
break;
case ATD_GET_IR1: // should get here from timer1ch0 isr - save value from IR1
gIR1 = newATDvalue;
gIRReadyForTx = FALSE;
gATD_state = ATD_GET_IR2;
if (gDoStabPID==1){ // calculate the new elevator servo output
// (larger PW = more down elevator)
// (larger IR1 = nose is down ???? correct???)
Error = (signed short)gIR1 - gPitchRef ;
elevator_output = ELEVATOR_CENTER + Error*ELEVATOR_KP;
if (elevator_output > /*(ELEVATOR_CENTER+300)*/ ELEVATOR_MAX) {
ELEVATOR_VAL = /*(ELEVATOR_CENTER+300);*/ ELEVATOR_MAX;
} else if (elevator_output < /*(ELEVATOR_CENTER-450)*/ ELEVATOR_MIN){
ELEVATOR_VAL = /*(ELEVATOR_CENTER-450);*/ ELEVATOR_MIN;
} else {
ELEVATOR_VAL = elevator_output;
}
// adjust the throttle servo?
}
ATD_set_channel(1); // start converting channel 1 - IR2
break;
case ATD_GET_IR2: // save value from IR2
gIR2 = newATDvalue;
gATD_state = ATD_GET_Z;
gIRReadyForTx = TRUE;
if (gDoStabPID==1){ // calculate the new aileron servo output
// (larger PW = right bank)
// (larger IR2 = right bank)
Error = (signed short)gIR2 - gRollRef;
aileron_output = AILERON_CENTER + Error*AILERON_KP;
if (aileron_output > /*(AILERON_CENTER+300)*/ AILERON_MAX) {
AILERON_VAL = /*(AILERON_CENTER+300);*/ AILERON_MAX;
} else if (aileron_output < /*(AILERON_CENTER-300)*/ AILERON_MIN){
AILERON_VAL = /*(AILERON_CENTER-300);*/ AILERON_MIN;
} else {
AILERON_VAL = aileron_output;
}
}
ATD_set_channel(2); // start converting channel 2 - Z
break;
23
case ATD_GET_Z: // save value from Z
gZ = newATDvalue;
gXYZReadyForTx = FALSE;
gATD_state = ATD_GET_X;
ATD_set_channel(3); // start converting channel 3 - X
break;
case ATD_GET_X: // save value from X
gX = newATDvalue;
gATD_state = ATD_GET_Y;
ATD_set_channel(4); // start converting channel 4 - Y
break;
case ATD_GET_Y: // save value from Y
gY = newATDvalue;
gATD_state = ATD_WAIT; // (later: go into a state to calculate new
outputs)
gXYZReadyForTx = TRUE;
ATD_isr_disable(); // completed the measuring sequence, disable this isr
break;
default:
gATD_state = ATD_WAIT;
ATD_isr_disable();
break;
}
//PTAD |= 1; // turn D2 off
}
void ATD_isr_enable(void){
ATD1SC_ATDIE = 1;
}
void ATD_isr_disable(void){
ATD1SC_ATDIE = 0;
}
void ATD_set_channel(BYTE channel){
ATD1SC_ATDCH = channel;
}
// ************ end ATD code ************
// ************ GPS/SCI2 code ************
//*
//
void GPS_init(void){
SCI2_init();
gGPSstate=GPSWAIT;
gGPSbytecount=0;
gGPSPosition.IsValid=FALSE;
gGPSVelocity.IsValid=FALSE;
LatPtr=(BYTE *)(&gGPSPosition.Lat);
LonPtr=(BYTE *)(&gGPSPosition.Lon);
AltPtr=(BYTE *)(&gGPSPosition.Alt);
gGPSVelocity.ESpeed=0;
gGPSVelocity.NSpeed=0;
gGPSVelocity.USpeed=0;
ESpeedPtr=(BYTE *)(&gGPSVelocity.ESpeed);
NSpeedPtr=(BYTE *)(&gGPSVelocity.NSpeed);
USpeedPtr=(BYTE *)(&gGPSVelocity.USpeed);
gGPSHeadingToTarget=0;
gGPSVelocityAndHeading.Velocity=0;
gGPSVelocityAndHeading.Heading=0;
gGPSVelocityAndHeading.IsValid=FALSE;
24
}
void SCI2_init(void){
SCI2C1_PE =1; // enable parity
SCI2C1_PT =1; // parity is odd
SCI2BD = BAUD9600; // setup the serial port for com with GPS receiver
SCI2C2 = initSCI2C2;
}
void SCI2_Rx_ISR_enable(void){
SCI2C2_RIE=1; // enable rx isr
}
void SCI2_Rx_ISR_disable(void){
SCI2C2_RIE=0; // disable rx isr
}
// inputs are dx and dy
// output is distance between points in meters
float GPS_GetDist(float dx,float dy) {
float dist= sqrtf(dx*dx + dy*dy); //
return dist;
}
// returns the bearing in degrees from point1 to point2
float GPS_GetBearing(float dx,float dy) {
float Heading= atanf(dx/dy)*gRadToDeg; // * need atan? need to be in degrees?
return Heading;
}
// stores the east-west (dx) and north-south (dy) distance in meters between point1
and point2
void GPS_GetDxDy(float *dx, float *dy, float lat1, float lon1, float lat2, float
lon2){
// cartesian approximation to great sphere formula
// cos scales east-west distance to localized distance between longitude lines
*dx= gOneRadDist*(lon2 - lon1)*gCosLat;//* only do cosf(lat1) once to optimize for
speed
*dy= gOneRadDist*(lat2 - lat1);
}
// returns the speed over the ground in m/s
float GPS_GetSpeed(float ESpeed, float NSpeed){ // absolute ground speed m/s
float velocity=sqrtf(NSpeed*NSpeed + ESpeed*ESpeed);// * keep this squared to avoid
doing sqrt?
return velocity;
}
// returns the heading angle of the airplane in degrees
float GPS_GetHeading(float ESpeed, float NSpeed){
// E/N vel vectors -> heading angle from North degrees
float heading=atanf(ESpeed/NSpeed)*gRadToDeg; // * need atan? need to be in
degrees?
//float heading = atan(ESpeed/NSpeed)*gRadToDeg;
return heading;
}
// receive data from GPS, store it in gData
// setup timer isr counter to detect lost packets??
// (clear the timer after each received byte, if the timer reaches
// some large value, reset the GPSstate to start with a new packet)
// ** need to handle DLE or ETX in data section
interrupt Vsci2rx void SCI2RDRFisr(void){
byte c;
if (SCI2S1_RDRF==1) { // read flag
c=SCI2D; // read value here to clear flag
25
switch(gGPSstate){ // wait until the start byte is received
case GPSWAIT:
if (c==DLE)
gGPSstate=GPSGOTFIRST;
else
gGPSstate=GPSWAIT;
break;
case GPSGOTFIRST: // already got start byte, check id byte now
// don't get new data until old data has been used???
if ((c==GPSPOS) /*&& (gGPSPosition.IsValid==false)*/)
gGPSstate=GPSGOTPOS;
else if ( (c==GPSVEL) /*&& (gGPSVelocity.IsValid==false)*/)
gGPSstate=GPSGOTVEL;
//else if (c==0x55)
// gGPSstate=GPSENDPACKET; //GPSGOTCONFIG;
//else if (c==GPSSTATUS)
// gGPSstate = GPSGOTSTATUS;
//else if (c==GPSHEALTH)
// gGPSstate = GPSGOTHEALTH;
//else if (c==GPSDGPSMODE)
// gGPSstate = GPSGOTDGPSMODE;
else
gGPSstate=GPSENDPACKET;
gGPSbytecount=0;
gGPSDLEcount=0;
break;
/*case GPSGOTDGPSMODE:
gGPSstate =GPSENDPACKET;
c++;
break;
case GPSGOTSTATUS:
gGPSbytecount++;
if (gGPSbytecount>=2){
__RESET_WATCHDOG();
c++; // stop here to look at status byte
gGPSstate =GPSENDPACKET;
}
break;
case GPSGOTHEALTH:
gGPSbytecount++;
if (gGPSbytecount>=2){
__RESET_WATCHDOG();
c++; // stop here to look at health byte
gGPSstate =GPSENDPACKET;
}
break; */
case GPSGOTPOS: // record position packet (build floats from bytes, MSB
first)
/*
if (c == DLE){
gGPSDLEcount++;
if (gGPSDLEcount == 1) // this is the first DLE
break; // skip this byte, wait for the next DLE
(data)
}
//*/
if (gGPSbytecount<4){ // get Lat
LatPtr[gGPSbytecount]=c;
} else if (gGPSbytecount<8){ // get Lon
LonPtr[gGPSbytecount-4]= c;
} else if (gGPSbytecount<12){ // get Alt
AltPtr[gGPSbytecount-8]= c;
} else {
26
gGPSPosition.IsValid=TRUE;
gGPSstate=GPSENDPACKET;
}
gGPSbytecount++;
//gGPSDLEcount=0;
break;
case GPSGOTVEL: // record velocity packet (build floats from bytes, MSB
first)
/*
if (c == DLE){
gGPSDLEcount++;
if (gGPSDLEcount == 1) // this is the first DLE
break; // skip this byte, wait for the next DLE
(data)
}
//*/
if (gGPSbytecount<4){ // get East Speed
ESpeedPtr[gGPSbytecount]=c;
} else if (gGPSbytecount<8){ // get North Speed
NSpeedPtr[gGPSbytecount-4]= c;
} else if (gGPSbytecount<12){ // get Up Speed
USpeedPtr[gGPSbytecount-8]= c;
} else {
gGPSVelocity.IsValid=TRUE;
gGPSstate=GPSENDPACKET;
gGPSDLEcount=0;
}
gGPSbytecount++;
//gGPSDLEcount=0;
break;
/*case GPSGOTCONFIG: // record io configuration data packet
gGPSstate=GPSENDPACKET;
break; */
case GPSENDPACKET: // get DLE,ETX to end the packet
//gGPSstate=GPSWAIT;
if (c==DLE){
++gGPSDLEcount;
}else {
//check for odd num of DLE followed by ETX -> end of packet
if ( (c==ETX) && (gGPSDLEcount&0x01==0x01) )
gGPSstate=GPSWAIT; // found the end of this packet, go to wait
state
gGPSDLEcount=0; // re-start count if string of DLEs is broken
}
break;
/*case GPSSTOP: // use this state to park for debugging if needed
gGPSstate=GPSWAIT;
break; */
default:
gGPSstate=GPSWAIT; // something is wrong, start over with next packet
break;
} // switch
} // if RDRF
}
//*/
// ************ end GPS code ************
void main(void) {
unsigned long LocalPressure;
27
BYTE IR[2];
BYTE accel[3];
float LocalGPSPosition[2];
float f_temp;
float LocalGPSVelocity[2];
float dx,dy;
BYTE count=0;
__RESET_WATCHDOG(); /* feed the dog before killing it*/
// kill the dog, enable BKND
SOPT = SOPT_BKGDPE_MASK;
ICG_init();
TIMER1_init();
RF_init();
ALTIMETER_init();
ATD_init();
GPS_init();
if (gDoServos==1){
TIMER2_init();
}
PTADD |= (1<<0)|(1<<1); // setup the LEDs to blink
PTAD |= (1<<0)|(1<<1); // turn both LEDs off
//PTAD |= (1<<0); // turn LED1 off
//PTAD &= ~(1<<1); // turn LED2 on
//PTAD &= ~(1<<0); // turn LED1 on
KB_ISR_enable(); // we are ready to receive altimeter data now
TIMER1_CH1_isr_enable(); // start the timer/ATD interrupts now
SCI2_Rx_ISR_enable(); // enable the GPS isr
EnableInterrupts;
// get x many GPS samples then save one as "home", calculate cos(lat), get baseline
altitude
/*
//while(count < 2){
// if (gGPSPosition.IsValid==TRUE){
while(gGPSPosition.IsValid==FALSE);
// count++;
DisableInterrupts;
gGPSPositionHome.Lat = gGPSPosition.Lat;
gGPSPositionHome.Lon = gGPSPosition.Lon;
gGPSPosition.IsValid=FALSE;
EnableInterrupts;
// }
// }
//gCosLat = cosf( gGPSPositionHome.Lat );
LocalGPSPosition[0] = gGPSPositionHome.Lat;
LocalGPSPosition[1] = gGPSPositionHome.Lon;
SCI1_encode_and_transmit((BYTE*)(LocalGPSPosition),8,RF_LATLON_HOME);
while (gPressureReadyForTx==FALSE);
DisableInterrupts;
gPressureHome = gPressure;
gPressureReadyForTx = FALSE;
EnableInterrupts;
28
while (gPressureReadyForTx==FALSE);
DisableInterrupts;
gPressureHome = gPressure;
gPressureReadyForTx = FALSE;
EnableInterrupts;
gPressureHome >>= 1; // make 16 bits
gPressureHome <<= 2*8; // move LSbytes to MSbytes
SCI1_encode_and_transmit((BYTE*)(&gPressureHome),2,RF_PRESSURE_HOME);
//*/
while(1) {
//PTAD ^= 0xff; // toggle both LEDs
PTAD ^= 2; // toggle D1
// Altimeter
if (gPressureReadyForTx==TRUE){
//PTAD &= ~(1<<0); // turn LED1 on - 8.6ms
DisableInterrupts; // start atomic code
LocalPressure = gPressure; // make a local copy of the pressure value
gPressureReadyForTx = FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the pressure value now
LocalPressure >>= 1; // divide by two for a 16-bit value
LocalPressure <<= 2*8; // move LSbytes to MSbytes
SCI1_encode_and_transmit((BYTE*)(&LocalPressure),2,RF_PRESSURE);
//PTAD |= (1<<0); // turn LED1 off
}
// thermopiles // 4 bytes
if (gIRReadyForTx==TRUE){
DisableInterrupts; // start atomic code
IR[0] = (BYTE)(gIR1>>2); // make a local copy of the IR values
IR[1] = (BYTE)(gIR2>>2);
gIRReadyForTx = FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the IR values now
SCI1_encode_and_transmit((BYTE*)(IR),2,RF_IR);
}
// accelerometer // 5 bytes
if (gXYZReadyForTx==TRUE){
DisableInterrupts; // start atomic code
accel[0] = (BYTE)(gX>>2); // make a local copy of the accel values
accel[1] = (BYTE)(gY>>2);
accel[2] = (BYTE)(gZ>>2);
gXYZReadyForTx = FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the accel values now
SCI1_encode_and_transmit((BYTE*)(accel),3,RF_ACCEL);
}
// GPS - position
if (gGPSPosition.IsValid==TRUE){
//PTAD &= ~1; // turn D2 on
//PTAD ^= 1; // toggle D2
DisableInterrupts; // start atomic code
LocalGPSPosition[0] = gGPSPosition.Lat;// make a local copy of the position
values
LocalGPSPosition[1] = gGPSPosition.Lon;
gGPSPosition.IsValid=FALSE;
EnableInterrupts; // end atomic code
// encode and transmit the position values now
29
SCI1_encode_and_transmit((BYTE*)(LocalGPSPosition),8,RF_LATLON);
/*
GPS_GetDxDy(&dx,&dy,LocalGPSPosition[0],LocalGPSPosition[1],
gGPSPositionHome.Lat,gGPSPositionHome.Lon);
f_temp = GPS_GetDist(dx,dy);
f_temp = GPS_GetBearing(dx,dy);
//*/
//PTAD |= 1; // turn D2 off
}
// GPS - velocity
if (gGPSVelocity.IsValid==TRUE){
PTAD &= ~1; // turn D2 on
//PTAD ^= 1; // toggle D2
DisableInterrupts; // start atomic code
LocalGPSVelocity[0] = gGPSVelocity.ESpeed;// make a local copy of the velocity
LocalGPSVelocity[1] = gGPSVelocity.NSpeed;
gGPSVelocity.IsValid=FALSE;
EnableInterrupts; // end atomic code
// just transmit the velocity vectors for now
SCI1_encode_and_transmit((BYTE*)(LocalGPSVelocity),8,RF_VEL);
/*
// calculate the compass heading and speed of the plane
f_temp = GPS_GetSpeed(
LocalGPSVelocity[0],LocalGPSVelocity[1]);
f_temp = GPS_GetHeading(
LocalGPSVelocity[0],LocalGPSVelocity[1]);
//*/
PTAD |= 1; // turn D2 off
}
} /* loop forever */
}
30
MATLAB CODE
f unct i on var ar gout = Ser i al Gui ( var ar gi n)
%SERI ALGUI M- f i l e f or Ser i al Gui . f i g
% SERI ALGUI , by i t sel f , cr eat es a new SERI ALGUI or r ai ses t he exi st i ng
% si ngl et on*.
%
% H = SERI ALGUI r et ur ns t he handl e t o a new SERI ALGUI or t he handl e t o
% t he exi st i ng si ngl et on*.
%
% SERI ALGUI ( ' Pr oper t y' , ' Val ue' , . . . ) cr eat es a new SERI ALGUI usi ng t he
% gi ven pr oper t y val ue pai r s. Unr ecogni zed pr oper t i es ar e passed vi a
% var ar gi n t o Ser i al Gui _Openi ngFcn. Thi s cal l i ng synt ax pr oduces a
% war ni ng when t her e i s an exi st i ng si ngl et on*.
%
% SERI ALGUI ( ' CALLBACK' ) and SERI ALGUI ( ' CALLBACK' , hObj ect , . . . ) cal l t he
% l ocal f unct i on named CALLBACK i n SERI ALGUI . M wi t h t he gi ven i nput
% ar gument s.
%
% *See GUI Opt i ons on GUI DE' s Tool s menu. Choose " GUI al l ows onl y one
% i nst ance t o r un ( si ngl et on) " .
%
%See al so: GUI DE, GUI DATA, GUI HANDLES
%Edi t t he above t ext t o modi f y t he r esponse t o hel p Ser i al Gui
%Last Modi f i ed by GUI DE v2. 5 12- Mar - 2007 17: 45: 37
%Begi n i ni t i al i zat i on code - DO NOT EDI T
gui _Si ngl et on = 1;
gui _St at e = st r uct ( ' gui _Name' , mf i l ename, . . .
' gui _Si ngl et on' , gui _Si ngl et on, . . .
' gui _Openi ngFcn' , @Ser i al Gui _Openi ngFcn, . . .
' gui _Out put Fcn' , @Ser i al Gui _Out put Fcn, . . .
' gui _Layout Fcn' , [ ] , . . .
' gui _Cal l back' , [ ] ) ;
i f nar gi n && i schar ( var ar gi n{1})
gui _St at e. gui _Cal l back = st r 2f unc( var ar gi n{1}) ;
end
i f nar gout
[ var ar gout {1: nar gout }] = gui _mai nf cn( gui _St at e, var ar gi n{: }) ;
el se
gui _mai nf cn( gui _St at e, var ar gi n{: }) ;
end
%End i ni t i al i zat i on code - DO NOT EDI T
%- - - Execut es j ust bef or e Ser i al Gui i s made vi si bl e.
f unct i on Ser i al Gui _Openi ngFcn( hObj ect , event dat a, handl es, var ar gi n)
%Thi s f unct i on has no out put ar gs, see Out put Fcn.
%hObj ect handl e t o f i gur e
%event dat a r eser ved - t o be def i ned i n a f ut ur e ver si on of MATLAB
%handl es st r uct ur e wi t h handl es and user dat a ( see GUI DATA)
%var ar gi n unr ecogni zed Pr oper t yName/ Pr oper t yVal ue pai r s f r omt he
% command l i ne ( see VARARGI N)
%Choose def aul t command l i ne out put f or Ser i al Gui
handl es. out put = hObj ect ;
%Updat e handl es st r uct ur e
gui dat a( hObj ect , handl es) ;
31
%UI WAI T makes Ser i al Gui wai t f or user r esponse ( see UI RESUME)
%ui wai t ( handl es. f i gur e1) ;
%t hi s r uns when t he st op but t on i s pr essed
f unct i on St opBut t on_Cal l back( hObj ect , event dat a, handl es)
%st op t he ser i al l oop bel ow
set ( handl es. St opBut t on, ' User Dat a' , 1) ;
di sp( ' cl i cked st op' )
%%t hi s r uns when t he st ar t but t on i s pr essed
%f unct i on St ar t But t on_Cal l back( hObj ect , event dat a, handl es)
%set ( handl es. Al t , ' St r i ng' , 1234) ;
%doSer i al =1;
%- - - Out put s f r omt hi s f unct i on ar e r et ur ned t o t he command l i ne.
f unct i on var ar gout = Ser i al Gui _Out put Fcn( hObj ect , event dat a, handl es)
%var ar gout cel l ar r ay f or r et ur ni ng out put ar gs ( see VARARGOUT) ;
%hObj ect handl e t o f i gur e
%event dat a r eser ved - t o be def i ned i n a f ut ur e ver si on of MATLAB
%handl es st r uct ur e wi t h handl es and user dat a ( see GUI DATA)
cl c
set ( handl es. St opBut t on, ' User Dat a' , 0) ;
%%%%%%%%%%%%%%%%%%%%%pr epar e f i l es f or dat a st or age
di r ect or y_name = ui get di r ( ) ;
dat et i me = dat est r ( now) ;
I R_f i l ename = [ di r ect or y_name, ' \ I R_' , dat et i me, ' . csv' ] ;
I R_f i l ename( f i nd( I R_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s f r om
dat e/ t i me
I R_f i l ename
f i d = f open( I R_f i l ename, ' w' ) ;
f cl ose( f i d) ;
XYZ_f i l ename = [ di r ect or y_name, ' \ XYZ_' , dat et i me, ' . csv' ] ;
XYZ_f i l ename( f i nd( XYZ_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s f r om
dat e/ t i me
XYZ_f i l ename
f i d = f open( XYZ_f i l ename, ' w' ) ;
f cl ose( f i d) ;
ALT_f i l ename = [ di r ect or y_name, ' \ ALT_' , dat et i me, ' . csv' ] ;
ALT_f i l ename( f i nd( ALT_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s f r om
dat e/ t i me
ALT_f i l ename
f i d = f open( ALT_f i l ename, ' w' ) ;
f cl ose( f i d) ;
LATLON_f i l ename = [ di r ect or y_name, ' \ LATLON_' , dat et i me, ' . csv' ] ;
LATLON_f i l ename( f i nd( LATLON_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal
char act er s f r omdat e/ t i me
LATLON_f i l ename
f i d = f open( LATLON_f i l ename, ' w' ) ;
f cl ose( f i d) ;
ENVEL_f i l ename = [ di r ect or y_name, ' \ ENVEL_' , dat et i me, ' . csv' ] ;
ENVEL_f i l ename( f i nd( ENVEL_f i l ename( 3: end) ==' : ' ) +2) = ' _' ; %r emove i l l egal char act er s
f r omdat e/ t i me
32
ENVEL_f i l ename
f i d = f open( ENVEL_f i l ename, ' w' ) ;
f cl ose( f i d) ;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
i =0;
whi l e( i <3)
set ( handl es. Count , ' st r i ng' , i )
i =i +1;
pause( 1)
end
%
%pause
%%%%%%% Begi n Ser i al Loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
doSer i al =1;
doSer i al Loop=1;
i f ( doSer i al )
ESC=hex2dec( ' 0A' ) ; %packet I D val ues
I RI D = hex2dec( ' 03' ) ; %%I R1 = 1 byt e, I R2 = 1 byt e
ACCELI D = hex2dec( ' 02' ) ; %%X = 1 byt e, Y = 1 byt e, Z = 1 byt e
ALTI D = hex2dec( ' 01' ) ; %%Al t = 16 bi t i nt eger
%%- - - - - - - - - - - - - - - - i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
HEAD_VELI D = hex2dec( ' 04' ) ; %%vel oci t y = 16 bi t I nt , headi ng = 16 bi t I nt
LAT_LONI D = hex2dec( ' 05' ) ; %%l at t i t ude = 4 byt e f l oat , l ongi t ude = 4 byt e f l oat
BER_DI STI D = hex2dec( ' 06' ) ; %%ber i ng = 16 bi t I nt , di st 2t ar get = 16 bi t I nt
ENVELOCI TY = hex2dec( ' 07' ) ; %East and Nor t h Vel oci t y vect or s, f l oat s
ALT_HOME = hex2dec( ' 08' ) ;
LAT_LON_HOME = hex2dec( ' 09' ) ;
%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
WAI TSTATE=0; %st at es
GOTFI RST=1;
GOTI R=2;
GOTACCEL=3;
GOTALT=4;
%%- - - - - - - - - - - - - - - - - - - i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
GOTHEAD_VEL=5;
GOTLAT_LON=6;
GOTBER_DI ST=7;
GOTENVEL=8;
GOTALT_HOME=9;
GOTLATLON_HOME=10;
%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
st at e=WAI TSTATE;
Ser i al Cl oseAl l ; %cl ose any hangi ng compor t connect i ons
s=Ser i al Connect ( 1, 1200) ; %connect usi ng ( por t , baud)
t i c;
whi l e ( get ( handl es. St opBut t on, ' User Dat a' ) == 0)
swi t ch st at e
case WAI TSTATE
b=Ser i al Recei veByt e( s) ; %get a r aw byt e
i f ( b==ESC)
st at e=GOTFI RST;
end
case GOTFI RST
b=Ser i al Recei veByt e( s) ; %get anot her r aw byt e
i f ( b==I RI D) %r ecei vi ng an I R t her mopi l e packet
33
st at e=GOTI R;
el sei f ( b==ACCELI D) %r ecei vi ng an accel er omet er packet
st at e=GOTACCEL;
el sei f ( b==ALTI D) %r ecei vi ng an al t i t ude packet
st at e=GOTALT;
el sei f ( b==HEAD_VELI D) %r ecei vi ng a Headi ng & Vel oci t y packet
st at e=GOTHEAD_VEL;
el sei f ( b==LAT_LONI D) %r ecei vi ng a Lat t i t ude and Longi t ude packet
st at e=GOTLAT_LON;
el sei f ( b==BER_DI STI D) %r ecei vi ng a Ber i ng and Di st ance packet
st at e=GOTBER_DI ST;
el sei f ( b==ENVELOCI TY) %r ecei vi ng a E N vel oci t y packet
st at e=GOTENVEL;
el sei f ( b==ALT_HOME)
st at e = GOTALT_HOME;
el sei f ( b==LAT_LON_HOME)
st at e = GOTLATLON_HOME;
el se
st at e=WAI TSTATE;
end
case GOTI R %adj ust t hi s t o r emove esc codes i n dat a
st at e=WAI TSTATE;
newf 1=Ser i al Recei veDat aByt e( s, ESC) ; %get I R1
set ( handl es. I R1, ' St r i ng' , newf 1) ;
newf 2=Ser i al Recei veDat aByt e( s, ESC) ; %get I R2
set ( handl es. I R2, ' St r i ng' , newf 2) ;
Ti meSt amp = t oc;
dl mwr i t e( I R_f i l ename, [ newf 1, newf 2, Ti meSt amp] , ' - append' ) ;
case GOTACCEL
st at e=WAI TSTATE;
newf 3=Ser i al Recei veDat aByt e( s, ESC) ; %get X
set ( handl es. Accel X, ' St r i ng' , newf 3) ;
newf 4=Ser i al Recei veDat aByt e( s, ESC) ; %get Y
set ( handl es. Accel Y, ' St r i ng' , newf 4) ;
newf 5=Ser i al Recei veDat aByt e( s, ESC) ; %get Z
set ( handl es. Accel Z, ' St r i ng' , newf 5) ;
Ti meSt amp = t oc;
dl mwr i t e( XYZ_f i l ename, [ newf 3, newf 4, newf 5, Ti meSt amp] , ' - append' ) ;
case GOTALT
st at e=WAI TSTATE;
newf 6=Ser i al Recei ve16bi t I nt ( s, ESC) *2; %get al t i t ude
set ( handl es. Al t , ' St r i ng' , newf 6) ;
Ti meSt amp = t oc;
dl mwr i t e( ALT_f i l ename, [ newf 6, Ti meSt amp] , ' - append' ) ;
case GOTALT_HOME
st at e=WAI TSTATE;
newf 21=Ser i al Recei ve16bi t I nt ( s, ESC) *2; %get home al t i t ude
set ( handl es. Al t _Home, ' St r i ng' , newf 21) ;
case GOTHEAD_VEL %%Tar get headi ng
st at e=WAI TSTATE;
[ newf 7, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. Head, ' St r i ng' , newf 7) ;
el se
set ( handl es. Head, ' St r i ng' , NaN) ;
end
[ newf 8, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
34
set ( handl es. Speed, ' St r i ng' , newf 8) ;
el se
set ( handl es. Speed, ' St r i ng' , NaN) ;
end
case GOTLAT_LON
st at e=WAI TSTATE;
[ newf 9, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 9 = newf 9 * 180 / pi ;
newf 9 = [ f l oor ( newf 9) , ( ( newf 9- f l oor ( newf 9) ) *60) ] ;
set ( handl es. Lat , ' St r i ng' , newf 9) ;
el se
set ( handl es. Lat , ' St r i ng' , NaN) ;
end
[ newf 10, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 10 = newf 10 * 180 / pi ;
newf 10 = [ f l oor ( newf 10) , ( ( newf 10- f l oor ( newf 10) ) *60) ] ;
set ( handl es. Lon, ' St r i ng' , newf 10) ;
el se
set ( handl es. Lon, ' St r i ng' , NaN) ;
end
Ti meSt amp = t oc;
dl mwr i t e( LATLON_f i l ename, [ newf 9, newf 10, Ti meSt amp] , ' -
append' , ' pr eci si on' , ' %. 6f ' ) ;
case GOTLATLON_HOME
st at e=WAI TSTATE;
[ newf 22, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 22 = newf 22 * 180 / pi ;
newf 22 = [ f l oor ( newf 22) , ( ( newf 22- f l oor ( newf 22) ) *60) ] ;
set ( handl es. Lat _Home, ' St r i ng' , newf 22) ;
el se
set ( handl es. Lat _Home, ' St r i ng' , NaN) ;
end
[ newf 23, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
newf 23 = newf 23 * 180 / pi ;
newf 23 = [ f l oor ( newf 23) , ( ( newf 23- f l oor ( newf 23) ) *60) ] ;
set ( handl es. Lon_Home, ' St r i ng' , newf 23) ;
el se
set ( handl es. Lon_Home, ' St r i ng' , NaN) ;
end
case GOTBER_DI ST %
st at e=WAI TSTATE;
[ newf 11, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. THead, ' St r i ng' , newf 11) ;
el se
set ( handl es. THead, ' St r i ng' , NaN) ;
end
[ newf 12, er r or ] = Ser i al Recei ve16bi t I nt ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. Di st , ' St r i ng' , newf 12) ;
el se
set ( handl es. Di st , ' St r i ng' , NaN) ;
end
case GOTENVEL %%E N vel oci t y
st at e=WAI TSTATE;
35
[ newf 13, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. EVel , ' St r i ng' , newf 13) ;
el se
set ( handl es. EVel , ' St r i ng' , NaN) ;
end
[ newf 14, er r or ] = Ser i al Recei veFl oat ( s, ESC) ;
i f ( er r or == 0)
set ( handl es. NVel , ' St r i ng' , newf 14) ;
el se
set ( handl es. NVel , ' St r i ng' , NaN) ;
end
Ti meSt amp = t oc;
dl mwr i t e( ENVEL_f i l ename, [ newf 13, newf 14, Ti meSt amp] , ' -
append' , ' pr eci si on' , ' %. 6f ' ) ;
%%- - - - - - - - - - - - - - - - end i n pr ogr ess- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
ot her wi se
st at e=wai t st at e;
end %swi t ch
set ( handl es. Count , ' st r i ng' , i )
i =i +1;
%Updat e handl es st r uct ur e
gui dat a( hObj ect , handl es) ;
dr awnow;
end %whi l e
Ser i al Cl ose( s)
Ser i al Cl oseAl l
cl ose al l %cl ose t he gui wi ndow
end %doSer i al
%%%%%%%End Ser i al Loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Get def aul t command l i ne out put f r omhandl es st r uct ur e
var ar gout {1} = handl es. out put ;
%- - - I f Enabl e == ' on' , execut es on mouse pr ess i n 5 pi xel bor der .
%- - - Ot her wi se, execut es on mouse pr ess i n 5 pi xel bor der or over St opBut t on.
f unct i on St opBut t on_But t onDownFcn( hObj ect , event dat a, handl es)
%hObj ect handl e t o St opBut t on ( see GCBO)
%event dat a r eser ved - t o be def i ned i n a f ut ur e ver si on of MATLAB
%handl es st r uct ur e wi t h handl es and user dat a ( see GUI DATA)
36
MATLAB GUI