UKHAS Wiki

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

#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);

//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
}*/