==== Main.c ====

<code c>//UAV parafoil main code
//#define GROUND		//comment\uncomment as you wish
//#define EEPROM		//put these in the makefile
#include "main.h"
//globals
#include "sqrt_density.h"
#ifdef GROUND
	volatile u16 diff;
	volatile u08 counter=0;
#endif
const char config_string[44] PROGMEM={0x10,0xBB,0x03,0x00,0x03,0x03,0x00,0x3D,0xB2,0xCA,0x58,
0x40,0x00,0x00,0x00,0x41,0x40,0x00,0x00,0x40,0xC0,0x00,0x00,0x1E,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x10,0x03};	//new 0xBB packet to configure air filter mode, with floats unchanged
const char comma PROGMEM=',';					//saves repetitivly storing commas
 
signed char EEMEM windeast[254];				//wind data
signed char EEMEM windnorth[254];
 
unsigned char EEMEM flight_status;				//what stage of flight
#ifdef EEPROM
	uint16_t EEMEM I2Crectransit;				//where in I2C EEPROM we go to 24 byte records
	extern volatile u08 I2Cerr;
#endif
unsigned char EEMEM why_cutdown;				//why we cutdown
 
volatile kalman_state our_state;
/*volatile*/ kalman_model our_model;
volatile u08 Kalman_flag;
 
volatile float Heading;
volatile float Target;
volatile u08 Heading_flag;
 
volatile u08 gyro_error=0x00;
volatile u16 temperature;
 
u08 checksum;
 
volatile gps_type Gps;
//ground control globals
#ifdef GROUND
	extern volatile u08 pwm_counter;
	extern volatile u08 pwm_period;
#endif
 
uint8_t mcusr_mirror __attribute__ ((section (".noinit")));
 
void get_mcusr(void) __attribute__((naked)) __attribute__((section(".init3")));
void get_mcusr(void)
{
	mcusr_mirror = MCUSR;
	MCUSR = 0;
	wdt_disable();
}													//avr-libc wdt code
 
