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];
}
void readsamples_telem(file in) {
+ telem_record[...] telem;
+
+ setdim(telem, 0);
+
setdim(clock, 0);
setdim(pressure_value, 0);
setdim(accelerometer_value, 0);
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;
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);
} 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;