X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=Misc%2Fparse;fp=Misc%2Fparse;h=47f3ced25d5ab2d26377fdd57d7314900cee181a;hb=b7adf88eaf34060be679b93d0761d345543e3e97;hp=0000000000000000000000000000000000000000;hpb=77b1bf3a9c62d0ffe73be18905e26e5f8087ec4a;p=fw%2Ftmflights diff --git a/Misc/parse b/Misc/parse new file mode 100755 index 0000000..47f3ced --- /dev/null +++ b/Misc/parse @@ -0,0 +1,474 @@ +#!/usr/bin/env nickle + +/* + * Pressure Sensor Model, version 1.1 + * + * written by Holly Grimes + * + * Uses the International Standard Atmosphere as described in + * "A Quick Derivation relating altitude to air pressure" (version 1.03) + * from the Portland State Aerospace Society, except that the atmosphere + * is divided into layers with each layer having a different lapse rate. + * + * Lapse rate data for each layer was obtained from Wikipedia on Sept. 1, 2007 + * at site MAXIMUM_ALTITUDE) /* FIX ME: use sensor data to improve model */ + return 0; + + /* calculate the base temperature and pressure for the atmospheric layer + associated with the inputted altitude */ + for(layer_number = 0; layer_number < NUMBER_OF_LAYERS - 1 && altitude > base_altitude[layer_number + 1]; layer_number++) { + delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + base_pressure *= exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + base_pressure *= pow(base, exponent); + } + base_temperature += delta_z * lapse_rate[layer_number]; + } + + /* calculate the pressure at the inputted altitude */ + delta_z = altitude - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + pressure = base_pressure * exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + pressure = base_pressure * pow(base, exponent); + } + + return pressure; +} + + +/* outputs the altitude associated with the given pressure. the altitude + returned is measured with respect to the mean sea level */ +real pressure_to_altitude(real pressure) { + + real next_base_temperature = LAYER0_BASE_TEMPERATURE; + real next_base_pressure = LAYER0_BASE_PRESSURE; + + real altitude; + real base_pressure; + real base_temperature; + real base; /* base for function to determine base pressure of next layer */ + real exponent; /* exponent for function to determine base pressure + of next layer */ + real coefficient; + int layer_number; /* identifies layer in the atmosphere */ + int delta_z; /* difference between two altitudes */ + + if (pressure < 0) /* illegal pressure */ + return -1; + if (pressure < MINIMUM_PRESSURE) /* FIX ME: use sensor data to improve model */ + return MAXIMUM_ALTITUDE; + + /* calculate the base temperature and pressure for the atmospheric layer + associated with the inputted pressure. */ + layer_number = -1; + do { + layer_number++; + base_pressure = next_base_pressure; + base_temperature = next_base_temperature; + delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + next_base_pressure *= exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + next_base_pressure *= pow(base, exponent); + } + next_base_temperature += delta_z * lapse_rate[layer_number]; + } + while(layer_number < NUMBER_OF_LAYERS - 1 && pressure < next_base_pressure); + + /* calculate the altitude associated with the inputted pressure */ + if (lapse_rate[layer_number] == 0.0) { + coefficient = (AIR_GAS_CONSTANT / GRAVITATIONAL_ACCELERATION) + * base_temperature; + altitude = base_altitude[layer_number] + + coefficient * log(pressure / base_pressure); + } + else { + base = pressure / base_pressure; + exponent = AIR_GAS_CONSTANT * lapse_rate[layer_number] + / GRAVITATIONAL_ACCELERATION; + coefficient = base_temperature / lapse_rate[layer_number]; + altitude = base_altitude[layer_number] + + coefficient * (pow(base, exponent) - 1); + } + + return altitude; +} + +real feet_to_meters(real feet) +{ + return feet * (12 * 2.54 / 100); +} + +real meters_to_feet(real meters) +{ + return meters / (12 * 2.54 / 100); +} + +/* + * Values for our MP3H6115A pressure sensor + * + * From the data sheet: + * + * Pressure range: 15-115 kPa + * Voltage at 115kPa: 2.82 + * Output scale: 27mV/kPa + * + * + * 27 mV/kPa * 2047 / 3300 counts/mV = 16.75 counts/kPa + * 2.82V * 2047 / 3.3 counts/V = 1749 counts/115 kPa + */ + +real counts_per_kPa = 27 * 2047 / 3300; +real counts_at_101_3kPa = 1674; + +real count_to_kPa(real count) +{ + return (count / 2047 + 0.095) / 0.009; +} +typedef struct { + int type; + int time; + int a; + int b; +} flight_record; + +flight_record +read_record(file in) { + flight_record r; + File::fscanf(in, "%c %x %x %x\n", + &r.type, &r.time, &r.a, &r.b); + return r; +} + +real g_count = 264.8; +#real g_count = 262; +#real g_count = 400; +int g_base = 15735; + +real +count_to_g(real count) +{ + return (g_base + g_count - count) / g_count; +} + +real base_alt = 0; +real base_sec = 0; +real last_alt; +real last_sec; +real last_alt_speed; +real accel_speed; +real accel_meters; + +real[...] barometer; +real[...] accelerometer; + +real sinc(real x) = x != 0 ? sin(x)/x : 1; + +real gaussian(real x) = exp(-(x**2)/2) / sqrt(2 * pi); + +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]; + real w = (dim(e) - 1) / 2; + real c(int center) { + real v = 0; + for (int o = -w; o <= w; o++) + v += sample(center + o) * e[o + w]; + return v; + } + return (real[dim(d)]) { [n] = c(n) }; +} + +real sum(real[...] x) { real s = 0; for(int i = 0; i < dim(x); i++) s += x[i]; return s; } + +real[...] kaiser_filter(real[...] d, int half_width) { +# real[half_width * 2 + 1] fir = { [n] = sinc(2 * pi * n / (2 * half_width)) }; + real M = half_width * 2 + 1; + real[M] fir = { [n] = kaiser(n, M, 8) }; + real fir_sum = sum(fir); + for (int i = 0; i < dim(fir); i++) fir[i] /= fir_sum; + return convolve(d, fir); +} + +int[...] int_filter(int[...] d, int shift) { + /* Emulate the exponential IIR filter used in the TeleMetrum flight + software */ + + int v = d[0]; + int n; + int[dim(d)] ret; + + for (n = 0; n < dim(d); n++) { + v -= (v + (1 << (shift - 1))) >> shift; + v += (d[n] + (1 << (shift - 1))) >> shift; + ret[n] = v; + } + return ret; +} + +real gravity = 9.80665; + +int[...] pressure_value, accelerometer_value; +real[...] clock; + +void readsamples_log(file in) { + setdim(pressure_value, 0); + setdim(accelerometer_value, 0); + while (!File::end(in)) { + flight_record r = read_record(in); + if (r.type == 'F') { + g_base = r.a; + } + if (r.type == 'A') { + clock[dim(clock)] = r.time / 100; + pressure_value[dim(pressure_value)] = r.b; + accelerometer_value[dim(accelerometer_value)] = r.a; + } + } +} + +typedef struct { + int time; + int accel; + int pressure; + string state; +} telem_record; + +autoimport String; + +telem_record read_telem(file in) { + string[*] r = wordsplit(chomp(fgets(in)), " "); + static int line = 0; + + 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]), + .pressure = string_to_integer(r[14]), + .state = r[9] + }; +} + +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 = {}; + + setdim(save, 0); + while (!File::end(in)) { + save[dim(save)] = read_telem(in); + if (save[dim(save)-1].state == "boost") + break; + } + int start = dim(save) - 4; + + int accel_total = 0; + for (int i = 0; i < start; i++) + accel_total += save[i].accel; + g_base = accel_total // start; + + for (int i = start; i < dim(save); i++) + telem[dim(telem)] = save[i]; + + while (!File::end(in)) { + 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++; + } + } +} + +readsamples_log(stdin); + +int[...] int_integrate(int[...] d, int base) { + int v = 0; + int[dim(d)] ret; + + ret[0] = 0; + for (int i = 1; i < dim(d); i++) + ret[i] = (v += (d[i-1] + d[i] + 1) // 2); + return ret; +} + +int[...] int_differentiate(int[...] d) { + return (int[dim(d)]) { [n] = n == 0 ? 0 : d[n] - d[n-1] }; +} + +int average(int[...] d, int n) { + int sum = 0; + for (int i = 0; i < n; i++) + sum += d[n]; + return sum // n; +} + +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); + 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]); +}