void setup()
{
	outb(UCSR0B, BV(RXCIE0)|BV(RXEN0)|BV(TXEN0));	//mega xx8 registers - enable tx and rx, with interrupts on RX only
	outb(UCSR0C, BV(UPM01)|BV(UPM00)|BV(UCSZ01)|BV(UCSZ00));	//setup 8O1
	outb(UBRR0L, BAUDDIV);
	outb(UBRR0H, BAUDDIV>>8);		//end of UART setup
	//ADC setup
	sbi(ADCSR, ADEN);               // enable ADC (turn on ADC power)
	cbi(ADCSR, ADFR);               // default to single sample convert mode
	outb(ADCSR, ((inb(ADCSR) & ~0x07) | 0x06));  // set prescaler 64
	outb(ADMUX, ((inb(ADMUX) & ~0xC0) | (0x03<<6))); // set reference 1.10V
	cbi(ADMUX, ADLAR);              // set to right-adjusted result
 
	//sbi(ADCSR, ADIE);               // enable ADC interrupts
 
	//IO setup
	GYRO_OFF;											//SS to the gyro - pullups
	PORTD|= (1<<2) ;									//pullup to radio cts
	PORTC|= (1<<1) ;									//dead man switch pullup
	DDRB=0b00101110;									//talk to servo and gyro
	DDRC=0b00001100;									//indicator leds
	DDRD=0b11111000;									//cutdown line, SMPS /EN, led, suart tx, ground led
 	SET;												//set software uart to gps
	sbi(PINB,2);										//experimental hack to set the /SS input to 1 ?
 
	//enable the power to the servo
 
	ENABLE_SMPS;
 
	//SPI setup - do this here as the /SS pin is already set as an output
	 SPCR = (1<<MSTR)|(1<<SPE)|(1<<CPHA)|(1<<CPOL);	//F_CPU/4=5MHz, master, CPOL=CPHA=1
	//outb( SPSR, (1<<SPI2X));
 
	//Timer 1 setup
	TCCR1B = (1<<ICNC1)|0x02;	//clock at F_CPU/8, enable noise cancelling on ICP
 
	TCNT1 = 0;		//reset timer1  
 
	// set PWM mode with OCR1A top-count - mode 15 from the datasheet
	sbi(TCCR1A,WGM10);
	sbi(TCCR1A,WGM11);
	sbi(TCCR1B,WGM12);
	sbi(TCCR1B,WGM13);
 
	// set top count value to give correct period PWM
	OCR1A = TOP_COUNT;
 
	// turn on channel B (OC1B) PWM output
	// set OC1B as non-inverted PWM for direct servo connection
	sbi(TCCR1A,COM1B1);
	cbi(TCCR1A,COM1B0);
 
	//Turn on the servo pwm - OCR1B will previously have been at 0 - we've left the SMPS for a while to stabilise
	OCR1B=3*PWM_COUNT_TIME;					//center servo - servo is on OC1B
 
 
	//Kalman setup									//\/MATRIX\/COMMENTS
	our_model.F.tl=1.0;								//propogator
	our_model.F.tr=DELTA_TIME;						
	our_model.F.bl=0.0;
	our_model.F.br=1.0-(DAMPING_CONSTANT*DELTA_TIME);
 
	our_model.B.tl=0.0;								//control input matrix
	our_model.B.tr=(DELTA_TIME*DELTA_TIME*CONTROL_GAIN)/2.0;
	our_model.B.bl=0.0;
	our_model.B.br=DELTA_TIME*CONTROL_GAIN;
 													//measurement input matrix
	our_model.H.tl=0.0;										//set to 1 for a gps update
	our_model.H.tr=0.0;
	our_model.H.bl=0.0;
	our_model.H.br=1.0;										//this is set for a gyro update
 													//model noise covariance matrix
	our_model.Q.tl=(PI/6)*(PI/6)*DELTA_TIME;				//estimated - corresponds to heading change
	our_model.Q.tr=0.0;
	our_model.Q.bl=0.0;
	our_model.Q.br=(PI/4)*(PI/4)*DELTA_TIME;				//estimated - corresponds to rate change
 
	our_model.R.tl=(PI/8)*(PI/8);					//Data covariance
	our_model.R.tr=0.0;
	our_model.R.bl=0.0;
	our_model.R.br=0.003;									//approximate for an MLX90609 - approx 10 LSB
 
	our_state.state.t=0.0;
	our_state.state.b=0.0;
 													//Initialisation
	our_state.P.tl=PI*PI;									//we might be pointing in any direction (~90^2)
	our_state.P.tr=0.0;
	our_state.P.bl=0.0;
	our_state.P.br=(PI/6)*(PI/6);							//rough estimate for turn rate error^2 at release
 
}
 
s16 read_mlx90609(u08 argument)
{
	GYRO_ON;									//SS=0
	SPDR=argument;
	while(!(SPSR & (1<<SPIF)));
	SPDR=0x00;
	while(!(SPSR & (1<<SPIF)));
	SPDR=0x00;
	while(!(SPSR & (1<<SPIF)));
	GYRO_OFF;
	_delay_loop_2((u16)((float)F_CPU*((float)120.0/(float)4000000.0)));	//this wait depends on the revision - 120us is unconservative
	GYRO_ON;
	SPDR=read_melexis;
	while(!(SPSR & (1<<SPIF)));
	SPDR=0x00;
	while(!(SPSR & (1<<SPIF)));
	u08 a=SPDR;
	gyro_error=SPDR&0b01000000;					//internal selftest NOTE, not on latest datasheet, obsolete?
	SPDR=0x00;
	while(!(SPSR & (1<<SPIF)));
	GYRO_OFF;									//all based on the rogallo code
	return ( (a<<7)|(SPDR>>1) ) & 0x07FF;		//11 bit resolution - offset by one bit to the left
}
 
