UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace:
code:parafoil_tsip

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
code:parafoil_tsip [2009/01/15 03:48] – laurencebcode:parafoil_tsip [2009/02/26 02:19] (current) – laurenceb
Line 6: Line 6:
 #include "main.h" #include "main.h"
 //globals //globals
 +#include "sqrt_density.h"
 #ifdef GROUND #ifdef GROUND
  volatile u16 diff;  volatile u16 diff;
Line 18: Line 18:
 signed char EEMEM windeast[254]; //wind data signed char EEMEM windeast[254]; //wind data
 signed char EEMEM windnorth[254]; signed char EEMEM windnorth[254];
 + 
 unsigned char EEMEM flight_status; //what stage of flight unsigned char EEMEM flight_status; //what stage of flight
 #ifdef EEPROM #ifdef EEPROM
Line 80: Line 80:
   SET; //set software uart to gps   SET; //set software uart to gps
  sbi(PINB,2); //experimental hack to set the /SS input to 1 ?  sbi(PINB,2); //experimental hack to set the /SS input to 1 ?
 + 
  //enable the power to the servo  //enable the power to the servo
    
  ENABLE_SMPS;  ENABLE_SMPS;
 + 
  //SPI setup - do this here as the /SS pin is already set as an output  //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  SPCR = (1<<MSTR)|(1<<SPE)|(1<<CPHA)|(1<<CPOL); //F_CPU/4=5MHz, master, CPOL=CPHA=1
  //outb( SPSR, (1<<SPI2X));  //outb( SPSR, (1<<SPI2X));
 + 
  //Timer 1 setup  //Timer 1 setup
  TCCR1B = (1<<ICNC1)|0x02; //clock at F_CPU/8, enable noise cancelling on ICP  TCCR1B = (1<<ICNC1)|0x02; //clock at F_CPU/8, enable noise cancelling on ICP
Line 107: Line 107:
  sbi(TCCR1A,COM1B1);  sbi(TCCR1A,COM1B1);
  cbi(TCCR1A,COM1B0);  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  //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  OCR1B=3*PWM_COUNT_TIME; //center servo - servo is on OC1B
- +  
- + 
  //Kalman setup //\/MATRIX\/COMMENTS  //Kalman setup //\/MATRIX\/COMMENTS
  our_model.F.tl=1.0; //propogator  our_model.F.tl=1.0; //propogator
Line 170: Line 170:
  GYRO_OFF; //all based on the rogallo code  GYRO_OFF; //all based on the rogallo code
  return ( (a<<7)|(SPDR>>1) ) & 0x07FF; //11 bit resolution - offset by one bit to the left  return ( (a<<7)|(SPDR>>1) ) & 0x07FF; //11 bit resolution - offset by one bit to the left
-} 
- 
-float air_density(float alt) //model from NASA 
-{     
-     float temp,pres; 
- if (alt < 11000.0) 
-     {    // below 11Km - Troposphere 
-          temp = 15.04 - (0.00649 * alt); 
-          pres = 101.29 * pow((temp + 273.1) / 288.08, 5.256);  
-     } 
-     else 
-     { 
-         if (alt < 25000.0) 
-         { // between 11Km and 25Km - lower Stratosphere 
-              temp = -56.46; 
-              pres = 22.65 * exp(1.73 - ( 0.000157 * alt)); 
-         } 
-         else 
-         { // above 25Km - upper Stratosphere 
-               temp = -131.21 + (0.00299 * alt); 
-               pres = 2.488 * pow((temp + 273.1) / 216.6, -11.388); 
-         } 
-     } 
-  
-     return(pres / (0.2869 * (temp + 273.1))); 
 } }
    
Line 325: Line 300:
  }  }
 } }
 + 
 void wiggleservo() void wiggleservo()
 { {
Line 357: Line 332:
  while(!Gps.packetflag); //wait for some new gps  while(!Gps.packetflag); //wait for some new gps
 } }
