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));
}
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() {
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
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;
* 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
* 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
* 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
* 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
--- /dev/null
+#!/usr/bin/env nickle
+
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * 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 = "<time-step>",
+ .desc = "Set time step for convergence" },
+ { .var = { .arg_string = &prefix },
+ .abbr = 'p',
+ .name = "prefix",
+ .expr_name = "<prefix>",
+ .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 = "<model-accel-error>",
+ .desc = "Model co-variance for acceleration" },
+ { .var = { .arg_real = &σ_h },
+ .abbr = 'H',
+ .name = "height",
+ .expr_name = "<measure-height-error>",
+ .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>", 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();
real time;
real height;
real acceleration;
+ real pressure;
} record_t;
public record_t parse_record(file f, real accel_scale) {
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) {
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 \
--- /dev/null
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <ao.h>
+#endif
+#include <ao_micropeak.h>
+#include <ao_log_micro.h>
+
+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;
+ }
+ }
+}
--- /dev/null
+/*
+ * Copyright © 2013 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <ao.h>
+#endif
+#include <ao_micropeak.h>
+
+#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 <ao_kalman.h>
+
+/* 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);
+}
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);
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();
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();
#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 */
#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;
/* 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
-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
./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
--- /dev/null
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * 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 <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <math.h>
+
+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>", 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);
+}
--- /dev/null
+#!/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
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
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
projects / fw / altos / commitdiff