X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=parse;h=47f3ced25d5ab2d26377fdd57d7314900cee181a;hb=fa1c4b2623d07418992de1b177f067a00322701b;hp=97344d32a39b0c6240e8917c7276468c23926d32;hpb=8d1ed1bed19b13fb8b178433db11637c7ae0517a;p=fw%2Ftmflights

diff --git a/parse b/parse
index 97344d3..47f3ced 100755
--- a/parse
+++ b/parse
@@ -199,8 +199,9 @@ read_record(file in) {
 	return r;
 }
 
-#real g_count = 264.8;
-real g_count = 262;
+real g_count = 264.8;
+#real g_count = 262;
+#real g_count = 400;
 int g_base = 15735;
 
 real
@@ -224,7 +225,7 @@ real sinc(real x) = x != 0 ? sin(x)/x : 1;
 
 real gaussian(real x) = exp(-(x**2)/2) / sqrt(2 * pi);
 
-load "/usr/share/nickle/examples/kaiser.5c"
+load "filter.5c"
 
 real[...] convolve(real[...] d, real[...] e) {
 	real sample(n) = n < 0 ? d[0] : n >= dim(d) ? d[dim(d)-1] : d[n];
@@ -297,9 +298,13 @@ autoimport String;
 
 telem_record read_telem(file in) {
 	string[*]	r = wordsplit(chomp(fgets(in)), " ");
+	static int line = 0;
 
-	if (dim(r) < 15)
+	line++;
+	if (dim(r) < 15) {
+		printf ("invalid record line %d\n", line);
 		return read_telem(in);
+	}
 	return (telem_record) {
 		.time = string_to_integer(r[10]),
 		.accel = string_to_integer(r[12]),
@@ -309,9 +314,14 @@ telem_record read_telem(file in) {
 }
 
 void readsamples_telem(file in) {
+	telem_record[...] telem;
+
+	setdim(telem, 0);
+
 	setdim(clock, 0);
 	setdim(pressure_value, 0);
 	setdim(accelerometer_value, 0);
+	real clock_bias = 0;
 
 	telem_record[...] save = {};
 	
@@ -328,20 +338,32 @@ void readsamples_telem(file in) {
 		accel_total += save[i].accel;
 	g_base = accel_total // start;
 
-	for (int i = start; i < dim(save); i++) {
-		clock[dim(clock)] = save[i].time/100;
-		pressure_value[dim(pressure_value)] = save[i].pressure;
-		accelerometer_value[dim(accelerometer_value)] = save[i].accel;
-	}
-	
+	for (int i = start; i < dim(save); i++)
+		telem[dim(telem)] = save[i];
+
 	while (!File::end(in)) {
-		telem_record t = read_telem(in);
-		if (dim(clock) > 0 &&
-		    abs(t.time / 100 - clock[dim(clock)-1]) > 500)
-			break;
-		clock[dim(clock)] = t.time / 100;
-		pressure_value[dim(pressure_value)] = t.pressure;
-		accelerometer_value[dim(accelerometer_value)] = t.accel;
+		int n = dim(telem);
+		telem[n] = read_telem(in);
+		telem[n].time += clock_bias;
+		if (n > 0 && telem[n].time < telem[n-1].time) {
+			clock_bias += 65536;
+			telem[n].time += 65536;
+		}
+	}
+	int clock_start = telem[0].time;
+	int clock_end = telem[dim(telem)-1].time;
+	int samples = clock_end - clock_start;
+
+	int j = 0;
+	for (int i = 0; i < samples; i++) {
+		clock[i] = i / 100;
+		pressure_value[i] = telem[j].pressure;
+		accelerometer_value[i] = telem[j].accel;
+		if (j < dim(telem)-1) {
+			int cur_time = clock_start + i;
+			if (cur_time - telem[j].time > telem[j+1].time - cur_time)
+				j++;
+		}
 	}
 }
 
@@ -372,6 +394,12 @@ int[...] rebase(int[...] d, int m, int a) = (int[dim(d)]) { [n] = d[n] * m + a }
 
 int size = dim(accelerometer_value);
 
+real[...] do_low_pass(real[] data, real ωpass, real ωstop, real error) {
+	real[*] fir = low_pass_filter (ωpass, ωstop, error);
+	File::fprintf (stderr, "low pass filter is %d long\n", dim(fir));
+	return convolve(data, fir);
+}
+
 if (false) {
 	accelerometer_value = rebase(accelerometer_value, -1, g_base);
 	int accel_i0_base = average(accelerometer_value, 30);
@@ -397,8 +425,14 @@ if (false) {
 } else {
 	real[size] accelerometer = { [n] = gravity * (count_to_g(accelerometer_value[n]) - 1.0) };
 	real[size] barometer = { [n] = pressure_to_altitude(count_to_kPa(pressure_value[n] / 16) * 1000) };
-	real[size] filtered_accelerometer = kaiser_filter(accelerometer, 8);
-	real[size] filtered_barometer = kaiser_filter(barometer, 32);
+	real[size] filtered_accelerometer = do_low_pass(accelerometer,
+							2 * π * 5/100,
+							2 * π * 8/100,
+							1e-8);
+	real[size] filtered_barometer = do_low_pass(barometer,
+						    2 * π * .5 / 100,
+						    2 * π * 1 / 100,
+						    1e-8);
 
 	real[...] integrate(real[...] d) {
 		real[dim(d)] ret;
@@ -415,7 +449,7 @@ if (false) {
 		return ret;
 	}
 
-	real[size] accel_speed = integrate(filtered_accelerometer);
+	real[size] accel_speed = integrate(accelerometer);
 	real[size] accel_pos = integrate(accel_speed);
 	real[size] baro_speed = differentiate(filtered_barometer);
 	real[size] baro_accel = differentiate(baro_speed);