-real[size] accel_speed = integrate(filtered_accelerometer);
-real[size] accel_pos = integrate(accel_speed);
-real[size] baro_speed = differentiate(filtered_barometer);
-real[size] baro_accel = differentiate(baro_speed);
-
-for (int i = 0; i < size; i++)
- printf("%g %g %g %g %g %g %g %g %g\n",
- clock[i] - clock[0],
- filtered_barometer[i] - filtered_barometer[0], accel_pos[i],
- baro_speed[i], accel_speed[i],
- baro_accel[i], filtered_accelerometer[i],
- barometer[i] - barometer[0], accelerometer[i]);
+if (false) {
+ accelerometer_value = rebase(accelerometer_value, -1, g_base);
+ int accel_i0_base = average(accelerometer_value, 30);
+ int[size] pres_d0 = int_filter(pressure_value, 4);
+ int[size] accel_i0 = int_filter(accelerometer_value, 4);
+ int[size] pres_d1 = int_filter(int_differentiate(pres_d0), 4);
+ int[size] accel_i1 = int_integrate(accelerometer_value, accel_i0_base);
+ int[size] pres_d2 = int_filter(int_differentiate(pres_d1), 4);
+ int[size] accel_i2 = int_integrate(accel_i1, 0);
+
+ real count_to_altitude(int count) = pressure_to_altitude(count_to_kPa(count / 16) * 1000);
+
+ for (int i = 0; i < size; i++)
+ printf("%g %g %g %g %g %g %g %g %g\n",
+ clock[i] - clock[0],
+ count_to_altitude(pres_d0[i]) - count_to_altitude(pres_d0[0]), accel_i2[i] / 10000 / g_count * gravity,
+ pres_d1[i] * 100, accel_i1[i] / 100 / g_count * gravity,
+ pres_d2[i] * 10000, accel_i0[i] / g_count * gravity,
+ count_to_altitude(pressure_value[i]) -
+ count_to_altitude(pressure_value[0]), accelerometer_value[i]
+ / g_count * gravity);
+
+} 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 = 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;
+ for (int i = 0; i < dim(ret); i++)
+ ret[i] = i == 0 ? 0 : ret[i-1] + (d[i-1] + d[i]) / 2 * (clock[i] - clock[i-1]);
+ return ret;
+ }
+
+ real[...] differentiate(real[...] d) {
+ real[dim(d)] ret;
+ for (int i = 1; i < dim(ret); i++)
+ ret[i] = (d[i] - d[i-1]) / (clock[i] - clock[i-1]);
+ ret[0] = ret[1];
+ return ret;
+ }
+
+ 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("%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],
+ baro_accel[i], filtered_accelerometer[i],
+ barometer[i] - barometer[0], accelerometer[i]);
+}