-/* + 
-float driftfactor(int altitude) +
-{ +
- return WEIGHTINGFACTOR*exp(SCALEHEIGHT*((float)altitude-0.5)); +
-} +
-*/ +
 void dataprint(u08 flight_status,volatile gps_type * gps) void dataprint(u08 flight_status,volatile gps_type * gps)
 { {
Line 425: Line 395:
  return ;   return ;
 } }
 + 
 void put_char(unsigned char c) void put_char(unsigned char c)
 { {
Line 451: Line 421:
  u08 Heightupto=1; //this rounds up  u08 Heightupto=1; //this rounds up
  float K;  float K;
 + float b,descent_variable=0.05882,descent_variable_noise=0.002;
  float descent_drift_n=0;  float descent_drift_n=0;
  float descent_drift_e=0;  float descent_drift_e=0;
  float Wind_e=0.0;  float Wind_e=0.0;
  float Wind_n=0.0;  float Wind_n=0.0;
- float descent_variable=DESCENT_INIT; 
- float sqrt_density; 
- float descent_variable_noise=DESCENT_NOISE_INIT; 
  setup(); //all the major setup stuff  setup(); //all the major setup stuff
  #ifdef EEPROM  #ifdef EEPROM
Line 600: Line 568:
  #ifdef EEPROM  #ifdef EEPROM
  I2Caddress=findtop(); //we need to avoid overwiting old data   I2Caddress=findtop(); //we need to avoid overwiting old data
- #endif/*+ #endif
  if(eeprom_read_byte(&flight_status)==0x01) //we are in descent phase - find Heightupto from GPS  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  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  Heightupto=(int)(Gps.altitude/100.0)+1; //c rounds down - we need to add one
- for(L=0;L<Heightupto && ((s08)eeprom_read_byte((u08*)&windeast[L])!=-126);L++)//- do not include present layer in integral hence < in loop terminator 
- { 
- K=driftfactor(L); 
- descent_drift_e+=(float)(s08)eeprom_read_byte((u08*)&windeast[L])*K; //we use this for optimised descent 
- descent_drift_n+=(float)(s08)eeprom_read_byte((u08*)&windnorth[L])*K; 
- }  
  }  }
  else  else
  {  {
- for(L=0;((s08)eeprom_read_byte((u08*)&windeast[L])!=-126) && L<255;L++) //calculate drift coefficients integrating over all non painted eeprom+ for(L=0;((s08)eeprom_read_byte((u08*)&windeast[L])!=-126) && L<255;L++) //calculate heightupto
  {  {
- K=driftfactor(L); 
- descent_drift_e+=(float)(s08)eeprom_read_byte((u08*)&windeast[L])*K; //we use this for optimised descent 
- descent_drift_n+=(float)(s08)eeprom_read_byte((u08*)&windnorth[L])*K; 
  Heightupto++;  Heightupto++;
  }  }
- }*/+ }
  L=0;  L=0;
  rprintfProgStr(PSTR("Setup complete, EEPROM top="));  rprintfProgStr(PSTR("Setup complete, EEPROM top="));
Line 636: Line 595:
   Wind_n += Gps.vnorth;   Wind_n += Gps.vnorth;
   L++;                              //incrament the denominator for our averaging   L++;                              //incrament the denominator for our averaging
-        if (Gps.altitude > Heightupto*100) //do we now fall into another (higher) 100m incrament ?+         if (Gps.altitude > Heightupto*100) //do we now fall into another (higher) 100m incrament ?
   {   {
  Wind_e/=L;  Wind_e/=L;
Line 642: Line 601:
  eeprom_write_byte((u08*)&windeast[Heightupto],(s08)(Wind_e));//we are storing as a signed byte  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  eeprom_write_byte((u08*)&windnorth[Heightupto],(s08)(Wind_n));//we find the average and shove it in the eeprom
- // K=driftfactor(Heightupto); 
- // descent_drift_e+=Wind_e*K; //we use this for optimised descent 
- // descent_drift_n+=Wind_n*K; 
  Heightupto++;   Heightupto++;
  L = 0;  L = 0;
Line 675: Line 631:
  while(time<8 && eeprom_read_byte(&flight_status)!=0x02) //while we havent landed and we havent previously landed  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+ 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_e=(float)(s08)eeprom_read_byte((u08*)&windeast[Heightupto]);
Line 693: Line 649:
  Wind_n=0.0;  Wind_n=0.0;
  }  }
