code:parafoil_v2
Main code
//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; volatile u08 radio_flag; 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 //setup the radio ready interrupt EICRA=0x03; //rising edge on int0 (radio cts line) causes an interrupt //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(INT0_vect) //set to trigger on rising edge, indicating a reset { radio_flag|=0x0F; //there will be 16 messages before next reset EIMSK&=~0x01; //it disables itself } 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,rate_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 timer++; } 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; timer++; //we use timer to turn on control after a few seconds } 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; } rate_integral+=our_state_local.state.b;//rate integrator if(rate_integral>RATE_INTEGRAL_LIMIT) { rate_integral=RATE_INTEGRAL_LIMIT; } if(rate_integral<-RATE_INTEGRAL_LIMIT) { rate_integral=-RATE_INTEGRAL_LIMIT; } if(timer>4) { control_vector.b=P_C*a+I_C*integral+D_C*our_state_local.state.b+R_C*rate_integral; //PIDRi 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 } #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=7; for(;a;a--) //7 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) { if(!(radio_flag&0x0F)) //the last 4 bits in used as a counter { if(radio_flag!=0xF0) //if we havent already timed out waiting for a reset { put_char(0x05); //tell the radio to reset EIMSK|=0x01; //enable INT0 interrupt, it disables itself for(radio_flag&=0x0F;((radio_flag>>4)<15)&&!(radio_flag&0x0F);radio_flag+=0x10)//wait for it to or timeout _delay_loop_2((u16)((float)F_CPU*0.0025));//loop delay of 10ms } } else //the radio is ok to run { radio_flag--; #ifdef VERBOSE u08 local_char; #endif rprintfProgStr(PSTR("UKHAS")); rprintfChar(flight_status); //>==ascending, <==descending, -==landed PRINT_COMMA; checksum=0x00; //reset the checksum rprintfFloat(6,(double)gps->latitude); PRINT_COMMA; #ifdef VERBOSE if (gps->latitude>0) { local_char='N'; } else { local_char='S'; } rprintfChar(local_char); PRINT_COMMA; #endif rprintfFloat(6,(double)gps->longitude); PRINT_COMMA; #ifdef VERBOSE if (gps->longitude>0) { local_char='E'; } else { local_char='W'; } rprintfChar(local_char); PRINT_COMMA; #endif rprintfFloat(1,(double)gps->altitude); PRINT_COMMA; #ifdef VERBOSE rprintfChar('M'); PRINT_COMMA; #endif 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 u08 n; float K; float descent_drift_n; float descent_drift_e; 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 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")); if(mcusr_mirror&(1<<BORF)) rprintfProgStr(PSTR(" Brownout triggered")); if(mcusr_mirror&(1<<EXTRF)) rprintfProgStr(PSTR(" Reset pressed")); rprintfCRLF(); if(mcusr_mirror&(1<<PORF)) //this is what should happen - power switched on rprintfProgStr(PSTR(" Mini Rogallo by Laurence Blaxter, 2008")); rprintfCRLF(); if(USER_PRESENT) //dead man switch to gnd D.4 { /////////////////////////////Start of mission setup code/loops wdt_enable(WDTO_4S); rprintfProgStr(PSTR("Press for wind data")); rprintfCRLF(); _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")); } rprintfProgStr(PSTR("Press to wipe wind data")); rprintfCRLF(); _delay_10ms(100); if(USER_PRESENT) { for (L=0;L<255;L++) { 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")); rprintfCRLF(); 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")); rprintfCRLF(); _delay_10ms(100); //a 1 second delay if(USER_PRESENT) { rprintfProgStr(PSTR("Testing the I2C...")); //actually we have already done it rprintfCRLF(); I2Cerr=0; I2Caddress=0; //eeprom address is setup set_address(&I2Caddress); //go to the first location if(!I2Cerr) //no error { rprintfProgStr(PSTR("[OK]")); //I2C debug crap rprintfCRLF(); 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")); rprintfCRLF(); _delay_10ms(100); wdt_reset(); if(USER_PRESENT) { rprintfProgStr(PSTR("Wiping...")); 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, waiting for GPS...")); rprintfCRLF(); while(Gps.status<4) //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")); rprintfCRLF(); getgps(); } rprintfProgStr(PSTR("3D GPS Lock, ready for launch")); rprintfCRLF(); } else //--------------------- erranous reset recovery code ----------------------------------------- { #ifdef EEPROM I2Caddress=findtop(); //we need to avoid overwiting old data #endif rprintfProgStr(PSTR("Setup complete, EEPROM top=")); rprintf("%d",I2Caddress/12); rprintfCRLF(); } L=0; 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*140) //do we now fall into another (higher) 140m incrament ? { Wind_e*=4.0/(float)L; //0.25m/s resolution Wind_n*=4.0/(float)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 after cutdown getgps(); /////////////////////////////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 { K=Gps.altitude/140.0; Heightupto=(int)(K); K=abs(K-Heightupto-0.5); //use weighted average of the two nearest layers //put wind in if its not blank painted eeprom if((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126) { Wind_e=(float)(s08)eeprom_read_byte((u08*)&windeast[Heightupto]); Wind_n=(float)(s08)eeprom_read_byte((u08*)&windnorth[Heightupto]); if((s08)eeprom_read_byte((u08*)&windeast[Heightupto++])!=-126)//if the next layer up is unpainted, weight { Wind_e*=K; Wind_n*=K; K=1-K; Wind_e+=K*(float)(s08)eeprom_read_byte((u08*)&windeast[Heightupto]); Wind_n+=K*(float)(s08)eeprom_read_byte((u08*)&windnorth[Heightupto]); } Wind_e/=4.0; //convert back to m/s units Wind_n/=4.0; } 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(); if(abs(Gps.veast)<0.1 && abs(Gps.vnorth)<0.1) time++; else time=0; } #else //have we (crash) landed? if(Gps.altitude<200.0 && abs(Gps.veast)<0.1 && abs(Gps.vnorth)<0.1) time++; else time=0; #endif #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 n=Gps.altitude/100.0; descent_drift_e=0; descent_drift_n=0; //for all layers underneath us for(Heightupto=0;(Heightupto<=n)&&((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126);Heightupto++) { K=DESCENT_CONSTANT*(float)pgm_read_byte(&sqrt_density[Heightupto]); descent_drift_e+=(float)(s08)eeprom_read_byte((u08*)&windeast[Heightupto])*K;//add layers of wind to our integral descent_drift_n+=(float)(s08)eeprom_read_byte((u08*)&windnorth[Heightupto])*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(); } }
header
#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.00037 //control loop #define P_C -0.25 //values from previous accomodated for 20/16ms,+-1servo input,1:3.5 gearing #define D_C -0.35 //and made 30% less agressive #define R_C -0.0005 //new rate integral term, saturates after ~6 turns #define INTEGRAL_LIMIT -0.2/I_C //integral servo shift limited to 1/5 of full scale range #define RATE_INTEGRAL_LIMIT -0.4/I_C //-we are now using flight data and descent valiable is a macro #define DESCENT_CONSTANT 2.46e-8 #define read_rate (u08)0b10010100 //gyro specific #define read_temp (u08)0b10011100 #define read_melexis (u08)0x80 #define gyro_null 1022 #define altitudelimit 5000 //flight termination conditions #define timelimit 3600 //NOTE: for flight (this is in seconds) #define targeteast (float)(-0.096236*Deg2rad) //target waypoint -> EARS, track in front of launch area #define targetnorth (float)(52.25106*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_v2.txt · Last modified: 2009/03/27 03:37 by laurenceb