ISR(TIMER1_COMPB_vect)						//fires at the end of the PWM pulse
{
	sei();									//we need to be able to nest UART interrupts in here
	vector measurement_vector;
	static u08 timer;
	static vector control_vector;
	static kalman_state our_state_local;
	static float integral;
	static float target;
	float a;
	control_vector.t=0.0;					//we control one servo, bottom
	if(!timer)
	{
		our_state_local=our_state;			//sets up the correct local state
	}
	if(Heading_flag)
	{
		our_model.H.tl=1.0;
		measurement_vector.t=Heading;	//from the main loop nav code
		target=Target;					//copy over the global variable
		Heading_flag=FALSE;
	}
	else
	{
		our_model.H.tl=0.0;				//setup the model to reflect the data
		measurement_vector.t=0.0;
	}
	measurement_vector.b=(float)(5.23598776/1024.0)*(float)(read_mlx90609(read_rate)-gyro_null);	//in radians, +-300 gyro
	if(gyro_error)
	{
		our_model.H.br=0.0;
		measurement_vector.b=0.0;		//faulty gyro, limp home mode
	}
	else
	{
		our_model.H.br=1.0;
	}
	predict_and_update(&our_state_local,&our_model,&control_vector,&measurement_vector);
	if(our_state_local.state.t>PI)		//keeps us in the +-PI range
	{
		our_state_local.state.t-=2*PI;
	}
	if(our_state_local.state.t<-PI)
	{
		our_state_local.state.t+=2*PI;
	}
	a=our_state_local.state.t-target;
	if(a>PI)						//heading offset
	{
		a-=2*PI;
	}
	if(a<-PI)
	{
		a+=2*PI;
	}
	integral+=a;					//integral term limits
	if(integral>INTEGRAL_LIMIT)
	{
		integral=INTEGRAL_LIMIT;
	}
	if(integral<-INTEGRAL_LIMIT)
	{
		integral=-INTEGRAL_LIMIT;
	}
	if(timer>4)
	{	
		control_vector.b=P_C*a+I_C*integral+D_C*our_state_local.state.b;	//PID controller
		if(control_vector.b>1)	//servo limits
		{
			control_vector.b=1.0;
		}
		if(control_vector.b<-1)
		{
			control_vector.b=-1.0;
		}
		OCR1B=(u16)(PWM_COUNT_TIME*(3.0+control_vector.b));		//pwm = 1.5 +-0.5ms
	}
	else
	{
		timer++;				//we use timer to turn on control after a few seconds
	}
	#ifdef GROUND
	run_ground_control();
	if(GROUND_LED_SET)		//if under ground control, use the recorded pwm as a control input
	{						//( from OC1B as its not subject to interrupts)
		control_vector.b=((1.0/(float)PWM_COUNT_TIME)*(float)(OCR1B))-3.0;//1500us->0,2000->+1,1000->-1
	}
	#endif
	if(!Kalman_flag)			//our global kalman state is unlocked
	{
		our_state=our_state_local;
		temperature=read_mlx90609(read_temp);
	}
}
 
u08 cutdownrulecheck(int time,volatile float * altitude)
{
	if(time<timelimit && *altitude<altitudelimit)
	{
		return 0;
	}
	OCR1B=3*PWM_COUNT_TIME;							//need to center the servo ready for release
	cutter_on;	
	u08 a=8;
	for(;a;a--)										//8 second cutdown
	{
		_delay_10ms(100);
		wdt_reset();
	}
	cutter_off;
	eeprom_write_byte(&flight_status,0x01);			//we have cutdown=decent
	rprintfProgStr(PSTR("CUTDOWN: "));
	#ifdef EEPROM
		eeprom_write_word(&I2Crectransit,time);			//records where the kalman data should start
	#endif
	if(time>=timelimit)								//lets us know what happened - radio will be in ready state after cutdown
	{
		rprintfProgStr(PSTR("Time\r\n"));
		eeprom_write_byte(&why_cutdown,0x01);
  		return 1;
	}
	else
	{
		rprintfProgStr(PSTR("Altitude\r\n"));
		eeprom_write_byte(&why_cutdown,0x02);
		return 2;
	}
}
 
void wiggleservo()
{
	static s08 position;
	position+=(0x04&(position<<2))-2;	//lsb is incr or decr bit
	if(position>=7)
	{
		cbi(position,0);
	}
	OCR1B=(u16) (((float)F_CPU*0.0005/48.0)*(position+18));	//1.5ms +-0.5ms
	if(position<=-6)
	{
		sbi(position,0);
	}
}
 
float getbatteryvoltage()
{
	outb(ADMUX, (inb(ADMUX) & ~0x1F) | (battery_chan & 0x1F));    // set channel
	sbi(ADCSR, ADIF);                       // clear hardware "conversion complete" flag 
	sbi(ADCSR, ADSC);                       // start conversion
	while( bit_is_set(ADCSR, ADSC) );       // wait until conversion complete
 	// CAUTION: MUST READ ADCL BEFORE ADCH!!!
	return battery_factor*((float)(inb(ADCL) | (inb(ADCH)<<8)));    // read ADC (full 10 bits);
}
 