- }/* + }
- if((int)Gps.altitude<=(Heightupto-1)*100) //round up - we need to match up with recorded layers +
- { +
- if((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126)//dont take off any painted eeprom +
- { //this might happen if we are still in the cut layer +
- K=driftfactor(Heightupto); +
- descent_drift_e-=Wind_e*K; //take off layers of wind from our integral +
- descent_drift_n-=Wind_n*K; +
- } +
- Heightupto--; +
- }*/+
  //clockwise is defined as +ive, everything uses the local ENU wind frame / wind in this frame  //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)  //direction to target (uses "equatorial radian units" - my invention, distance equal to PI on equator)
Line 761: Line 707:
  i2cstop();  i2cstop();
  #endif  #endif
- descent_variable_noise+=DESCENT_PROCESS_NOISE;//this is a single component state vector extended kalman filter + L=(int)(Gps.altitude/100.0); 
- if(Gps.vup<-0.5) //sanity check for a descent + if(descent_variable>LOWER_DESCENT_LIMIT && descent_variable<UPPER_DESCENT_LIMIT)//check its sensible
- { //we run if we are descending and estimate the transit time coefficient +
- sqrt_density=sqrt(air_density(Gps.altitude)); +
- K=(-100.0/1012000.0)/(Gps.vup*sqrt_density);//estimate of the proportionality constant +
- sqrt_density=(-GPS_VEL_NOISE*K/Gps.vup)+descent_variable_noise;//reuse this variable +
- descent_variable+=descent_variable_noise*((K-descent_variable)/sqrt_density); +
- descent_variable_noise-=descent_variable_noise*descent_variable_noise/sqrt_density; +
- } +
- for(Heightupto=0;Heightupto<=(u16)(Gps.altitude/100.0)&&((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126);Heightupto++)+
  {  {
- K=descent_variable*sqrt(air_density(100.0*((float)Heightupto-0.5))); + for(Heightupto=0;(Heightupto<=L)&&((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126);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; + 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;
  }  }
- Heightupto++; //set the correct value for obtaining wind vector 
  getgps();  getgps();
  }  }
Line 784: Line 734:
  }  }
  /////////////////////////////Recovery loop  /////////////////////////////Recovery loop
-  
  cbi(TIMSK1, OCIE1B); //turn off kalman  cbi(TIMSK1, OCIE1B); //turn off kalman
  cbi(TCCR1A, COM1B1); //turn off servo  cbi(TCCR1A, COM1B1); //turn off servo
Line 800: Line 749:
  getgps();  getgps();
  }  }
-} +}</code>
- +
-</code> +
- +
- +
  
 ==== Main.h ==== ==== Main.h ====
Line 816: Line 760:
 #include "rprintf.h" #include "rprintf.h"
 #include "i2cmem.h" #include "i2cmem.h"
-#include "sqrt_density.h" 
 #include <avr/io.h> #include <avr/io.h>
 #include <stdlib.h> #include <stdlib.h>
Line 826: Line 769:
 #include <avr/pgmspace.h>  #include <avr/pgmspace.h> 
 #define BAUDRATE (int)9600 //9600 baud for lassen iq #define BAUDRATE (int)9600 //9600 baud for lassen iq
-#define DELTA_TIME (float)0.02 //20ms+#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 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 SET PORTD |= 0x10 //PORTD.4 is the tx
 #define CLEAR PORTD &= ~0x10 #define CLEAR PORTD &= ~0x10
 + 
 //kalman filter - NOTE: these constants are airframe specific //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 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 (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 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 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 PWM_COUNT_TIME (u16)((float)F_CPU*0.0005/8.0) //500us
- +  
-#define I_C -0.001 //control loop+#define I_C -0.001 //control loop
 #define P_C -0.3 #define P_C -0.3
 #define D_C -0.5 #define D_C -0.5
 #define INTEGRAL_LIMIT 0.2/I_C //integral servo shift limited to 1/5 of full scale range #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 SCALEHEIGHT (float)(-1.0/160.0) //optimised descent- 1/( atm scale height in 100m units * 2) - http://en.wikipedia.org/wiki/Scale_height 
