UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools


code:parafoil_main

This uses the interrupt driven NMEA collection code, along with the two state Kalman filter. The ground control module can be added or removed as needed. Compiles to 14.7KB with ground control, so should run on an atmega168 clocked at around 16MHz or more with a 512 or 1024Byte bootloader.

Dependencies

TSIP version for lassen iq

TSIP based version here (nmea version has been “mothballed”)

Main.h

#include "global.h"
#include "avrlibdefs.h"
#include "avrlibtypes.h"
#include "matrix.h"
#include "kalman.h"
#include <avr/io.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/eeprom.h>
 
#define minutes (float)1.0/600000.0				//gps macros
#define baudrate (int)4800
#define delta_time (float)0.02					//20ms
#define sqrt_delta_time (float)0.141			//kalman filter
#define control_gain (float)90.0
#define damping_constant (float)0.2
 
#define top_count (u16)((float)F_CPU*delta_time/8.0)		//timer1 pwm frequency
#define pwm_count_time ((float)F_CPU*0.0005/8.0)			//500us
#define safe_time_start	(u08)((u16)(5.0*pwm_count_time)>>8)	//make sure we are after the pwm pulse
#define safe_time_end (u08)((top_count-1000)>>8)			//allow 800 clock cycles
 
#define I_C 0.0001								//control loop
#define P_C 0.02
#define D_C 0.04
#define integral_limit 0.2/I_C
 
#define read_rate (u08)0b10010100				//gyro specific
#define read_temp (u08)0b10011100
#define read_melexis (u08)0x80
#define mask_word (u16)0x07FF
 
#define gyro_null 1009
 
#define altitudelimit 4000						//flight termination conditions
#define timelimit 3600
 
#define targeteast 2.0							//target waypoint
#define targetnorth 52.0
 
#define Rad2deg 180.0/PI						//bloody obvious
#define Deg2rad PI/180.0
 
#define battery_factor (float)8.0/1024			//for measuring battery voltage
#define battery_chan 0x05						//ADC6
 
#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,3)			//hardware, however its wired up
 
#define led_left PORTC = ((PORTC & ~0x18) | 0x08)
#define led_right PORTC = ((PORTC & ~0x18) | 0x10)
 
#define undead_man !(PIND&0x10)
 
#define cutter_on PORTD|=(1<<7)					//release mechanism (hot resistor off mosfet)
#define cutter_off PORTD&=~(1<<7)
 
#define gyro_off PORTB|=1<<2					//SS line
#define gyro_on PORTB&=~(1<<2)
 
#ifdef ADCSRA
	#ifndef ADCSR
		#define ADCSR   ADCSRA
	#endif
#endif
#ifdef ADATE
	#define ADFR    ADATE
#endif
 
typedef struct
{
	float longitude;
	float latitude;
	float altitude;
	float speed;
	float heading;
	u08 packetflag;								//packetflag lets us see when our packet has been updated
	u08 status;
} gps_type;
 
int put_char(char c, FILE * stream);			//talk to the UART
int get_char( FILE * stream);

Main.c

//UAV parafoil main code
#include "main.h"
#include "ground.h"
//globals
unsigned char EEMEM windeast[255];
unsigned char EEMEM windnorth[255];
 
unsigned char EEMEM flight_status;
 
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 gps_type Gps;
//ground control globals
#ifdef GROUND
	extern volatile u08 pwm_counter;
	extern volatile u08 pwm_period;
#else
	#define pwm_counter (u08)0x00