void getgps()
{
	wdt_reset();
	Gps.packetflag=FALSE;					//"Unlock" the global variable, so GPS parsing ISR can update it
	while(!Gps.packetflag);					//wait for some new gps
}
 
void dataprint(u08 flight_status,volatile gps_type * gps)
{
	u08 local_char;
	rprintfProgStr(PSTR("UKHAS"));
	rprintfChar(flight_status);	//>==ascending, <==descending, -==landed
	PRINT_COMMA;
	checksum=0x00;					//reset the checksum
	rprintfFloat(6,(double)gps->latitude);
	PRINT_COMMA;
	if (gps->latitude>0)
	{
		local_char='N';
	}
	else
	{
		local_char='S';
	}
	rprintfChar(local_char);
	PRINT_COMMA;
	rprintfFloat(6,(double)gps->longitude);
	PRINT_COMMA;
	if (gps->longitude>0)
	{
		local_char='E';
	}
	else
	{
		local_char='W';
	}
	rprintfChar(local_char);
	PRINT_COMMA;
	rprintfFloat(1,(double)gps->altitude);
	PRINT_COMMA;
	rprintfChar('M');
	PRINT_COMMA;
	rprintf("%d",(int)gps->nosats);
	PRINT_COMMA;	
	if(flight_status=='<')			//print the kalman data if the control loop is running
	{
		rprintfFloat(1,(double)Heading);
		PRINT_COMMA;
		rprintfFloat(2,(double)Target);
		PRINT_COMMA;
		Kalman_flag=TRUE;			//lock the kalman data
		rprintfFloat(1,(double)our_state.state.t);
		PRINT_COMMA;
		rprintfFloat(1,(double)our_state.state.b);
		Kalman_flag=FALSE;
		PRINT_COMMA;
		#ifdef GROUND				//only print ground control info if ground control is enabled
			rprintf("%x",counter);
			PRINT_COMMA;
		#endif
	}
	rprintfFloat(1,(double)getbatteryvoltage());
	PRINT_COMMA;
	rprintfFloat(1,(double) (0.1953*(float)(temperature-1277)) );//mlx90609 rough temperature conversion
	PRINT_COMMA;
	rprintf("%x",checksum);
	rprintfCRLF();
	return ;								
}
 
void put_char(unsigned char c)
{
	loop_until_bit_is_set(UCSR0A, UDRE0);	//unbuffered tx comms
	UDR0 = c;
	checksum+=c;
}
 
void _delay_10ms(char time)
{
	for(;time;time--)
	{
		_delay_loop_2((u16)((float)F_CPU*0.0025));	//10ms delay - loop uses 4 cycles per iteration   	
	}
}
 
