From: Keith Packard Date: Wed, 16 Jan 2013 23:22:46 +0000 (-0800) Subject: Merge branch 'master' into telescience-v0.2 X-Git-Tag: altosdroid_v1.2-1~105^2 X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=a04c4f7b07e97d568f8f6f56dd363329817fb52c;hp=0c2fa9614ffe22901ba0fd089e1e02c362f9fbe0;p=fw%2Faltos Merge branch 'master' into telescience-v0.2 --- diff --git a/micropeak/MicroStatsTable.java b/micropeak/MicroStatsTable.java index f373e25d..cf30fcb7 100644 --- a/micropeak/MicroStatsTable.java +++ b/micropeak/MicroStatsTable.java @@ -75,21 +75,21 @@ public class MicroStatsTable extends JComponent { MicroStat flight_time; public void setStats(MicroStats stats) { - max_height.set_values(String.format("%5.0f m", stats.apogee_height), - String.format("%5.0f ft", AltosConvert.meters_to_feet(stats.apogee_height))); - max_speed.set_values(String.format("%5.0f m/s", stats.max_speed), - String.format("%5.0f mph", AltosConvert.meters_to_mph(stats.max_speed)), - String.format("Mach %4.1f", AltosConvert.meters_to_mach(stats.max_speed))); - max_accel.set_values(String.format("%5.0f m/s²", stats.max_accel), - String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)), - String.format("%5.0f G", AltosConvert.meters_to_g(stats.max_accel))); - avg_accel.set_values(String.format("%5.0f m/s²", stats.boost_accel(), - String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())), - String.format("%5.0f G", AltosConvert.meters_to_g(stats.boost_accel())))); + max_height.set_values(String.format("%7.1f m", stats.apogee_height), + String.format("%7.1f ft", AltosConvert.meters_to_feet(stats.apogee_height))); + max_speed.set_values(String.format("%7.1f m/s", stats.max_speed), + String.format("%7.1f mph", AltosConvert.meters_to_mph(stats.max_speed)), + String.format("Mach %7.3f", AltosConvert.meters_to_mach(stats.max_speed))); + max_accel.set_values(String.format("%7.1f m/s²", stats.max_accel), + String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)), + String.format("%7.3f G", AltosConvert.meters_to_g(stats.max_accel))); + avg_accel.set_values(String.format("%7.1f m/s²", stats.boost_accel(), + String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())), + String.format("%7.3f G", AltosConvert.meters_to_g(stats.boost_accel())))); boost_duration.set_values(String.format("%6.1f s", stats.boost_duration())); coast_duration.set_values(String.format("%6.1f s", stats.coast_duration())); - descent_speed.set_values(String.format("%5.0f m/s", stats.descent_speed()), - String.format("%5.0f ft/s", AltosConvert.meters_to_feet(stats.descent_speed()))); + descent_speed.set_values(String.format("%7.1f m/s", stats.descent_speed()), + String.format("%7.1f ft/s", AltosConvert.meters_to_feet(stats.descent_speed()))); descent_duration.set_values(String.format("%6.1f s", stats.descent_duration())); flight_time.set_values(String.format("%6.1f s", stats.landed_time)); } @@ -104,31 +104,31 @@ public class MicroStatsTable extends JComponent { setLayout(layout); int y = 0; max_height = new MicroStat(layout, y++, "Maximum height", - String.format("%5.0f m", stats.apogee_height), - String.format("%5.0f ft", AltosConvert.meters_to_feet(stats.apogee_height))); + String.format("%7.1f m", stats.apogee_height), + String.format("%7.1f ft", AltosConvert.meters_to_feet(stats.apogee_height))); max_speed = new MicroStat(layout, y++, "Maximum speed", - String.format("%5.0f m/s", stats.max_speed), - String.format("%5.0f mph", AltosConvert.meters_to_mph(stats.max_speed)), + String.format("%7.1f m/s", stats.max_speed), + String.format("%7.1f mph", AltosConvert.meters_to_mph(stats.max_speed)), String.format("Mach %4.1f", AltosConvert.meters_to_mach(stats.max_speed))); max_accel = new MicroStat(layout, y++, "Maximum boost acceleration", - String.format("%5.0f m/s²", stats.max_accel), - String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)), - String.format("%5.0f G", AltosConvert.meters_to_g(stats.max_accel))); + String.format("%7.1f m/s²", stats.max_accel), + String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.max_accel)), + String.format("%7.3f G", AltosConvert.meters_to_g(stats.max_accel))); avg_accel = new MicroStat(layout, y++, "Average boost acceleration", - String.format("%5.0f m/s²", stats.boost_accel(), - String.format("%5.0f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())), - String.format("%5.0f G", AltosConvert.meters_to_g(stats.boost_accel())))); + String.format("%7.1f m/s²", stats.boost_accel(), + String.format("%7.1f ft/s²", AltosConvert.meters_to_feet(stats.boost_accel())), + String.format("%7.3f G", AltosConvert.meters_to_g(stats.boost_accel())))); boost_duration = new MicroStat(layout, y++, "Boost duration", - String.format("%6.0f s", stats.boost_duration())); + String.format("%6.1f s", stats.boost_duration())); coast_duration = new MicroStat(layout, y++, "Coast duration", String.format("%6.1f s", stats.coast_duration())); descent_speed = new MicroStat(layout, y++, "Descent rate", - String.format("%5.0f m/s", stats.descent_speed()), - String.format("%5.0f ft/s", AltosConvert.meters_to_feet(stats.descent_speed()))); + String.format("%7.1f m/s", stats.descent_speed()), + String.format("%7.1f ft/s", AltosConvert.meters_to_feet(stats.descent_speed()))); descent_duration = new MicroStat(layout, y++, "Descent duration", String.format("%6.1f s", stats.descent_duration())); flight_time = new MicroStat(layout, y++, "Flight Time", - String.format("%6.0f s", stats.landed_time)); + String.format("%6.1f s", stats.landed_time)); } public MicroStatsTable() { diff --git a/src/Makefile b/src/Makefile index a17f51ac..e4b11039 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.5c b/src/kalman/kalman.5c index cfb7abea..46fa8e1f 100755 --- a/src/kalman/kalman.5c +++ b/src/kalman/kalman.5c @@ -55,6 +55,12 @@ real default_σ_m = 5; real default_σ_h = 20; real default_σ_a = 2; +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_both(real t, real σ_m, real σ_h, real σ_a) { if (σ_m == 0) σ_m = default_σ_m; @@ -87,11 +93,7 @@ parameters_t param_both(real t, real σ_m, real σ_h, real σ_a) { * Model error covariance. The only inaccuracy in the * model is the assumption that acceleration is constant */ - .q = (real[3,3]) { - { 0, 0, 0 }, - { 0, 0, 0 }, - {.0, 0, σ_m**2 }, - }, + .q = model_error (t, σ_m**2), /* * Measurement error covariance * Our sensors are independent, so @@ -140,11 +142,7 @@ parameters_t param_baro(real t, real σ_m, real σ_h) { * Model error covariance. The only inaccuracy in the * model is the assumption that acceleration is constant */ - .q = (real[3,3]) { - { 0, 0, 0 }, - { 0, 0, 0 }, - {.0, 0, σ_m**2 }, - }, + .q = model_error (t, σ_m**2), /* * Measurement error covariance * Our sensors are independent, so @@ -191,11 +189,7 @@ parameters_t param_accel(real t, real σ_m, real σ_a) { * Model error covariance. The only inaccuracy in the * model is the assumption that acceleration is constant */ - .q = (real[3,3]) { - { 0, 0, 0 }, - { 0, 0, 0 }, - {.0, 0, σ_m**2 }, - }, + .q = model_error (t, σ_m**2), /* * Measurement error covariance * Our sensors are independent, so @@ -236,11 +230,7 @@ parameters_t param_vel(real t) { * Model error covariance. The only inaccuracy in the * model is the assumption that acceleration is constant */ - .q = (real[3,3]) { - { 0, 0, 0 }, - { 0, 0, 0 }, - {.0, 0, σ_m**2 }, - }, + .q = model_error (t, σ_m**2), /* * Measurement error covariance * Our sensors are independent, so 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/micropeak/Makefile b/src/micropeak/Makefile index ff0a4499..315b93f6 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -33,7 +33,9 @@ ALTOS_SRC = \ ao_eeprom_tiny.c \ ao_panic.c \ ao_log_micro.c \ - ao_async.c + ao_async.c \ + ao_microflight.c \ + ao_microkalman.c INC=\ ao.h \ diff --git a/src/micropeak/ao_microflight.c b/src/micropeak/ao_microflight.c new file mode 100644 index 00000000..714bb90a --- /dev/null +++ b/src/micropeak/ao_microflight.c @@ -0,0 +1,143 @@ +/* + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include +#include + +uint32_t pa; +uint32_t pa_ground; +uint32_t pa_min; + +static void +ao_microsample(void) +{ + ao_pa_get(); + ao_microkalman_predict(); + ao_microkalman_correct(); +} + +#define NUM_PA_HIST 16 + +#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) + +static uint32_t pa_hist[NUM_PA_HIST]; + +void +ao_microflight(void) +{ + int16_t sample_count; + uint16_t time; + uint32_t pa_interval_min, pa_interval_max; + int32_t pa_diff; + uint8_t h, i; + uint8_t accel_lock = 0; + uint32_t pa_sum = 0; + + /* Wait for motion, averaging values to get ground pressure */ + + time = ao_time(); + ao_pa_get(); + ao_microkalman_init(); + pa_ground = pa; + sample_count = 0; + h = 0; + for (;;) { + time += SAMPLE_SLEEP; + if (sample_count == 0) + ao_led_on(AO_LED_REPORT); + ao_delay_until(time); + ao_microsample(); + if (sample_count == 0) + ao_led_off(AO_LED_REPORT); + pa_hist[h] = pa; + h = SKIP_PA_HIST(h,1); + pa_diff = pa_ground - ao_pa; + + /* Check for a significant pressure change */ + if (pa_diff > BOOST_DETECT) + break; + + if (sample_count < GROUND_AVG * 2) { + if (sample_count < GROUND_AVG) + pa_sum += pa; + ++sample_count; + } else { + pa_ground = pa_sum >> GROUND_AVG_SHIFT; + pa_sum = 0; + sample_count = 0; + } + } + + /* Go back and find the first sample a decent interval above the ground */ + pa_min = pa_ground - LAND_DETECT; + for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) { + if (pa_hist[i] < pa_min) + break; + } + + /* Log the remaining samples so we get a complete history since leaving the ground */ + for (; i != h; i = SKIP_PA_HIST(i,2)) { + pa = pa_hist[i]; + ao_log_micro_data(); + } + + /* Now sit around until the pressure is stable again and record the max */ + + sample_count = 0; + pa_min = ao_pa; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + for (;;) { + time += SAMPLE_SLEEP; + ao_delay_until(time); + if ((sample_count & 3) == 0) + ao_led_on(AO_LED_REPORT); + ao_microsample(); + if ((sample_count & 3) == 0) + ao_led_off(AO_LED_REPORT); + if (sample_count & 1) + ao_log_micro_data(); + + /* If accelerating upwards, don't look for min pressure */ + if (ao_pa_accel < ACCEL_LOCK_PA) + accel_lock = ACCEL_LOCK_TIME; + else if (accel_lock) + --accel_lock; + else if (ao_pa < pa_min) + pa_min = ao_pa; + + if (sample_count == (GROUND_AVG - 1)) { + pa_diff = pa_interval_max - pa_interval_min; + + /* Check to see if the pressure is now stable */ + if (pa_diff < LAND_DETECT) + break; + sample_count = 0; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + } else { + if (ao_pa < pa_interval_min) + pa_interval_min = ao_pa; + if (ao_pa > pa_interval_max) + pa_interval_max = ao_pa; + ++sample_count; + } + } +} diff --git a/src/micropeak/ao_microkalman.c b/src/micropeak/ao_microkalman.c new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/micropeak/ao_microkalman.c @@ -0,0 +1,74 @@ +/* + * 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. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include + +#define FIX_BITS 16 + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix8(x) ((x) >> 8) +#define from_fix(x) ((x) >> 16) +#define fix8_to_fix16(x) ((x) << 8) +#define fix16_to_fix8(x) ((x) >> 8) + +#include + +/* Basic time step (96ms) */ +#define AO_MK_STEP to_fix16(0.096) +/* step ** 2 / 2 */ +#define AO_MK_STEP_2_2 to_fix16(0.004608) + +uint32_t ao_k_pa; /* 24.8 fixed point */ +int32_t ao_k_pa_speed; /* 16.16 fixed point */ +int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +uint32_t ao_pa; /* integer portion */ +int16_t ao_pa_speed; /* integer portion */ +int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void) +{ + ao_pa = pa; + ao_k_pa = pa << 8; +} + +void +ao_microkalman_predict(void) +{ + ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); + ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; +} + +void +ao_microkalman_correct(void) +{ + int16_t e; /* Height error in Pa */ + + e = pa - from_fix8(ao_k_pa); + + ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); + ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; + ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; + ao_pa = from_fix8(ao_k_pa); + ao_pa_speed = from_fix(ao_k_pa_speed); + ao_pa_accel = from_fix(ao_k_pa_accel); +} diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c index f361aa26..10f0d192 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/micropeak/ao_micropeak.c @@ -24,16 +24,10 @@ static struct ao_ms5607_sample sample; static struct ao_ms5607_value value; -uint32_t pa; -uint32_t pa_avg; -uint32_t pa_ground; -uint32_t pa_min; alt_t ground_alt, max_alt; alt_t ao_max_height; -static uint32_t pa_sum; - -static void +void ao_pa_get(void) { ao_ms5607_sample(&sample); @@ -60,21 +54,9 @@ ao_pips(void) ao_delay(AO_MS_TO_TICKS(200)); } -#define NUM_PA_HIST 16 - -#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) - -static uint32_t pa_hist[NUM_PA_HIST]; - int main(void) { - int16_t sample_count; - uint16_t time; - uint32_t pa_interval_min, pa_interval_max; - int32_t pa_diff; - uint8_t h, i; - ao_led_init(LEDS_AVAILABLE); ao_timer_init(); @@ -93,93 +75,9 @@ main(void) ao_log_micro_dump(); ao_delay(BOOST_DELAY); - /* Wait for motion, averaging values to get ground pressure */ - time = ao_time(); - ao_pa_get(); - pa_avg = pa_ground = pa << FILTER_SHIFT; - sample_count = 0; - h = 0; - for (;;) { - time += SAMPLE_SLEEP; - if (sample_count == 0) - ao_led_on(AO_LED_REPORT); - ao_delay_until(time); - ao_pa_get(); - if (sample_count == 0) - ao_led_off(AO_LED_REPORT); - pa_hist[h] = pa; - h = SKIP_PA_HIST(h,1); - pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; - pa_diff = pa_ground - pa_avg; - - /* Check for a significant pressure change */ - if (pa_diff > (BOOST_DETECT << FILTER_SHIFT)) - break; - - if (sample_count < GROUND_AVG * 2) { - if (sample_count < GROUND_AVG) - pa_sum += pa; - ++sample_count; - } else { - pa_ground = pa_sum >> (GROUND_AVG_SHIFT - FILTER_SHIFT); - pa_sum = 0; - sample_count = 0; - } - } - pa_ground >>= FILTER_SHIFT; + ao_microflight(); - /* Go back and find the first sample a decent interval above the ground */ - pa_min = pa_ground - LAND_DETECT; - for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) { - if (pa_hist[i] < pa_min) - break; - } - - /* Log the remaining samples so we get a complete history since leaving the ground */ - for (; i != h; i = SKIP_PA_HIST(i,2)) { - pa = pa_hist[i]; - ao_log_micro_data(); - } - - /* Now sit around until the pressure is stable again and record the max */ - - sample_count = 0; - pa_min = pa_avg; - pa_interval_min = pa_avg; - pa_interval_max = pa_avg; - for (;;) { - time += SAMPLE_SLEEP; - ao_delay_until(time); - if ((sample_count & 3) == 0) - ao_led_on(AO_LED_REPORT); - ao_pa_get(); - if ((sample_count & 3) == 0) - ao_led_off(AO_LED_REPORT); - if (sample_count & 1) - ao_log_micro_data(); - pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; - if (pa_avg < pa_min) - pa_min = pa_avg; - - if (sample_count == (GROUND_AVG - 1)) { - pa_diff = pa_interval_max - pa_interval_min; - - /* Check to see if the pressure is now stable */ - if (pa_diff < (LAND_DETECT << FILTER_SHIFT)) - break; - sample_count = 0; - pa_interval_min = pa_avg; - pa_interval_max = pa_avg; - } else { - if (pa_avg < pa_interval_min) - pa_interval_min = pa_avg; - if (pa_avg > pa_interval_max) - pa_interval_max = pa_avg; - ++sample_count; - } - } - pa_min >>= FILTER_SHIFT; ao_log_micro_save(); ao_compute_height(); ao_report_altitude(); diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h index e408d7c5..382b98d9 100644 --- a/src/micropeak/ao_micropeak.h +++ b/src/micropeak/ao_micropeak.h @@ -18,7 +18,6 @@ #ifndef _AO_MICROPEAK_H_ #define _AO_MICROPEAK_H_ -#define FILTER_SHIFT 3 #define SAMPLE_SLEEP AO_MS_TO_TICKS(96) /* 16 sample, or about two seconds worth */ @@ -32,14 +31,11 @@ #define BOOST_DELAY AO_SEC_TO_TICKS(30) /* Pressure change (in Pa) to detect landing */ -#define LAND_DETECT 12 /* 1m at sea level, 1.2m at 2000m */ +#define LAND_DETECT 24 /* 2m at sea level, 2.4m at 2000m */ /* Current sensor pressure value */ extern uint32_t pa; -/* IIR filtered pressure value */ -extern uint32_t pa_avg; - /* Average pressure value on ground */ extern uint32_t pa_ground; @@ -52,5 +48,31 @@ extern alt_t ground_alt, max_alt; /* max_alt - ground_alt */ extern alt_t ao_max_height; +void +ao_pa_get(void); + +void +ao_microflight(void); + +#define ACCEL_LOCK_PA -20 +#define ACCEL_LOCK_TIME 10 + +extern uint32_t ao_k_pa; /* 24.8 fixed point */ +extern int32_t ao_k_pa_speed; /* 16.16 fixed point */ +extern int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +extern uint32_t ao_pa; /* integer portion */ +extern int16_t ao_pa_speed; /* integer portion */ +extern int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void); + +void +ao_microkalman_predict(void); + +void +ao_microkalman_correct(void); + #endif diff --git a/src/test/Makefile b/src/test/Makefile index 092bf360..87bd70fe 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,14 +1,14 @@ -vpath % ..:../core:../drivers:../util +vpath % ..:../core:../drivers:../util:../micropeak PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \ - ao_aprs_test + ao_aprs_test ao_micropeak_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -60,4 +60,7 @@ ao_aprs_data.wav: ao_aprs_test ./ao_aprs_test | sox $(SOX_INPUT_ARGS) - $(SOX_OUTPUT_ARGS) $@ check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests - ./ao_fec_test && ./run-tests \ No newline at end of file + ./ao_fec_test && ./run-tests + +ao_micropeak_test: ao_micropeak_test.c ao_microflight.c + cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm \ No newline at end of file diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c new file mode 100644 index 00000000..04686402 --- /dev/null +++ b/src/test/ao_micropeak_test.c @@ -0,0 +1,192 @@ +/* + * Copyright © 2009 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. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include + +FILE *emulator_in; +char *emulator_app; +char *emulator_name; +char *emulator_info; +uint8_t ao_flight_debug; + +#define AO_FLIGHT_TEST + +typedef int32_t alt_t; + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) + +#define AO_LED_REPORT 0 + +static void ao_led_on(uint8_t led) { +} + +static void ao_led_off(uint8_t led) { +} + +static void ao_delay_until(uint16_t target) { +} + +static uint16_t ao_time(void) { + return 0; +} + +#include "ao_microflight.c" +#include "ao_microkalman.c" +#include "ao_convert_pa.c" + +uint16_t now; +uint8_t running; + +void ao_log_micro_data() { + running = 1; +} + +void +ao_micro_report(void) +{ + if (running) { + alt_t ground = ao_pa_to_altitude(pa_ground); + printf ("%6.2f %10d %10d %10d\n", now / 100.0, + ao_pa_to_altitude(pa) - ground, + ao_pa_to_altitude(ao_pa) - ground, + ao_pa_to_altitude(pa_min) - ground); + } +} + +void +ao_micro_finish(void) +{ + ao_micro_report(); +} + +void +ao_pa_get(void) +{ + char line[4096]; + char *toks[128]; + char *saveptr; + int t, ntok; + static int time_id; + static int pa_id; + double time; + double pressure; + static double last_time; + static int been_here; + static int start_samples; + + if (been_here && start_samples < 100) { + start_samples++; + return; + } + ao_micro_report(); + for (;;) { + if (!fgets(line, sizeof (line), emulator_in)) + exit(0); + for (t = 0; t < 128; t++) { + toks[t] = strtok_r(t ? NULL : line, ", ", &saveptr); + if (!toks[t]) + break; + } + ntok = t; + if (toks[0][0] == '#') { + if (strcmp(toks[0],"#version") == 0) { + for (t = 1; t < ntok; t++) { + if (!strcmp(toks[t], "time")) + time_id = t; + if (!strcmp(toks[t],"pressure")) + pa_id = t; + } + } + continue; + } + time = strtod(toks[time_id],NULL); + pressure = strtod(toks[pa_id],NULL); + if (been_here && time - last_time < 0.1) + continue; + been_here = 1; + last_time = time; + now = floor (time * 100 + 0.5); + pa = pressure; + break; + } +} + +void +ao_dump_state(void) +{ +} + +static const struct option options[] = { + { .name = "summary", .has_arg = 0, .val = 's' }, + { .name = "debug", .has_arg = 0, .val = 'd' }, + { .name = "info", .has_arg = 1, .val = 'i' }, + { 0, 0, 0, 0}, +}; + +void run_flight_fixed(char *name, FILE *f, int summary, char *info) +{ + emulator_name = name; + emulator_in = f; + emulator_info = info; + ao_microflight(); + ao_micro_finish(); +} + +int +main (int argc, char **argv) +{ + int summary = 0; + int c; + int i; + char *info = NULL; + + emulator_app="baro"; + while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) { + switch (c) { + case 's': + summary = 1; + break; + case 'd': + ao_flight_debug = 1; + break; + case 'i': + info = optarg; + break; + } + } + + if (optind == argc) + run_flight_fixed("", stdin, summary, info); + else + for (i = optind; i < argc; i++) { + FILE *f = fopen(argv[i], "r"); + if (!f) { + perror(argv[i]); + continue; + } + run_flight_fixed(argv[i], f, summary, info); + fclose(f); + } + exit(0); +} diff --git a/src/test/plotmicro b/src/test/plotmicro new file mode 100755 index 00000000..cdfcc581 --- /dev/null +++ b/src/test/plotmicro @@ -0,0 +1,14 @@ +#!/bin/sh +for i in "$@"; do +gnuplot -p << EOF & +set title "$i" +set ylabel "height (m)" +set xlabel "time (s)" +set xtics border out nomirror +set ytics border out nomirror +set y2tics border out nomirror +plot "$i" using 1:2 with lines lt 2 axes x1y1 title "raw height",\ + "$i" using 1:3 with lines lt 4 axes x1y1 title "kalman height",\ + "$i" using 1:4 with lines lt 1 axes x1y1 title "max height" +EOF +done 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