Differences

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

--- code:parafoil_tsip [2009/01/16 16:11] – laurenceb
+++ code:parafoil_tsip [2009/02/26 02:19] (current) – laurenceb
@@ Line 421: / Line 421: @@
 	u08 Heightupto=1;					//this rounds up
 	float K;
-	float b,descent_variable=DESCENT_INIT,descent_variable_noise=DESCENT_NOISE_INIT;
+	float b,descent_variable=0.05882,descent_variable_noise=0.002;
 	float descent_drift_n=0;
 	float descent_drift_e=0;
@@ Line 708: / Line 708: @@
 		#endif
 		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)pgm_read_byte(&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
-		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=(float)pgm_read_byte(&sqrt_density[Heightupto]);
-			K=descent_variable_noise/((GPS_VEL_NOISE*b*b*b*b)+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);
@@ Line 747: / Line 750: @@
 	}
 }</code>
 ==== Main.h ====
@@ Line 768: / Line 769: @@
 #include <avr/pgmspace.h>
 #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 SET	PORTD |= 0x10						//PORTD.4 is the tx
@@ Line 781: / Line 782: @@
 #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 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))
+#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-18
+#define DESCENT_PROCESS_NOISE 1.0e-22				//tested with badger data
 #define DESCENT_INIT 1.5e-8
 #define DESCENT_NOISE_INIT 5.0e-17
@@ Line 809: / Line 810: @@
 #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