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:52] – old revision restored 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 300: Line 300:
  }  }
 } }
 + 
 void wiggleservo() void wiggleservo()
 { {
Line 332: Line 332:
  while(!Gps.packetflag); //wait for some new gps  while(!Gps.packetflag); //wait for some new gps
 } }
 + 
 void dataprint(u08 flight_status,volatile gps_type * gps) void dataprint(u08 flight_status,volatile gps_type * gps)
 { {
Line 395: Line 395:
  return ;   return ;
 } }
 + 
 void put_char(unsigned char c) void put_char(unsigned char c)
 { {
Line 708: Line 708:
  #endif  #endif
  L=(int)(Gps.altitude/100.0);  L=(int)(Gps.altitude/100.0);
- for(Heightupto=0;(Heightupto<=L)&&((s08)eeprom_read_byte((u08*)&windeast[Heightupto])!=-126);Heightupto++)+ if(descent_variable>LOWER_DESCENT_LIMIT && descent_variable<UPPER_DESCENT_LIMIT)//check its sensible
  {  {
- K=descent_variable*(float)sqrt_density[Heightupto];  + 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++;+ 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  descent_variable_noise+=DESCENT_PROCESS_NOISE; //this is a single component state vector extended kalman filter
- if(Gps.vup<-0.5) //sanity check for a descent + 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 + { //we run if we are descending and estimate the transit time coefficient 
- b=(-100.0)/(6378000.0*Gps.vup*(float)sqrt_density[Heightupto]);//estimate of the proportionality constant + b=(float)pgm_read_byte(&sqrt_density[Heightupto]); 
- K=descent_variable_noise/((-GPS_VEL_NOISE*b/Gps.vup)+descent_variable_noise);//this is K+ 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+=K*(b-descent_variable);
  descent_variable_noise-=descent_variable_noise*K;  descent_variable_noise-=descent_variable_noise*K;
Line 745: Line 749:
  getgps();  getgps();
  }  }
-} +}</code>
-</code> +
- +
- +
  
 ==== Main.h ==== ==== Main.h ====
Line 760: 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 770: 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 842: 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 870: 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.1231991525.txt.gz · Last modified: 2009/01/15 03:52 by laurenceb
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki