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
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];
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]),
}
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 = {};
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++;
+ }
}
}
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, 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;
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);