summaryrefslogtreecommitdiff
path: root/src/micropeak
diff options
context:
space:
mode:
Diffstat (limited to 'src/micropeak')
-rw-r--r--src/micropeak/Makefile4
-rw-r--r--src/micropeak/ao_microflight.c143
-rw-r--r--src/micropeak/ao_microkalman.c74
-rw-r--r--src/micropeak/ao_micropeak.c106
-rw-r--r--src/micropeak/ao_micropeak.h32
5 files changed, 249 insertions, 110 deletions
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 <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;
+ }
+ }
+}
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 <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);
+}
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
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-31 20:32:26 +0000