Integrate raw accel data. Read whole telem file

[fw/tmflights] / parse

diff --git a/parse b/parse
index cd447bbeeacfa9cd7cd89e7ced65b005f1a9073b..7593b386d6503d5ef9fd5471f2c9755c8c25f013 100755 (executable)
--- a/parse
+++ b/parse
@@ -200,6 +200,8 @@ read_record(file in) {
 }
 
 real g_count = 264.8;
+#real g_count = 262;
+#real g_count = 400;
 int g_base = 15735;
 
 real
@@ -296,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]),
@@ -311,6 +317,7 @@ void readsamples_telem(file in) {
        setdim(clock, 0);
        setdim(pressure_value, 0);
        setdim(accelerometer_value, 0);
+       real clock_bias = 0;
 
        telem_record[...] save = {};
        
@@ -335,16 +342,19 @@ void readsamples_telem(file in) {
        
        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;
+               int n = dim(clock);
+               real sample_time = t.time / 100 + clock_bias;
+               if (n > 0 && sample_time < clock[n-1]) {
+                       clock_bias += 65536 / 100;
+                       sample_time += 65536 / 100;
+               }
+               clock[n] = sample_time;
                pressure_value[dim(pressure_value)] = t.pressure;
                accelerometer_value[dim(accelerometer_value)] = t.accel;
        }
 }
 
-readsamples_log(stdin);
+readsamples_telem(stdin);
 
 int[...] int_integrate(int[...] d, int base) {
        int v = 0;
@@ -397,7 +407,7 @@ if (false) {
        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, 128);
+       real[size] filtered_barometer = kaiser_filter(barometer, 16);
 
        real[...] integrate(real[...] d) {
                real[dim(d)] ret;
@@ -414,13 +424,23 @@ 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);
 
+       printf("%7s %12s %12s %12s %12s %12s %12s %12s %12s\n",
+              "time",
+              "height(baro)",
+              "height(accel)",
+              "speed(baro)",
+              "speed(accel)",
+              "accel(baro)",
+              "accel(accel)",
+              "raw(baro)",
+              "raw(accel)");
        for (int i = 0; i < size; i++)
-               printf("%g %g %g %g %g %g %g %g %g\n",
+               printf("%7.2f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f %12.6f\n",
                       clock[i] - clock[0],
                       filtered_barometer[i] - filtered_barometer[0], accel_pos[i],
                       baro_speed[i], accel_speed[i],