altos: Move micropeak sources around

This sticks the micropeak sources in appropriate directories, rather
than in the micropeak product directory so that they can be shared
with future micropeak-style products.

Signed-off-by: Keith Packard <keithp@keithp.com>

5 files changed, 434 insertions, 0 deletions

diff --git a/src/core/ao_log_micro.c b/src/core/ao_log_micro.c
new file mode 100644
index 00000000..d665efb5
--- /dev/null
+++ b/src/core/ao_log_micro.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright © 2012 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.
+ */
+
+#include <ao.h>
+#include <ao_micropeak.h>
+#include <ao_log_micro.h>
+#include <ao_async.h>
+
+static uint16_t ao_log_offset = STARTING_LOG_OFFSET;
+
+void
+ao_log_micro_save(void)
+{
+	uint16_t	n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t);
+	ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground));
+	ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min));
+	ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples));
+}
+
+void
+ao_log_micro_restore(void)
+{
+	ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground));
+	ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min));
+}
+
+void
+ao_log_micro_data(void)
+{
+	uint16_t	low_bits = pa;
+
+	if (ao_log_offset < MAX_LOG_OFFSET) {
+		ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits));
+		ao_log_offset += sizeof (low_bits);
+	}
+}
+
+#define POLY 0x8408
+
+static uint16_t
+ao_log_micro_crc(uint16_t crc, uint8_t byte)
+{
+	uint8_t	i;
+
+	for (i = 0; i < 8; i++) {
+		if ((crc & 0x0001) ^ (byte & 0x0001))
+			crc = (crc >> 1) ^ POLY;
+		else
+			crc = crc >> 1;
+		byte >>= 1;
+	}
+	return crc;
+}
+
+static void
+ao_log_hex_nibble(uint8_t b)
+{
+	if (b < 10)
+		ao_async_byte('0' + b);
+	else
+		ao_async_byte('a' - 10 + b);
+}
+
+static void
+ao_log_hex(uint8_t b)
+{
+	ao_log_hex_nibble(b>>4);
+	ao_log_hex_nibble(b&0xf);
+}
+
+static void
+ao_log_newline(void)
+{
+	ao_async_byte('\r');
+	ao_async_byte('\n');
+}
+
+void
+ao_log_micro_dump(void)
+{
+	uint16_t	n_samples;
+	uint16_t	nbytes;
+	uint8_t		byte;
+	uint16_t	b;
+	uint16_t	crc = 0xffff;
+
+	ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples));
+	if (n_samples == 0xffff)
+		n_samples = 0;
+	nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples;
+	ao_async_start();
+	ao_async_byte('M');
+	ao_async_byte('P');
+	for (b = 0; b < nbytes; b++) {
+		if ((b & 0xf) == 0)
+			ao_log_newline();
+		ao_eeprom_read(b, &byte, 1);
+		ao_log_hex(byte);
+		crc = ao_log_micro_crc(crc, byte);
+	}
+	ao_log_newline();
+	crc = ~crc;
+	ao_log_hex(crc >> 8);
+	ao_log_hex(crc);
+	ao_log_newline();
+	ao_async_stop();
+}
diff --git a/src/core/ao_log_micro.h b/src/core/ao_log_micro.h
new file mode 100644
index 00000000..976852ee
--- /dev/null
+++ b/src/core/ao_log_micro.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright © 2012 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_LOG_MICRO_H_
+#define _AO_LOG_MICRO_H_
+
+#define PA_GROUND_OFFSET	0
+#define PA_MIN_OFFSET		4
+#define N_SAMPLES_OFFSET	8
+#define STARTING_LOG_OFFSET	10
+#define MAX_LOG_OFFSET		512
+
+void
+ao_log_micro_save(void);
+
+void
+ao_log_micro_restore(void);
+
+void
+ao_log_micro_data(void);
+
+void
+ao_log_micro_dump(void);
+
+#endif /* _AO_LOG_MICRO_H_ */
diff --git a/src/core/ao_microflight.c b/src/core/ao_microflight.c
new file mode 100644
index 00000000..714bb90a
--- /dev/null
+++ b/src/core/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/core/ao_microkalman.c b/src/core/ao_microkalman.c
new file mode 100644
index 00000000..0684ea2b
--- /dev/null
+++ b/src/core/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/core/ao_report_micro.c b/src/core/ao_report_micro.c
new file mode 100644
index 00000000..0e8e287f
--- /dev/null
+++ b/src/core/ao_report_micro.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright © 2012 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.
+ */
+
+#include <ao.h>
+
+#define mid(time)	ao_led_for(AO_LED_REPORT, time)
+#define pause(time)	ao_delay(time)
+
+static void
+ao_report_digit(uint8_t digit) __reentrant
+{
+	if (!digit) {
+		mid(AO_MS_TO_TICKS(1000));
+		pause(AO_MS_TO_TICKS(300));
+	} else {
+		while (digit--) {
+			mid(AO_MS_TO_TICKS(300));
+			pause(AO_MS_TO_TICKS(300));
+		}
+	}
+	pause(AO_MS_TO_TICKS(1000));
+}
+
+void
+ao_report_altitude(void)
+{
+	__pdata alt_t	agl = ao_max_height;
+	static __xdata uint8_t	digits[11];
+	__pdata uint8_t ndigits, i;
+
+	if (agl < 0)
+		agl = 0;
+	ndigits = 0;
+	do {
+		digits[ndigits++] = agl % 10;
+		agl /= 10;
+	} while (agl);
+
+	i = ndigits;
+	do
+		ao_report_digit(digits[--i]);
+	while (i != 0);
+}