#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(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 2.56V
	cbi(ADMUX, ADLAR);              // set to right-adjusted result
 
	//sbi(ADCSR, ADIE);               // enable ADC interrupts
 
	//Timer 1 setup
	TCCR1B = ((TCCR1B & ~0x07) | 0x02);	//clock at F_CPU/8
 
	TCNT1 = 0;		//reset timer1  
 
	// set PWM mode with ICR top-count
	cbi(TCCR1A,WGM10);
	sbi(TCCR1A,WGM11);
	sbi(TCCR1B,WGM12);
	sbi(TCCR1B,WGM13);
 
	// set top count value to give correct period PWM
	ICR1 = top_count;
 
	// clear output compare value A
	OCR1A = 0;
	// clear output compare value B
	OCR1B = 0;
 
	// turn on channel A (OC1A) PWM output
	// set OC1A as non-inverted PWM for direct servo connection
	sbi(TCCR1A,COM1A1);
	cbi(TCCR1A,COM1A0);
 
	//Timer0 setup
	#ifdef GROUND
	TCCR0B = ((TCCR0B & ~0x07) | 0x04);		//clock at F_CPU/256
 
	TCNT0=0x00;
	#endif
	//External interrupts setup
 
	outb(PCMSK1,(1<<PCINT8)|(1<<PCINT9));
 
	//SPI setup
	outb( SPCR, (1<<MSTR)|(1<<SPE));		//F_CPU/4, master, CPOL=CPHA=0
 
	//IO setup
	gyro_off;											//SS to the gyro - pullups
	PORTD|=( (1<<3) | (1<<4) );								//pullups to radio cts and dead man switch			
	DDRB=0b00101111;										//talk to servo and gyro
	DDRC=0b00011000;										//indicator leds
	DDRD=0b10100000;										//cutdown line and "heartbeat" led	
 
	//Kalman setup
	our_model.F.tl=1.0;										//kalman model setup
	our_model.F.tr=delta_time;								//propogator
	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;
 
	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=5.0*delta_time;					//estimated - corresponds to heading change
	our_model.Q.tr=0.0;
	our_model.Q.bl=0.0;
	our_model.Q.br=10.0*delta_time;					//estimated - corresponds to rate change
 
	our_model.R.tl=100.0;							//Data covariance
	our_model.R.tr=0.0;
	our_model.R.bl=0.0;
	our_model.R.br=2.0;								//approximate for an MLX90609
 
	our_state.state.t=0.0;
	our_state.state.b=0.0;
 
	our_state.P.tl=8000.0;						//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=400.0;						//rough estimate for turn rate error^2 at release
 
}
 
float read_gyro()
{
	u16 a;
	gyro_on;									//SS = 0
	SPDR=read_rate;
	while(!(inb(SPSR) & (1<<SPIF)));
	SPDR=0x00;
	while(!(inb(SPSR) & (1<<SPIF)));
	SPDR=0x00;
	while(!(inb(SPSR) & (1<<SPIF)));
	gyro_off;
	_delay_us(150);
	gyro_on;
	SPDR=read_melexis;
	while(!(inb(SPSR) & (1<<SPIF)));
	SPDR=0x00;
	while(!(inb(SPSR) & (1<<SPIF)));
	a=(u16)SPDR<<8;
	gyro_error=SPDR&0b01000000;					//internal selftest NOTE, not on latest datasheet, obsolete?
	SPDR=0x00;
	while(!(inb(SPSR) & (1<<SPIF)));
	a|=(u16)SPDR;
	gyro_off;
	a=(a>>1)&mask_word;							//all based on the rogallo code
	return (float)(125.0/1024.0)*(float)(a-gyro_null);		//+-125 degree/sec gyro, 11 bit resolution
}
 
void set_servo(float setting)
{
	OCR1A=(u16)(pwm_count_time*(3.0+setting));		//pwm = 1.5 +-0.5us
}
 
ISR(TIMER1_COMPA_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=read_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;
	}
	our_state_local=predict_and_update(&our_state_local,&our_model,&control_vector,&measurement_vector);
	if(our_state_local.state.t>180)		//keeps us in the =-180 degree range
	{
		our_state_local.state.t-=360;
	}
	if(our_state_local.state.t<-180)
	{
		our_state_local.state.t+=360;
	}
	a=our_state_local.state.t-target;
	if(a>180)						//heading offset
	{
		a-=360;
	}
	if(a<-180)
	{
		a+=360;
	}
	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 control
		if(control_vector.b>1)	//servo limits
		{
			control_vector.b=1.0;
		}
		if(control_vector.b<-1)
		{
			control_vector.b=-1.0;
		}
		set_servo(control_vector.b);
	}
	else
	{
		timer++;				//we use timer to turn on control after a few seconds
	}
	#ifdef GROUND
	if(pwm_counter==0x0A)		//if under ground control, use the recorded pwm as a control input
	{
		control_vector.b=((float)pwm_period*(1.0/pwm_record_factor)-3.0);//1500us->0,2000->+1,1000->-1
	}
	#endif
	if(!Kalman_flag)			//our global kalman state is unlocked
	{
		our_state=our_state_local;
	}
}
 