int main()
{
	wdt_enable(WDTO_8S);				//watchdog set to 4 seconds
	#ifdef EEPROM
		u32 I2Caddress;
	#endif
	u16 time;
	int L;
	u08 Heightupto=1;					//this rounds up
	float K;
	float b,descent_variable=0.05882,descent_variable_noise=0.002;
	float descent_drift_n=0;
	float descent_drift_e=0;
	float Wind_e=0.0;
	float Wind_n=0.0;
	setup();							//all the major setup stuff
	#ifdef EEPROM
		init_i2c();	
	#endif								//initialise the i2c bit rate
	//FILE mystdio_usart = FDEV_SETUP_STREAM(put_char, get_char, _FDEV_SETUP_RW);  //so we can printf to the radio
	//stdout= &mystdio_usart;
	rprintfInit(put_char);
	for(L=0;L<44;L++)
	{
		suart_send(pgm_read_byte(&config_string[L]));	//configs gps
	}
	sei();
	if(mcusr_mirror&(1<<WDRF))							//let us know why we are booting up
	rprintfProgStr(PSTR(" Watchdog Timeout\r\n"));
	if(mcusr_mirror&(1<<BORF))
	rprintfProgStr(PSTR(" Brownout triggered\r\n"));
	if(mcusr_mirror&(1<<EXTRF))
	rprintfProgStr(PSTR(" Reset pressed\r\n"));
	if(mcusr_mirror&(1<<PORF))							//this is what should happen - power switched on
	rprintfProgStr(PSTR(" Mini Rogallo by Laurence Blaxter, 2008\r\n"));
	if(USER_PRESENT)						//dead man switch to gnd D.4
	{
	/////////////////////////////Start of mission setup code/loops
 
		while(Gps.status<3)					//wait for a 3D gps lock
		{
			rprintfProgStr(PSTR("Awaiting Lock:"));
			rprintfFloat(7,(double)Gps.latitude);
			rprintfProgStr(PSTR(",N,"));
			rprintfFloat(7,(double)Gps.longitude);
			rprintf(",E,%d",(int)Gps.nosats);
			rprintfProgStr(PSTR("Sats\r\n"));
			getgps();
		}
		wdt_enable(WDTO_4S);
		rprintfProgStr(PSTR("3D GPS Lock, press for wind data\r\n"));
		_delay_10ms(100);								//a 1 second delay
		if(USER_PRESENT)
		{
			rprintfProgStr(PSTR("Cutdown cause:"));		//lets us know why we cutdown
			rprintf("%d",eeprom_read_byte(&why_cutdown));
			rprintfCRLF();
			for (L=0;L<255;L++)
			{
				wdt_reset();
				rprintfProgStr(PSTR("Record no."));
				rprintf("%d:%d,E,",L,(int)(s08)eeprom_read_byte((u08*)&windeast[L]));
				rprintf("%d",(int)(s08)eeprom_read_byte((u08*)&windnorth[L]));
				rprintfProgStr(PSTR(",W\r\n"));
				if((s08)(eeprom_read_byte((u08*)&windeast[L]))!=-126)//paint the eeprom with -126
				{
					eeprom_write_byte((u08*)&windeast[L],(s08)-126);
					eeprom_write_byte((u08*)&windnorth[L],(s08)-126);
				}
			}
		}
		if(eeprom_read_byte(&flight_status))			//set eeprom status to ascending
		{
			eeprom_write_byte(&flight_status,0x00);
		}
		#ifdef GROUND
			rprintfProgStr(PSTR("Test ground control function\r\n"));
			enable_ground_control();
			for(L=0;L<125;L++)							//2.5 second delay
			{
				_delay_10ms(2);							//time to test ground control
				run_ground_control();
				wdt_reset();							//reset watchdog
			}
			disable_ground_control();
		#endif
		for(L=100;L;L--)										
		{
			rprintfProgStr(PSTR("Gyro:"));
			rprintf("%d,",(int)read_mlx90609(read_rate));
			rprintfProgStr(PSTR("Selftest:"));
			rprintf("%d",(int)gyro_error);
			rprintfCRLF();
			wdt_reset();
		}
		#ifdef EEPROM
		rprintfProgStr(PSTR("Press for I2C data\r\n"));
		_delay_10ms(100);								//a 1 second delay
		if(USER_PRESENT)
		{
			rprintfProgStr(PSTR("Testing the I2C..."));	//actually we have already done it
			I2Cerr=0;
			I2Caddress=0;								//eeprom address is setup
			set_address(&I2Caddress);					//go to the first location
			if(!I2Cerr)									//no error						
			{
				rprintfProgStr(PSTR("[OK]\r\n"));		//I2C debug crap
				time=eeprom_read_word(&I2Crectransit);	//reads the transit location into time variable
				for(;I2Caddress<131072;)				//reads out all our data
				{
					read_data((u08*)&K,4,&I2Caddress);	//read out a float
					if(*(u32*)&K==0xFFFFFFFF)			//0xFFFFFFFF
						break;							//quit when we get to painted EEPROM
					rprintfFloat(9,K);					//this will be position fix info / filter and target info
					PRINT_COMMA;
					if(((I2Caddress-1)/12)>=time)			//time is number of records before transition to 24 byte records
						L++;
					else
						L+=2;
					if(L>=6)
					{
						rprintf("%d",I2Cerr);
						rprintfCRLF();
						L=0;
					}
					wdt_reset();
				}
				I2Caddress=0;
			}
			else
			{
				rprintfProgStr(PSTR("[FAIL]:"));
				rprintf("%d",I2Cerr);
				rprintfCRLF();
			}
		}
		rprintfProgStr(PSTR("Press to wipe EEPROM\r\n"));
		_delay_10ms(100);
		wdt_reset();
		if(USER_PRESENT)
		{
			rprintfProgStr(PSTR("Wiping...\r\n"));
			I2Cerr=0;
			painteeprom();
			I2Caddress=0;								//paint the eeprom and send us to the start
			rprintfProgStr(PSTR("Error status:"));
			rprintf("%d",I2Cerr);
			rprintfCRLF();
		}
		wdt_reset();
		#endif
		rprintfProgStr(PSTR("Config done\r\n"));
	}
	else	//---------------------       erranous reset recovery code    -----------------------------------------
	{
		#ifdef EEPROM
			I2Caddress=findtop();				//we need to avoid overwiting old data			
		#endif
		if(eeprom_read_byte(&flight_status)==0x01)	//we are in descent phase - find Heightupto from GPS
		{
			getgps();							//get the gps - hopefully we have a valid altitude
			Heightupto=(int)(Gps.altitude/100.0)+1;			//c rounds down - we need to add one
		}
		else
		{
			for(L=0;((s08)eeprom_read_byte((u08*)&windeast[L])!=-126) && L<255;L++)		//calculate heightupto
			{
				Heightupto++;
			}
		}
		L=0;
		rprintfProgStr(PSTR("Setup complete, EEPROM top="));
		rprintf("%d",I2Caddress/12);
		rprintfCRLF();
	}
	if(!eeprom_read_byte(&flight_status))			//0x00=ascent
	{
	/////////////////////////////Ascent loop
 
		for(time=0;!cutdownrulecheck(time,&(Gps.altitude));time++)
		{
 			Wind_e += Gps.veast;					//add it to our total wind
 			Wind_n += Gps.vnorth;
 			L++;                              		//incrament the denominator for our averaging
	        	if (Gps.altitude > Heightupto*100)		//do we now fall into another (higher) 100m incrament ?
 			{
				Wind_e/=L;
				Wind_n/=L;                           
				eeprom_write_byte((u08*)&windeast[Heightupto],(s08)(Wind_e));//we are storing as a signed byte
				eeprom_write_byte((u08*)&windnorth[Heightupto],(s08)(Wind_n));//we find the average and shove it in the eeprom
				Heightupto++;				
				L = 0;
				Wind_e=0.0;
				Wind_n=0.0;
			}
			wiggleservo();
			if (radio_cts)                    	//CTS from radio
			{
				temperature=read_mlx90609(read_temp);				
				dataprint('>',&Gps);
			}
			#ifdef EEPROM
			set_address(&I2Caddress);
			write_data((u08*)&Gps.altitude,12,&I2Caddress);		//shove the position in the eeprom
			i2cstop();
			#endif
			getgps();
		}
	}
	//we need to update the GPS and Height counter after cutdown
 
	getgps();
	Heightupto=(int)(Gps.altitude/100.0)+1;
 
	/////////////////////////////Decent guidance loop
 
	sbi(TIMSK1, OCIE1B);						//enable output compare interrupt - turns on guidance code
	time=0;
	while(time<8 && eeprom_read_byte(&flight_status)!=0x02)	//while we havent landed and we havent previously landed
	{
		if((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126)		//put wind in if its not blank painted eeprom
		{
			Wind_e=(float)(s08)eeprom_read_byte((u08*)&windeast[Heightupto]);
			Wind_n=(float)(s08)eeprom_read_byte((u08*)&windnorth[Heightupto]);
		}
		else
		{
			if(L>0)									//allows us to use data from last layer
			{										//if we are still in it and have arrived
				Wind_e/=L;							//here from ascent loop with some data
				Wind_n/=L;
				L=-1;
			}
			else if (L!=-1)							//if we havent used last layer data for find a solution
			{
				Wind_e=0.0;							//painted eeprom has no valid data
				Wind_n=0.0;
			}
		}
		//clockwise is defined as +ive, everything uses the local ENU wind frame / wind in this frame
		//direction to target (uses "equatorial radian units" - my invention, distance equal to PI on equator)
		Target = atan2((targeteast - Gps.longitude)*cos(targetnorth) - descent_drift_e,targetnorth - Gps.latitude - descent_drift_n);
	    //wind compensation  in ENU frame
		Heading = atan2(Gps.veast-Wind_e,Gps.vnorth-Wind_n);
		if(!(Gps.vnorth-Wind_n))
		{
			Heading = 0.0;				//stops us flooding with nan if zero wind and stationary
		}
		Heading_flag=TRUE;				//Kalman filter is now free to run with the latest data
		//"eye candy"
		K=Heading-Target;				//k is now our heading offset (from now on is just for leds)
		if (K >PI)
		{
			K -= 2*PI;
		}								//all to keep us in +-180 degree range
		if(K < -PI)
		{
			K += 2*PI;
		}  
		if (K > 0) 					            
		{
			led_left;					//left/right indictor: bicolour LED between 3 and 4
		}
		else
		{
			led_right;   
		}
		//data
 		if (radio_cts)                    		//CTS from radio
		{
			dataprint('<',&Gps);
		}
		#ifdef GROUND
		if(Gps.altitude<200.0)					//near the ground, ground control
		{
			enable_ground_control();
		}
		#endif
		//have we (crash) landed?
		if(Gps.altitude<200.0 && Gps.veast<0.1 && Gps.vnorth<0.1)
		{
			time++;
		}
		else
		{
			time=0;
		}
		#ifdef EEPROM
		set_address(&I2Caddress);
		write_data((u08*)&Gps.altitude,12,&I2Caddress);
		write_data((u08*)&Target,4,&I2Caddress);
		Kalman_flag=TRUE;									//lock the data
		write_data((u08*)&(our_state.state),8,&I2Caddress);	//shove ze data in the eeprom
		Kalman_flag=FALSE;
		i2cstop();
		#endif
		L=(int)(Gps.altitude/100.0);
		if(descent_variable>LOWER_DESCENT_LIMIT && descent_variable<UPPER_DESCENT_LIMIT)//check its sensible
		{
			for(Heightupto=0;(Heightupto<=L)&&((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126);Heightupto++)
			{
				K=descent_variable*(float)pgm_read_byte(&sqrt_density[Heightupto]);			
				descent_drift_e+=(s08)eeprom_read_byte((u08*)&windeast[Heightupto])*K;	//add layers of wind to our integral
				descent_drift_n+=(s08)eeprom_read_byte((u08*)&windnorth[Heightupto])*K;	
			}
		}
		Heightupto++;										//we increment Heightupto so it is correct for obtaining the wind
		descent_variable_noise+=DESCENT_PROCESS_NOISE;		//this is a single component state vector extended kalman filter
		if(Gps.vup<-0.5 && Gps.vup>-50.0)					//sanity check for a descent
		{			//we run if we are descending and estimate the transit time coefficient
			b=(float)pgm_read_byte(&sqrt_density[Heightupto]);
			K=descent_variable_noise/((GPS_VEL_NOISE*b*b*descent_variable*descent_variable*descent_variable*descent_variable)+descent_variable_noise);//this is K
			b=(-100.0)/(6378000.0*Gps.vup*b);		//estimate of the proportionality constant			
			descent_variable+=K*(b-descent_variable);
			descent_variable_noise-=descent_variable_noise*K;
		}
		getgps();
	}
	if(eeprom_read_byte(&flight_status)!=0x02)	//0x02== landed
	{
		eeprom_write_byte(&flight_status,0x02);	//set status to landed if it isnt already
	}
	/////////////////////////////Recovery loop
	cbi(TIMSK1, OCIE1B);						//turn off kalman
	cbi(TCCR1A, COM1B1);						//turn off servo
	#ifdef GROUND
		disable_ground_control();				//turn off input capture
	#endif
	DISABLE_SMPS;								//turn off SMPS - always turn off the servo pwm first!
	for(;;)										//low power use recovery/landing mode
	{
		if (radio_cts)                    		//CTS from radio
		{
			temperature=read_mlx90609(read_temp);
			dataprint('-',&Gps);					//send data
		}
		getgps();
	}
}</code>

==== Main.h ====
<code c>#include "global.h"
#include "avrlibdefs.h"
#include "avrlibtypes.h"
#include "matrix.h"
#include "kalman.h"
#include "TSIP.h"
#include "rprintf.h"
#include "i2cmem.h"
#include <avr/io.h>
#include <stdlib.h>
#include <math.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay_basic.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h> 
#define BAUDRATE (int)9600						//9600 baud for lassen iq
#define DELTA_TIME (float)0.02						//20ms
#define DELAY _delay_loop_2((u16)((float)F_CPU/(4.0*(float)BAUDRATE)))	//delay for suart routine
#define SET	PORTD |= 0x10						//PORTD.4 is the tx
#define CLEAR PORTD &= ~0x10
 
//kalman filter - NOTE: these constants are airframe specific
#define DAMPING_CONSTANT (float)0.3				//approx how fast oscillations die down as faction per second
#define CONTROL (float)0.75					//full control input of 1 gives ~ this turn rate
#define CONTROL_GAIN (DAMPING_CONSTANT*CONTROL)			//as used in the kalman filter
 
#define TOP_COUNT (u16)((float)F_CPU*DELTA_TIME/8.0)		//timer1 pwm frequency
#define PWM_COUNT_TIME (u16)((float)F_CPU*0.0005/8.0)			//500us
 
#define I_C -0.001						//control loop
#define P_C -0.3
#define D_C -0.5
#define INTEGRAL_LIMIT 0.2/I_C					//integral servo shift limited to 1/5 of full scale range
 
#define GPS_VEL_NOISE (0.25*(6378000.0/100.0)*(6378000.0/100.0))//these seem to be tolerant to errors of ~an order of mag
#define DESCENT_PROCESS_NOISE 1.0e-22				//tested with badger data
#define DESCENT_INIT 1.5e-8
#define DESCENT_NOISE_INIT 5.0e-17

#define read_rate (u08)0b10010100				//gyro specific
#define read_temp (u08)0b10011100
#define read_melexis (u08)0x80
 
#define gyro_null 1022
 
#define altitudelimit 10000						//flight termination conditions
#define timelimit 30							//NOTE: for debug
 
#define targeteast (float)(0.1*Deg2rad)			//target waypoint ~ Cambridge
#define targetnorth (float)(52.0*Deg2rad)
 
#define Rad2deg 180.0/PI						//bloody obvious
#define Deg2rad PI/180.0
 
#define PRINT_COMMA rprintfChar(pgm_read_byte(&comma))	//prints a comma
 
#define battery_factor (float)0.02505			//for measuring battery voltage- checked from test
#define battery_chan 0x00					//ADC0
 
#define toggle_pin PIND=0x20				//led on port D.5
 
#define BAUDDIV  (u16)( ((float)F_CPU/(BAUDRATE*16UL)) -1 )//baud divider
 
#define radio_cts bit_is_set(PIND,2)			//hardware, however its wired up
 
#define led_left PORTC = ((PORTC & ~0x0C) | 0x04)
#define led_right PORTC = ((PORTC & ~0x0C) | 0x08)
 
#define USER_PRESENT !(PINC&0x02)
 
#define cutter_on PORTD|=(1<<7)					//release mechanism (hot resistor off mosfet)
#define cutter_off PORTD&=~(1<<7)
 
#define DISABLE_SMPS PORTD|=(1<<6)				//turns the SMPS for the servo on/off - never send pwm if its off
#define ENABLE_SMPS PORTD&=~(1<<6)
 
#define GYRO_OFF PORTB|=1<<1					//SS line
#define GYRO_ON PORTB&=~(1<<1)
 
#ifdef ADCSRA
	#ifndef ADCSR
		#define ADCSR   ADCSRA
	#endif
#endif
#ifdef ADATE
	#define ADFR    ADATE
#endif
#ifdef GROUND
	void enable_ground_control();								//enables the input capture interrupt
	void disable_ground_control();								//disables input capture interrupt
	#define PULSE_MIN (u16)(0.0008*(float)F_CPU/8.0)			//pwm must be in the range 0.8 to 2.2 ms
	#define PULSE_MAX (u16)(0.0022*(float)F_CPU/8.0)			//due to the bit shift we divide by 16
	#define GROUND_LED_ON PORTD|=(1<<3)
	#define GROUND_LED_OFF PORTD&=~(1<<3)
	#define GROUND_LED_SET PORTD&(1<<3)
	void run_ground_control();
#endif
 
typedef struct
{
	float vup;
	float vnorth;
	float veast;
	float time;
	float altitude;
	float longitude;
	float latitude;
	u08 packetflag;								//packetflag lets us see when our packet has been updated
	u08 status;
	u08 nosats;
} gps_type;
 
void put_char(unsigned char c);							//talk to the UART
void suart_send(char c);
void run_ground_control();
void _delay_10ms(char time);
float driftfactor(int altitude);
u08 I2Cdataprint(u32* I2Caddress);
</code>