code:parafoil_tsip
Main.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(); } }
Main.h
#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/parafoil_tsip.txt · Last modified: 2009/02/26 02:19 by laurenceb