From dc7216d286cc7fe8007f5208ad97a630166572f3 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 21 Sep 2012 13:29:17 +0200 Subject: altos: Shrink Pa to altitude table This improves the computation of the table enough that errors from a 470 entry table are almost all < 0.5m. Signed-off-by: Keith Packard --- src/util/make-altitude-pa | 78 ++++++++++++++++++++++++++++------------------- 1 file changed, 46 insertions(+), 32 deletions(-) (limited to 'src/util') diff --git a/src/util/make-altitude-pa b/src/util/make-altitude-pa index 190b36fc..eae5ebe9 100644 --- a/src/util/make-altitude-pa +++ b/src/util/make-altitude-pa @@ -29,10 +29,10 @@ const real LAYER0_BASE_PRESSURE = 101325; /* lapse rate and base altitude for each layer in the atmosphere */ const real[NUMBER_OF_LAYERS] lapse_rate = { - -0.0065, 0.0, 0.001, 0.0028, 0.0, -0.0028, -0.002 + -0.0065, 0.0, 0.001, 0.0028, 0.0, -0.0028, -0.002, }; const int[NUMBER_OF_LAYERS] base_altitude = { - 0, 11000, 20000, 32000, 47000, 51000, 71000 + 0, 11000, 20000, 32000, 47000, 51000, 71000, }; @@ -54,7 +54,7 @@ real altitude_to_pressure(real altitude) { /* 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++) { + for(layer_number = 0; layer_number < NUMBER_OF_LAYERS - 2 && 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 @@ -113,7 +113,7 @@ real pressure_to_altitude(real pressure) { /* calculate the base temperature and pressure for the atmospheric layer associated with the inputted pressure. */ layer_number = -1; - do { + while (layer_number < NUMBER_OF_LAYERS - 2) { layer_number++; base_pressure = next_base_pressure; base_temperature = next_base_temperature; @@ -130,8 +130,9 @@ real pressure_to_altitude(real pressure) { next_base_pressure *= pow(base, exponent); } next_base_temperature += delta_z * lapse_rate[layer_number]; + if (pressure >= next_base_pressure) + break; } - 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) { @@ -148,20 +149,9 @@ real pressure_to_altitude(real pressure) { 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 MS5607 * @@ -174,14 +164,15 @@ real meters_to_feet(real meters) typedef struct { real m, b; - int m_i, b_i; } line_t; +/* + * Linear least-squares fit values in the specified array + */ line_t best_fit(real[] values, int first, int last) { real sum_x = 0, sum_x2 = 0, sum_y = 0, sum_xy = 0; int n = last - first + 1; real m, b; - int m_i, b_i; for (int i = first; i <= last; i++) { sum_x += i; @@ -197,9 +188,10 @@ line_t best_fit(real[] values, int first, int last) { real min_Pa = 0; real max_Pa = 120000; -/* Target is an array of < 2000 entries */ -int pa_sample_shift = 3; -int pa_part_shift = 3; +/* Target is an array of < 1000 entries */ +int pa_sample_shift = 2; +int pa_part_shift = 6; +int pa_part_mask = (1 << pa_part_shift) - 1; int num_part = ceil(max_Pa / (2 ** (pa_part_shift + pa_sample_shift))); @@ -211,6 +203,10 @@ real sample_to_altitude(int sample) = pressure_to_altitude(sample_to_Pa(sample)) int part_to_sample(int part) = part << pa_part_shift; +int sample_to_part(int sample) = sample >> pa_part_shift; + +bool is_part(int sample) = (sample & pa_part_mask) == 0; + real[num_samples] alt = { [n] = sample_to_altitude(n) }; int seg_len = 1 << pa_part_shift; @@ -219,18 +215,22 @@ line_t [num_part] fit = { [n] = best_fit(alt, n * seg_len, n * seg_len + seg_len - 1) }; -int[num_samples/seg_len + 1] alt_part; +real[num_samples/seg_len + 1] alt_part; +real[dim(alt_part)] alt_error = {0...}; -alt_part[0] = floor (fit[0].b + 0.5); -alt_part[dim(fit)] = floor(fit[dim(fit)-1].m * dim(fit) * seg_len + fit[dim(fit)-1].b + 0.5); +alt_part[0] = fit[0].b; +alt_part[dim(fit)] = fit[dim(fit)-1].m * dim(fit) * seg_len + fit[dim(fit)-1].b; for (int i = 0; i < dim(fit) - 1; i++) { real here, there; here = fit[i].m * (i+1) * seg_len + fit[i].b; there = fit[i+1].m * (i+1) * seg_len + fit[i+1].b; - alt_part[i+1] = floor ((here + there) / 2 + 0.5); +# printf ("at %d mis-fit %8.2f\n", i, there - here); + alt_part[i+1] = (here + there) / 2; } +real round(real x) = floor(x + 0.5); + real sample_to_fit_altitude(int sample) { int sub = sample // seg_len; int off = sample % seg_len; @@ -239,7 +239,7 @@ real sample_to_fit_altitude(int sample) { real i_v; r_v = sample * l.m + l.b; - i_v = (alt_part[sub] * (seg_len - off) + alt_part[sub+1] * off) / seg_len; + i_v = (round(alt_part[sub]) * (seg_len - off) + round(alt_part[sub+1]) * off) / seg_len; return i_v; } @@ -249,27 +249,41 @@ real total_error = 0; for (int sample = 0; sample < num_samples; sample++) { real Pa = sample_to_Pa(sample); - real meters = pressure_to_altitude(Pa); + real meters = alt[sample]; real meters_approx = sample_to_fit_altitude(sample); real error = abs(meters - meters_approx); + int part = sample_to_part(sample); + + if (error > alt_error[part]) + alt_error[part] = error; + total_error += error; if (error > max_error) { max_error = error; max_error_sample = sample; } -# printf (" %7d, /* %6.2f kPa %5d sample approx %d */\n", -# floor (meters + 0.5), Pa / 1000, sample, floor(sample_to_fit_altitude(sample) + 0.5)); + if (false) { + printf (" %8.1f %8.2f %8.2f %8.2f %s\n", + Pa, + meters, + meters_approx, + meters - meters_approx, + is_part(sample) ? "*" : ""); + } } -printf ("/*max error %f at %7.3f%%. Average error %f*/\n", max_error, max_error_sample / (num_samples - 1) * 100, total_error / num_samples); +printf ("/*max error %f at %7.3f kPa. Average error %f*/\n", + max_error, sample_to_Pa(max_error_sample) / 1000, total_error / num_samples); printf ("#define NALT %d\n", dim(alt_part)); printf ("#define ALT_SHIFT %d\n", pa_part_shift + pa_sample_shift); +printf ("#ifndef SATURATE\n#define SATURATE(x) (x)\n#endif\n"); for (int part = 0; part < dim(alt_part); part++) { real kPa = sample_to_Pa(part_to_sample(part)) / 1000; - printf ("%9d, /* %6.2f kPa */\n", - alt_part[part], kPa); + printf ("SATURATE(%9d), /* %6.2f kPa error %6.2fm */\n", + round (alt_part[part]), kPa, + alt_error[part]); } -- cgit v1.2.3 From 68308908afbd1f04b17056d2be408c89b3578c86 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 12 Oct 2012 13:59:50 -0700 Subject: altos: Parameterize altitude table access and initialization This allows projects to store the altitude data in different representations or with different access modes. By default, altitude data is stored in meters, but the initializers include decimeter values so those can be used instead if desired. Signed-off-by: Keith Packard --- src/core/ao_convert_pa.c | 10 +++++++++- src/util/make-altitude-pa | 10 +++++----- 2 files changed, 14 insertions(+), 6 deletions(-) (limited to 'src/util') diff --git a/src/core/ao_convert_pa.c b/src/core/ao_convert_pa.c index 1413681d..2793b77c 100644 --- a/src/core/ao_convert_pa.c +++ b/src/core/ao_convert_pa.c @@ -19,10 +19,18 @@ #include "ao.h" #endif -static const int32_t altitude_table[] = { +#ifndef AO_CONST_ATTRIB +#define AO_CONST_ATTRIB +#endif + +static const alt_t altitude_table[] AO_CONST_ATTRIB = { #include "altitude-pa.h" }; +#ifndef FETCH_ALT +#define FETCH_ALT(o) altitude_table[o] +#endif + #define ALT_SCALE (1 << ALT_SHIFT) #define ALT_MASK (ALT_SCALE - 1) diff --git a/src/util/make-altitude-pa b/src/util/make-altitude-pa index eae5ebe9..22831d50 100644 --- a/src/util/make-altitude-pa +++ b/src/util/make-altitude-pa @@ -239,8 +239,8 @@ real sample_to_fit_altitude(int sample) { real i_v; r_v = sample * l.m + l.b; - i_v = (round(alt_part[sub]) * (seg_len - off) + round(alt_part[sub+1]) * off) / seg_len; - return i_v; + i_v = (round(alt_part[sub]*10) * (seg_len - off) + round(alt_part[sub+1]*10) * off) / seg_len; + return i_v/10; } real max_error = 0; @@ -279,11 +279,11 @@ printf ("/*max error %f at %7.3f kPa. Average error %f*/\n", printf ("#define NALT %d\n", dim(alt_part)); printf ("#define ALT_SHIFT %d\n", pa_part_shift + pa_sample_shift); -printf ("#ifndef SATURATE\n#define SATURATE(x) (x)\n#endif\n"); +printf ("#ifndef AO_ALT_VALUE\n#define AO_ALT_VALUE(x) (alt_t) (x)\n#endif\n"); for (int part = 0; part < dim(alt_part); part++) { real kPa = sample_to_Pa(part_to_sample(part)) / 1000; - printf ("SATURATE(%9d), /* %6.2f kPa error %6.2fm */\n", - round (alt_part[part]), kPa, + printf ("AO_ALT_VALUE(%10.1f), /* %6.2f kPa error %6.2fm */\n", + round (alt_part[part]*10) / 10, kPa, alt_error[part]); } -- cgit v1.2.3 From 79f4e684713cff6bf999cac52f5d9525a6f7d278 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 22 Oct 2012 21:39:12 -0700 Subject: altos: make check-avr-mem utility executable Signed-off-by: Keith Packard --- src/util/check-avr-mem | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 src/util/check-avr-mem (limited to 'src/util') diff --git a/src/util/check-avr-mem b/src/util/check-avr-mem old mode 100644 new mode 100755 -- cgit v1.2.3 From 249ee968305ae6e8fcf0a10e5cf9cc5826bd81dd Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 Jan 2013 15:13:31 -0800 Subject: altos: Add computation of MicroPeak Kalman correction coefficients Signed-off-by: Keith Packard --- src/Makefile | 1 + src/kalman/kalman_micro.5c | 329 +++++++++++++++++++++++++++++++++++++++++++++ src/kalman/load_csv.5c | 21 ++- src/util/make-kalman | 2 + 4 files changed, 349 insertions(+), 4 deletions(-) create mode 100644 src/kalman/kalman_micro.5c (limited to 'src/util') diff --git a/src/Makefile b/src/Makefile index 473cc60a..67ad97d1 100644 --- a/src/Makefile +++ b/src/Makefile @@ -8,6 +8,7 @@ vpath make-kalman util vpath make-whiten util vpath kalman.5c kalman vpath kalman_filter.5c kalman +vpath kalman_micro.5c kalman vpath load_csv.5c kalman vpath matrix.5c kalman diff --git a/src/kalman/kalman_micro.5c b/src/kalman/kalman_micro.5c new file mode 100644 index 00000000..266a1182 --- /dev/null +++ b/src/kalman/kalman_micro.5c @@ -0,0 +1,329 @@ +#!/usr/bin/env nickle + +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +autoimport ParseArgs; + +load "load_csv.5c" +import load_csv; + +load "matrix.5c" +import matrix; + +load "kalman_filter.5c" +import kalman; + +load "../util/atmosphere.5c" + +real height_scale = 1.0; +real accel_scale = 1.0; +real speed_scale = 1.0; + +/* + * State: + * + * x[0] = pressure + * x[1] = delta pressure + * x[2] = delta delta pressure + */ + +/* + * Measurement + * + * z[0] = pressure + */ + +real default_σ_m = 5; +real default_σ_h = 2.4; /* pascals */ + +real[3,3] model_error(t, Φ) = multiply_mat_val ((real[3,3]) { + { t**5 / 20, t**4 / 8, t**3 / 6 }, + { t**4 / 8, t**3 / 3, t**2 / 2 }, + { t**3 / 6, t**2 / 2, t } + }, Φ); + +parameters_t param_baro(real t, real σ_m, real σ_h) { + if (σ_m == 0) { + printf ("Using default σ_m\n"); + σ_m = default_σ_m; + } + if (σ_h == 0) { + printf ("Using default σ_h\n"); + σ_h = default_σ_h; + } + + σ_m = imprecise(σ_m) * accel_scale; + σ_h = imprecise(σ_h) * height_scale; + + t = imprecise(t); + return (parameters_t) { +/* + * Equation computing state k from state k-1 + * + * height = height- + velocity- * t + acceleration- * t² / 2 + * velocity = velocity- + acceleration- * t + * acceleration = acceleration- + */ + .a = (real[3,3]) { + { 1, t * height_scale / speed_scale , t**2/2 * height_scale / accel_scale }, + { 0, 1, t * speed_scale / accel_scale }, + { 0, 0, 1 } + }, +/* + * Model error covariance. The only inaccuracy in the + * model is the assumption that acceleration is constant + */ + .q = model_error (t, σ_m**2), + +/* + * Measurement error covariance + * Our sensors are independent, so + * this matrix is zero off-diagonal + */ + .r = (real[1,1]) { + { σ_h ** 2 }, + }, +/* + * Extract measurements from state, + * this just pulls out the height + * values. + */ + .h = (real[1,3]) { + { 1, 0, 0 }, + }, + }; +} + +bool just_kalman = true; +real accel_input_scale = 1; + +real error_avg; + +void update_error_avg(real e) { + if (e < 0) + e = -e; + +# if (e > 1000) +# e = 1000; + + error_avg -= error_avg / 8; + error_avg += (e * e) / 8; +} + +void run_flight(string name, file f, bool summary, real σ_m, real σ_h) { + state_t current_both = { + .x = (real[3]) { 0, 0, 0 }, + .p = (real[3,3]) { { 0 ... } ... }, + }; + state_t current_accel = current_both; + state_t current_baro = current_both; + real t; + real kalman_apogee_time = -1; + real kalman_apogee = 0; + real raw_apogee_time_first; + real raw_apogee_time_last; + real raw_apogee = 0; + real speed = 0; + real prev_acceleration = 0; + real height, max_height = 0; + state_t apogee_state; + parameters_fast_t fast_baro; + real fast_delta_t = 0; + bool fast = true; + int speed_lock = 0; + + error_avg = 0; + for (;;) { + record_t record = parse_record(f, 1.0); + if (record.done) + break; + if (is_uninit(&t)) + t = record.time; + real delta_t = record.time - t; + if (delta_t < 0.096) + continue; +# delta_t = 0.096; /* pretend that we're getting micropeak-rate data */ +# record.time = record.time + delta_t; + t = record.time; + if (record.height > raw_apogee) { + raw_apogee_time_first = record.time; + raw_apogee = record.height; + } + if (record.height == raw_apogee) + raw_apogee_time_last = record.time; + + real pressure = record.pressure; + + if (current_baro.x[0] == 0) + current_baro.x[0] = pressure; + + vec_t z_baro = (real[1]) { record.pressure * height_scale }; + + real error_h; + + if (fast) { + if (delta_t != fast_delta_t) { + fast_baro = convert_to_fast(param_baro(delta_t, σ_m, σ_h)); + fast_delta_t = delta_t; + } + + current_baro.x = predict_fast(current_baro.x, fast_baro); + error_h = current_baro.x[0] - pressure; + current_baro.x = correct_fast(current_baro.x, z_baro, fast_baro); + } else { + parameters_t p_baro = param_baro(delta_t, σ_m, σ_h); + + state_t pred_baro = predict(current_baro, p_baro); + error_h = current_baro.x[0] - pressure; + state_t next_baro = correct(pred_baro, z_baro, p_baro); + current_baro = next_baro; + } + + update_error_avg(error_h); + + /* Don't check for maximum altitude if we're accelerating upwards */ + if (current_baro.x[2] / accel_scale < -2 * σ_m) + speed_lock = 10; + else if (speed_lock > 0) + speed_lock--; + + height = pressure_to_altitude(current_baro.x[0] / height_scale); + if (speed_lock == 0 && height > max_height) + max_height = height; + + printf ("%16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %16.8f %d %d\n", + record.time, + record.pressure, + pressure_to_altitude(record.pressure), + current_baro.x[0] / height_scale, + current_baro.x[1] / speed_scale, + current_baro.x[2] / accel_scale, + height, + max_height, + error_h, + error_avg, + error_avg > 50000 ? 0 : 95000, + speed_lock > 0 ? 0 : 4500); + + if (kalman_apogee_time < 0) { + if (current_baro.x[1] > 1) { + kalman_apogee = current_both.x[0]; + kalman_apogee_time = record.time; + } + } + } + real raw_apogee_time = (raw_apogee_time_last + raw_apogee_time_first) / 2; + if (summary && !just_kalman) { + printf("%s: kalman (%8.2f m %6.2f s) raw (%8.2f m %6.2f s) error %6.2f s\n", + name, + kalman_apogee, kalman_apogee_time, + raw_apogee, raw_apogee_time, + kalman_apogee_time - raw_apogee_time); + } +} + +void main() { + bool summary = false; + int user_argind = 1; + real time_step = 0.01; + string compute = "none"; + string prefix = "AO_K"; + real σ_m = default_σ_m; + real σ_h = default_σ_h; + + ParseArgs::argdesc argd = { + .args = { + { .var = { .arg_flag = &summary }, + .abbr = 's', + .name = "summary", + .desc = "Print a summary of the flight" }, + { .var = { .arg_real = &time_step, }, + .abbr = 't', + .name = "time", + .expr_name = "", + .desc = "Set time step for convergence" }, + { .var = { .arg_string = &prefix }, + .abbr = 'p', + .name = "prefix", + .expr_name = "", + .desc = "Prefix for compute output" }, + { .var = { .arg_string = &compute }, + .abbr = 'c', + .name = "compute", + .expr_name = "{baro,none}", + .desc = "Compute Kalman factor through convergence" }, + { .var = { .arg_real = &σ_m }, + .abbr = 'M', + .name = "model", + .expr_name = "", + .desc = "Model co-variance for acceleration" }, + { .var = { .arg_real = &σ_h }, + .abbr = 'H', + .name = "height", + .expr_name = "", + .desc = "Measure co-variance for height" }, + }, + + .unknown = &user_argind, + }; + + ParseArgs::parseargs(&argd, &argv); + + if (compute != "none") { + parameters_t param; + + printf ("/* Kalman matrix for micro %s\n", compute); + printf (" * step = %f\n", time_step); + printf (" * σ_m = %f\n", σ_m); + switch (compute) { + case "baro": + printf (" * σ_h = %f\n", σ_h); + param = param_baro(time_step, σ_m, σ_h); + break; + } + printf (" */\n\n"); + mat_t k = converge(param); + int[] d = dims(k); + int time_inc = floor(1/time_step + 0.5); + for (int i = 0; i < d[0]; i++) + for (int j = 0; j < d[1]; j++) { + string name; + if (d[1] == 1) + name = sprintf("%s_K%d_%d", prefix, i, time_inc); + else + name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc); + printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]); + } + printf ("\n"); + exit(0); + } + string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] }; + + # height_scale = accel_scale = speed_scale = 1; + + if (dim(rest) == 0) + run_flight("", stdin, summary, σ_m, σ_h); + else { + for (int i = 0; i < dim(rest); i++) { + twixt(file f = File::open(rest[i], "r"); File::close(f)) { + run_flight(rest[i], f, summary, σ_m, σ_h); + } + } + } +} +main(); diff --git a/src/kalman/load_csv.5c b/src/kalman/load_csv.5c index 15e83166..0086c6db 100644 --- a/src/kalman/load_csv.5c +++ b/src/kalman/load_csv.5c @@ -31,6 +31,7 @@ namespace load_csv { real time; real height; real acceleration; + real pressure; } record_t; public record_t parse_record(file f, real accel_scale) { @@ -40,16 +41,28 @@ namespace load_csv { int time_off = 4; int height_off = 11; int accel_off = 8; - if (string_to_integer(data[0]) == 2) { + int pres_off = 9; + switch (string_to_integer(data[0])) { + case 2: time_off = 4; accel_off = 9; + pres_off = 10; height_off = 12; + break; + case 5: + time_off = 4; + accel_off = 10; + pres_off = 11; + height_off = 13; + break; } return (record_t) { .done = false, - .time = string_to_real(data[time_off]), - .height = imprecise(string_to_real(data[height_off])), - .acceleration = imprecise(string_to_real(data[accel_off]) * accel_scale) }; + .time = string_to_real(data[time_off]), + .height = imprecise(string_to_real(data[height_off])), + .acceleration = imprecise(string_to_real(data[accel_off]) * accel_scale), + .pressure = imprecise(string_to_real(data[pres_off])) + }; } public void dump(file f) { diff --git a/src/util/make-kalman b/src/util/make-kalman index fd33bab0..580a4515 100644 --- a/src/util/make-kalman +++ b/src/util/make-kalman @@ -6,6 +6,7 @@ SIGMA_BOTH="-M 2 -H 6 -A 2" SIGMA_BARO="-M 2 -H 6 -A 2" SIGMA_ACCEL="-M 2 -H 4 -A 4" SIGMA_BOTH_NOISY_ACCEL="-M 2 -H 6 -A 3" +SIGMA_MICRO="-M 10" echo '#if NOISY_ACCEL' echo @@ -39,3 +40,4 @@ nickle kalman.5c -p AO_BARO -c baro -t 0.01 $SIGMA_BARO nickle kalman.5c -p AO_BARO -c baro -t 0.1 $SIGMA_BARO nickle kalman.5c -p AO_BARO -c baro -t 1 $SIGMA_BARO +nickle kalman_micro.5c -p AO_MK_BARO -c baro -t 0.096 $SIGMA_MICRO \ No newline at end of file -- cgit v1.2.3 From c3024b759fcdf8b84a2139c1535c573a31eb5c95 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 4 Feb 2013 10:51:49 -0800 Subject: altos: Add atmosphere.5c Shared code for building pressure tables Signed-off-by: Keith Packard --- src/util/atmosphere.5c | 153 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 src/util/atmosphere.5c (limited to 'src/util') diff --git a/src/util/atmosphere.5c b/src/util/atmosphere.5c new file mode 100644 index 00000000..9b5107f0 --- /dev/null +++ b/src/util/atmosphere.5c @@ -0,0 +1,153 @@ +#!/usr/bin/nickle -f +/* + * 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; +} -- cgit v1.2.3 From 85d32468210c9989ae52bd29f883c4380af43961 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 28 Apr 2013 23:25:37 -0700 Subject: altos: Add ublox checksum app to generate ublox config lines Signed-off-by: Keith Packard --- src/util/ublox-cksum | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 src/util/ublox-cksum (limited to 'src/util') diff --git a/src/util/ublox-cksum b/src/util/ublox-cksum new file mode 100644 index 00000000..05e8d6b1 --- /dev/null +++ b/src/util/ublox-cksum @@ -0,0 +1,50 @@ +#!/usr/bin/env nickle + +typedef struct { + int a, b; +} ck_t; + +/* Fletcher algorithm */ +ck_t checksum(int[] msg) +{ + ck_t ck = { .a = 0, .b = 0 }; + for (int i = 4; i < dim(msg); i++) { + ck.a += msg[i]; + ck.b += ck.a; + ck.a &= 0xff; + ck.b &= 0xff; + } + return ck; +} + +void main() +{ + string[...] input; + int[...] msg; + + setdim(input, 0); + while (!File::end(stdin)) { + input[dim(input)] = gets(); + } + + setdim(msg, 0); + for (int i = 0; i < dim(input); i++) { + string[*] words = String::wordsplit(input[i], " ,\t"); + for (int j = 0; j < dim(words); j++) { + if (words[j] == "/" + "*") + break; + if (String::length(words[j]) > 0 && + Ctype::isdigit(words[j][0])) { + msg[dim(msg)] = string_to_integer(words[j]); + } + } + } + printf("\t0xb5, 0x62, \t\t/* length: %d bytes */\n", dim(msg)); + for (int i = 0; i < dim(input); i++) + printf("%s\n", input[i]); + ck_t ck = checksum(msg); + printf ("\t0x%02x, 0x%02x,\n", + ck.a, ck.b); +} + +main(); -- cgit v1.2.3