u08 cutdownrulecheck(int time, float * altitude)
{
	if(*altitude>altitudelimit)
	{
		printf("CUTDOWN, Altitude limit\r\n");
  		return 1;
	}
	if(time>timelimit)
	{
		printf("CUTDOWN, Time limit\r\n");
		return 2;
	}
	return 0;
}
 
u08 checksum(char *c_buf)
{
	u08 sum=0;
	while(*c_buf)
	{
		sum += *c_buf++;
	}
	return sum;
}
 
void cutdown(char time)
{
	cutter_on;
	for(;time;time--)
	{
		_delay_ms(1000);
		wdt_reset();
	}
	cutter_off;
}
 
void wiggleservo()
{
	set_servo(1);
	_delay_ms(150);
	set_servo(-1);
	_delay_ms(150);
	set_servo(0);
}
 
float getbatteryvoltage()
{
	//a2dCompleteFlag = FALSE;                // clear conversion complete flag
	outb(ADMUX, (inb(ADMUX) & ~0x1F) | (battery_chan & 0x1F));    // set channel
	sbi(ADCSR, ADIF);                       // clear hardware "conversion complete" flag 
	sbi(ADCSR, ADSC);                       // start conversion
	//while(!a2dCompleteFlag);              // wait until conversion complete
	//while( bit_is_clear(ADCSR, ADIF) );       // wait until conversion complete
	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 dataprint(gps_type * gps)
{
	char outputbuffer[80];
	char longitude;
	char latitude;
	if (gps->longitude>0)
	{
		longitude='E';
	}
	else
	{
		longitude='W';
	}
	if (gps->latitude>0)
	{
		latitude='N';
	}
	else
	{
		latitude='S';
	}
	Kalman_flag=TRUE;							//lock the kalman data
	while((TCNT1>>8)>(safe_time_start) && (TCNT1>>8)<(safe_time_end))	//wait until we arent going to be interrupted
	sprintf(outputbuffer,"%.6f,%c,%.6f,%c,%.1f,M,%.1f,%.1f,%.1f,%.1f,%.1f,%X",(double)gps->latitude,latitude,
	(double)gps->longitude,longitude,(double)gps->altitude,(double)Heading,(double)Target,(double)our_state.state.t,
	(double)our_state.state.b,(double)getbatteryvoltage(),pwm_counter);	//we generate a string-position and filter info
	Kalman_flag=FALSE;				//unlock the data
	if(eeprom_read_byte(&flight_status))		//reuse the latitude variable
	{
		latitude='<';				//descending
	}
	else
	{
		latitude='>';				//ascending
	}
	printf("UKHAS%c,%s,%02X\r\n",latitude,outputbuffer,checksum(outputbuffer));	//with checksum and up/down info 	
	return ;								
}
 
int put_char(char c, FILE * stream)
{
	loop_until_bit_is_set(UCSR0A, UDRE0);	//unbuffered tx comms
	UDR0 = c;
 	return 0;
}
 
int get_char(FILE * stream)
{
	return -1;
}
 
int main()
{
	wdt_enable(WDTO_4S);				//watchdog set to 4 seconds
	int time=0;
	int L;
	int Heightupto=0;
	float K;
	float Wind_x=0;
	float Wind_y=0;
	FILE mystdio = FDEV_SETUP_STREAM(put_char, get_char, _FDEV_SETUP_RW);  //so we can printf to the radio
	setup();
	stdout = &mystdio;
	sei();
	if(undead_man)							//dead man switch to gnd D.4
	{
		while(!Gps.status)					//wait for a gps lock
		{
			printf("Waiting for GPS\r\n%.5f\r\n%.5f",(double)Gps.latitude,(double)Gps.longitude);
			wdt_reset();
			Gps.packetflag=FALSE;
			while(!Gps.packetflag);					//wait for some new gps
		}
		for (L=0;L<255;L++)
		{
			printf("Record number %d: East,%d",L,(int)eeprom_read_byte(&windeast[L]));
			printf(", West,%d\r\n",(int)eeprom_read_byte(&windnorth[L]));
			wdt_reset();
		}
		if(eeprom_read_byte(&flight_status))		//set eeprom status to ascending
		{
			eeprom_write_byte(&flight_status,0x00);
		}
		printf("Test ground control function\r\n");
		#ifdef GROUND
		enable_ground_control();
		for(;L>250;L--)								//5 second delay
		{
			_delay_ms(1000);						//time to test ground control
			wdt_reset();							//reset watchdog
		}
		disable_ground_control();
		#endif
		L=0;										//need it =0 for next stage
		printf("Ok, ready for launch\r\n");
	}
	if(!eeprom_read_byte(&flight_status))			//0x00=ascent
	{
		while(!cutdownrulecheck(time,&(Gps.altitude)))
		{
 			time++;
 			Wind_x += Gps.speed*sin(Deg2rad*Gps.heading);            //add it to our total wind
 			Wind_y += Gps.speed*cos(Deg2rad*Gps.heading);
 			L++;                              //incrament the denominator for our averaging
        	        //find the altitude in 50m incraments
			if ((int)(Gps.altitude/50.0) > Heightupto)    //do we now fall into another (higher) 50m incrament ?
 			{
				Heightupto++;                           
				eeprom_write_byte(&windeast[Heightupto],(u08)(Wind_x/L + 127));         //we are storing as a signed byte
				eeprom_write_byte(&windnorth[Heightupto],(u08)(Wind_y/L + 127));         //we find the average and shove it in the eeprom
				L = 0;                                                   
			}
			wiggleservo();
			if (radio_cts)                    	//CTS from radio
			{
				dataprint(&Gps);
			}
			wdt_reset();
			Gps.packetflag=FALSE;				//"Unlock" global structure
			while(!Gps.packetflag);				//wait for some new gps
		}
		cutdown(12);
		eeprom_write_byte(&flight_status,0x01);	//we have cutdown=decent
	}
	sbi(TIMSK1, OCIE1A);						//enable output compare interrupt - turns on guidance code
	for(;;)
	{
		L=(int)Gps.altitude/50.0;
		Wind_x=(float)(eeprom_read_byte(&windeast[L])-127);
		Wind_y=(float)(eeprom_read_byte(&windnorth[L])-127);	//load valid wind data
		//direction to target (uses equatorial degree units)
		Target = Rad2deg*atan2((targeteast - Gps.longitude)*cos(Deg2rad*Gps.latitude),targetnorth - Gps.latitude);
	        //wind compensation  in ENU frame
		Heading =Rad2deg*atan2(Gps.speed*sin(Gps.heading)-Wind_x,Gps.speed*cos(Gps.heading)-Wind_y);
		Heading_flag=TRUE;
		K=Heading-Target;				//k is now our heading offset (from now on is just for leds)
		if (K >180)
		{
			K -= 360;
		}								//all to keep us in +-180 degree range
		if(K < -180)
		{
			K += 360;
		}  
		if (K > 0) 					            
		{
			led_left;					//left/right indictor: bicolour LED between 3 and 4
		}
		else
		{
			led_right;   
		}
 		if (radio_cts)                    		//CTS from radio
		{
			dataprint(&Gps);
		}
		#ifdef GROUND
		if(Gps.altitude<200.0)					//near the ground, ground control
		{
			enable_ground_control();
		}
		#endif
		wdt_reset();
 		Gps.packetflag=FALSE;					//"Unlock" the global variable, so GPS parsing ISR can update it
		while(!Gps.packetflag);					//wait for some new gps
	}
}
 
/* 
char *datastring()							//old code, might be useful
{
	static char outputbuffer[80];
	char longitude;
	char latitude;
 
	if (gps.longitude>0)
	{
		longitude='E';
	}
	else
	{
		longitude='W';
	}
	if (gps.latitude>0)
	{
		latitude='N';
	}
	else
	{
		latitude='S';
	}
	sprintf(outputbuffer,"%.6f,%c,%.6f,%c,%.1f,M,%.1f,%.1f,%.1f,%.1f,%.1f,%X",(double)gps.latitude,latitude,
	(double)gps.longitude,longitude,(double)gps.altitude,(double)Heading,(double)Target,(double)our_state.state.t,
	(double)our_state.state.b,(double)getbatteryvoltage(),pwm_counter);
	sprintf(outputbuffer,"%s%X",outputbuffer,checksum(outputbuffer));		//we generate a string 
	return outputbuffer;								//with checksum, containing position and filter info
}*/
code/parafoil_main.txt · Last modified: 2008/08/16 21:11 by laurenceb