Airfest 2013: Bdale's new airframe on Loki M with TMv1 and Tmega

[fw/tmflights] / parse

diff --git a/parse b/parse
index 7593b386d6503d5ef9fd5471f2c9755c8c25f013..47f3ced25d5ab2d26377fdd57d7314900cee181a 100755 (executable)
--- a/parse
+++ b/parse
@@ -225,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];
@@ -314,6 +314,10 @@ 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);
@@ -334,27 +338,36 @@ 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);
-               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;
+               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++;
                }
-               clock[n] = sample_time;
-               pressure_value[dim(pressure_value)] = t.pressure;
-               accelerometer_value[dim(accelerometer_value)] = t.accel;
        }
 }
 
-readsamples_telem(stdin);
+readsamples_log(stdin);
 
 int[...] int_integrate(int[...] d, int base) {
        int v = 0;
@@ -381,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);
@@ -406,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, 16);
+       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;