-#define WEIGHTINGFACTOR (float)(20.0)/(1012000.0)//time to drop 100m at STP/one radian expressed as meters on earth surface 
-#define GPS_VEL_NOISE 0.25 
-#define DESCENT_PROCESS_NOISE 0.0001 
-  
 #define read_rate (u08)0b10010100 //gyro specific #define read_rate (u08)0b10010100 //gyro specific
 #define read_temp (u08)0b10011100 #define read_temp (u08)0b10011100
 #define read_melexis (u08)0x80 #define read_melexis (u08)0x80
- +  
-#define gyro_null 1012+#define gyro_null 1022
    
 #define altitudelimit 10000 //flight termination conditions #define altitudelimit 10000 //flight termination conditions
 #define timelimit 30 //NOTE: for debug #define timelimit 30 //NOTE: for debug
    
-#define targeteast (float)(-1.0*Deg2rad) //target waypoint ~ Cambridge+#define targeteast (float)(0.1*Deg2rad) //target waypoint ~ Cambridge
 #define targetnorth (float)(52.0*Deg2rad) #define targetnorth (float)(52.0*Deg2rad)
    
 #define Rad2deg 180.0/PI //bloody obvious #define Rad2deg 180.0/PI //bloody obvious
 #define Deg2rad PI/180.0 #define Deg2rad PI/180.0
 + 
 #define PRINT_COMMA rprintfChar(pgm_read_byte(&comma)) //prints a comma #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_factor (float)0.02505 //for measuring battery voltage- checked from test
-#define battery_chan 0x00 //ADC0+#define battery_chan 0x00 //ADC0
    
-#define toggle_pin PIND=0x20 //led on port D.5+#define toggle_pin PIND=0x20 //led on port D.5
    
 #define BAUDDIV  (u16)( ((float)F_CPU/(BAUDRATE*16UL)) -1 )//baud divider #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 radio_cts bit_is_set(PIND,2) //hardware, however its wired up
 + 
 #define led_left PORTC = ((PORTC & ~0x0C) | 0x04) #define led_left PORTC = ((PORTC & ~0x0C) | 0x04)
 #define led_right PORTC = ((PORTC & ~0x0C) | 0x08) #define led_right PORTC = ((PORTC & ~0x0C) | 0x08)
 + 
 #define USER_PRESENT !(PINC&0x02) #define USER_PRESENT !(PINC&0x02)
 + 
 #define cutter_on PORTD|=(1<<7) //release mechanism (hot resistor off mosfet) #define cutter_on PORTD|=(1<<7) //release mechanism (hot resistor off mosfet)
 #define cutter_off PORTD&=~(1<<7) #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 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 ENABLE_SMPS PORTD&=~(1<<6)
 + 
 #define GYRO_OFF PORTB|=1<<1 //SS line #define GYRO_OFF PORTB|=1<<1 //SS line
 #define GYRO_ON PORTB&=~(1<<1) #define GYRO_ON PORTB&=~(1<<1)
 + 
 #ifdef ADCSRA #ifdef ADCSRA
  #ifndef ADCSR  #ifndef ADCSR
Line 898: Line 841:
 #endif #endif
 #ifdef GROUND #ifdef GROUND
- #define enable_ground_control TIMSK1|=(1<<ICIE1) //enables the input capture interrupt + void enable_ground_control(); //enables the input capture interrupt 
- #define disable_ground_control TIMSK1&=~(1<<ICIE1) //disables input capture interrupt + void disable_ground_control(); //disables input capture interrupt 
- #define PULSE_MIN 0.001*F_CPU/8 //pwm must be in the range 1 to 2 ms + #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 0.002*F_CPU/8+ #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_ON PORTD|=(1<<3)
  #define GROUND_LED_OFF PORTD&=~(1<<3)  #define GROUND_LED_OFF PORTD&=~(1<<3)
  #define GROUND_LED_SET PORTD&(1<<3)  #define GROUND_LED_SET PORTD&(1<<3)
 + void run_ground_control();
 #endif #endif
    
Line 926: Line 870:
 void _delay_10ms(char time); void _delay_10ms(char time);
 float driftfactor(int altitude); float driftfactor(int altitude);
 +u08 I2Cdataprint(u32* I2Caddress);
 </code> </code>
code/parafoil_tsip.1231991295.txt.gz · Last modified: 2009/01/15 03:48 by laurenceb
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki