From 5e9193f6375be27e5f7a0321fd34b6acfe81247f Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Tue, 14 May 2013 09:12:29 -0700
Subject: altos: Add 'g' command to ublox GPS code.

Take the gps_dump function from ao_gps_skytraq.c and move it to a new
file so it can be shared with the u-blox driver. That affects every
skytraq and u-blox user as they need to include the new file.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/test/Makefile              |  4 ++--
 src/test/ao_gps_test_skytraq.c | 12 +++++++-----
 src/test/ao_gps_test_ublox.c   | 11 +++++++----
 3 files changed, 16 insertions(+), 11 deletions(-)

(limited to 'src/test')

diff --git a/src/test/Makefile b/src/test/Makefile
index 169c1dc6..8032a163 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -35,10 +35,10 @@ ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.
 ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h
 	cc $(CFLAGS) -o $@ $<
 
-ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h
+ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_gps_show.c ao_host.h
 	cc $(CFLAGS) -o $@ $<
 
-ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h ao_gps_ublox.h
+ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_gps_show.c ao_host.h ao_gps_ublox.h
 	cc $(CFLAGS) -o $@ $<
 
 ao_convert_test: ao_convert_test.c ao_convert.c altitude.h
diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c
index 81008b39..89cbd767 100644
--- a/src/test/ao_gps_test_skytraq.c
+++ b/src/test/ao_gps_test_skytraq.c
@@ -75,6 +75,11 @@ struct ao_gps_tracking_orig {
 #define ao_telemetry_satellite ao_gps_tracking_orig
 #define ao_telemetry_satellite_info ao_gps_sat_orig
 
+extern __xdata struct ao_telemetry_location	ao_gps_data;
+extern __xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
+
+uint8_t ao_gps_mutex;
+
 void
 ao_mutex_get(uint8_t *mutex)
 {
@@ -432,17 +437,14 @@ uint8_t	ao_task_minimize_latency;
 #define ao_usb_getchar()	0
 
 #include "ao_gps_print.c"
+#include "ao_gps_show.c"
 #include "ao_gps_skytraq.c"
 
 void
 ao_dump_state(void *wchan)
 {
 	if (wchan == &ao_gps_data)
-		ao_gps_print(&ao_gps_data);
-	else
-		ao_gps_tracking_print(&ao_gps_tracking_data);
-	putchar('\n');
-	return;
+		ao_gps_show();
 }
 
 int
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
index 80671735..afd4dba4 100644
--- a/src/test/ao_gps_test_ublox.c
+++ b/src/test/ao_gps_test_ublox.c
@@ -77,6 +77,11 @@ struct ao_telemetry_satellite {
 #define ao_gps_tracking_orig ao_telemetry_satellite
 #define ao_gps_sat_orig ao_telemetry_satellite_info
 
+extern __xdata struct ao_telemetry_location	ao_gps_data;
+extern __xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
+
+uint8_t ao_gps_mutex;
+
 void
 ao_mutex_get(uint8_t *mutex)
 {
@@ -224,6 +229,7 @@ uint8_t	ao_task_minimize_latency;
 #define ao_usb_getchar()	0
 
 #include "ao_gps_print.c"
+#include "ao_gps_show.c"
 #include "ao_gps_ublox.c"
 
 static void
@@ -342,10 +348,7 @@ void
 ao_dump_state(void *wchan)
 {
 	if (wchan == &ao_gps_data)
-		ao_gps_print(&ao_gps_data);
-	else
-		ao_gps_tracking_print(&ao_gps_tracking_data);
-	putchar('\n');
+		ao_gps_show();
 	return;
 }
 
-- 
cgit v1.2.3


From a4e4eec827d61a05fda52ddb68b55f17b6028d5e Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Tue, 14 May 2013 09:25:08 -0700
Subject: altos: gps serial routines are called ao_gps_*, not ao_ublox_*

This caused the u-blox driver to use serial port 1 instead of the
project-specified serial port.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/drivers/ao_gps_ublox.c   | 28 +++++++++++-----------------
 src/test/ao_gps_test_ublox.c |  6 +++---
 2 files changed, 14 insertions(+), 20 deletions(-)

(limited to 'src/test')

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index 56a4da16..80869561 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -39,13 +39,7 @@ struct ao_ublox_cksum {
 static __pdata struct ao_ublox_cksum ao_ublox_cksum;
 static __pdata uint16_t ao_ublox_len;
 
-#ifndef ao_ublox_getchar
-#define ao_ublox_getchar	ao_serial1_getchar
-#define ao_ublox_putchar	ao_serial1_putchar
-#define ao_ublox_set_speed	ao_serial1_set_speed
-#endif
-
-#define ao_ublox_byte()	((uint8_t) ao_ublox_getchar())
+#define ao_ublox_byte()	((uint8_t) ao_gps_getchar())
 
 static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c)
 {
@@ -61,7 +55,7 @@ static void ao_ublox_init_cksum(void)
 static void ao_ublox_put_u8(uint8_t c)
 {
 	add_cksum(&ao_ublox_cksum, c);
-	ao_ublox_putchar(c);
+	ao_gps_putchar(c);
 }
 
 static void ao_ublox_put_i8(int8_t c)
@@ -408,14 +402,14 @@ ao_gps_setup(void)
 
 	ao_delay(AO_SEC_TO_TICKS(3));
 
-	ao_ublox_set_speed(AO_SERIAL_SPEED_9600);
+	ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 
 	/*
 	 * A bunch of nulls so the start bit
 	 * is clear
 	 */
 	for (i = 0; i < 64; i++)
-		ao_ublox_putchar(0x00);
+		ao_gps_putchar(0x00);
 
 	/*
 	 * Send the baud-rate setting and protocol-setting
@@ -423,27 +417,27 @@ ao_gps_setup(void)
 	 */
 	for (k = 0; k < 3; k++)
 		for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
-			ao_ublox_putchar(ao_gps_set_nmea[i]);
+			ao_gps_putchar(ao_gps_set_nmea[i]);
 
 	/*
 	 * Increase the baud rate
 	 */
-	ao_ublox_set_speed(AO_SERIAL_SPEED_57600);
+	ao_gps_set_speed(AO_SERIAL_SPEED_57600);
 
 	/*
 	 * Pad with nulls to give the chip
 	 * time to see the baud rate switch
 	 */
 	for (i = 0; i < 64; i++)
-		ao_ublox_putchar(0x00);
+		ao_gps_putchar(0x00);
 }
 
 static void
 ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 {
 	ao_ublox_init_cksum();
-	ao_ublox_putchar(0xb5);
-	ao_ublox_putchar(0x62);
+	ao_gps_putchar(0xb5);
+	ao_gps_putchar(0x62);
 	ao_ublox_put_u8(class);
 	ao_ublox_put_u8(id);
 	ao_ublox_put_u8(len);
@@ -453,8 +447,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len)
 static void
 ao_ublox_putend(void)
 {
-	ao_ublox_putchar(ao_ublox_cksum.a);
-	ao_ublox_putchar(ao_ublox_cksum.b);
+	ao_gps_putchar(ao_ublox_cksum.a);
+	ao_gps_putchar(ao_ublox_cksum.b);
 }
 
 static void
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
index afd4dba4..a0e04cb6 100644
--- a/src/test/ao_gps_test_ublox.c
+++ b/src/test/ao_gps_test_ublox.c
@@ -130,7 +130,7 @@ static uint16_t	recv_len;
 static void check_ublox_message(char *which, uint8_t *msg);
 
 char
-ao_serial1_getchar(void)
+ao_gps_getchar(void)
 {
 	char	c;
 	uint8_t	uc;
@@ -163,7 +163,7 @@ static int	message_len;
 static uint16_t	send_len;
 
 void
-ao_serial1_putchar(char c)
+ao_gps_putchar(char c)
 {
 	int	i;
 	uint8_t	uc = (uint8_t) c;
@@ -196,7 +196,7 @@ ao_serial1_putchar(char c)
 #define AO_SERIAL_SPEED_115200	3
 
 static void
-ao_serial1_set_speed(uint8_t speed)
+ao_gps_set_speed(uint8_t speed)
 {
 	int	fd = ao_gps_fd;
 	struct termios	termios;
-- 
cgit v1.2.3


From 277577fecc71e3c52b823938f396cf42be403ebe Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sun, 26 May 2013 19:01:58 -0600
Subject: altos: Add pyro code testing to ao_flight_test for TeleMega

This parses the pyro settings and signals when the pyro channels are
fired in the output.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_pyro.c        | 120 +++++++++++++++++++++++++++-------------------
 src/test/Makefile         |   2 +-
 src/test/ao_flight_test.c |  57 ++++++++++++++++++++++
 3 files changed, 130 insertions(+), 49 deletions(-)

(limited to 'src/test')

diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c
index aac8fda5..b655eaca 100644
--- a/src/core/ao_pyro.c
+++ b/src/core/ao_pyro.c
@@ -15,10 +15,12 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+#ifndef AO_FLIGHT_TEST
 #include <ao.h>
-#include <ao_pyro.h>
 #include <ao_sample.h>
 #include <ao_flight.h>
+#endif
+#include <ao_pyro.h>
 
 #if IS_COMPANION
 #include <ao_companion.h>
@@ -130,12 +132,13 @@ ao_pyro_ready(struct ao_pyro *pyro)
 	return TRUE;
 }
 
+#ifndef AO_FLIGHT_TEST
 #define ao_pyro_fire_port(port, bit, pin) do {	\
 		ao_gpio_set(port, bit, pin, 1);	\
 		ao_delay(AO_MS_TO_TICKS(50));	\
 		ao_gpio_set(port, bit, pin, 0);	\
 	} while (0)
-
+#endif
 
 static void
 ao_pyro_fire(uint8_t p)
@@ -172,66 +175,58 @@ ao_pyro_fire(uint8_t p)
 
 uint8_t	ao_pyro_wakeup;
 
-static void
-ao_pyro(void)
+static uint8_t
+ao_pyro_check(void)
 {
-	uint8_t		p, any_waiting;
 	struct ao_pyro	*pyro;
+	uint8_t		p, any_waiting;
+	
+	any_waiting = 0;
+	for (p = 0; p < AO_PYRO_NUM; p++) {
+		pyro = &ao_config.pyro[p];
 
-	ao_config_get();
-	while (ao_flight_state < ao_flight_boost)
-		ao_sleep(&ao_flight_state);
-
-	for (;;) {
-		ao_alarm(AO_MS_TO_TICKS(100));
-		ao_sleep(&ao_pyro_wakeup);
-		ao_clear_alarm();
-		any_waiting = 0;
-		for (p = 0; p < AO_PYRO_NUM; p++) {
-			pyro = &ao_config.pyro[p];
+		/* Ignore igniters which have already fired
+		 */
+		if (pyro->fired)
+			continue;
 
-			/* Ignore igniters which have already fired
-			 */
-			if (pyro->fired)
-				continue;
+		/* Ignore disabled igniters
+		 */
+		if (!pyro->flags)
+			continue;
 
-			/* Ignore disabled igniters
-			 */
-			if (!pyro->flags)
+		any_waiting = 1;
+		/* Check pyro state to see if it should fire
+		 */
+		if (!pyro->delay_done) {
+			if (!ao_pyro_ready(pyro))
 				continue;
 
-			any_waiting = 1;
-			/* Check pyro state to see if it shoule fire
+			/* If there's a delay set, then remember when
+			 * it expires
 			 */
-			if (!pyro->delay_done) {
-				if (!ao_pyro_ready(pyro))
-					continue;
-
-				/* If there's a delay set, then remember when
-				 * it expires
-				 */
-				if (pyro->flags & ao_pyro_delay)
-					pyro->delay_done = ao_time() + pyro->delay;
-			}
-
-			/* Check to see if we're just waiting for
-			 * the delay to expire
-			 */
-			if (pyro->delay_done) {
-				if ((int16_t) (ao_time() - pyro->delay_done) < 0)
-					continue;
+			if (pyro->flags & ao_pyro_delay) {
+				pyro->delay_done = ao_time() + pyro->delay;
+				if (!pyro->delay_done)
+					pyro->delay_done = 1;
 			}
+		}
 
-			ao_pyro_fire(p);
+		/* Check to see if we're just waiting for
+		 * the delay to expire
+		 */
+		if (pyro->delay_done) {
+			if ((int16_t) (ao_time() - pyro->delay_done) < 0)
+				continue;
 		}
-		if (!any_waiting)
-			break;
+
+		ao_pyro_fire(p);
+		pyro->fired = 1;
+		ao_pyro_fired |= (1 << p);
 	}
-	ao_exit();
+	return any_waiting;
 }
 
-__xdata struct ao_task ao_pyro_task;
-
 #define NO_VALUE	0xff
 
 #define AO_PYRO_NAME_LEN	3
@@ -283,6 +278,34 @@ const struct {
 	{ "", ao_pyro_none,		NO_VALUE, HELP(NULL) },
 };
 
+#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0])
+
+#ifndef AO_FLIGHT_TEST
+static void
+ao_pyro(void)
+{
+	uint8_t		any_waiting;
+
+	ao_config_get();
+	while (ao_flight_state < ao_flight_boost)
+		ao_sleep(&ao_flight_state);
+
+	for (;;) {
+		ao_alarm(AO_MS_TO_TICKS(100));
+		ao_sleep(&ao_pyro_wakeup);
+		ao_clear_alarm();
+		if (ao_flight_state >= ao_flight_landed)
+			break;
+		any_waiting = ao_pyro_check();
+		if (!any_waiting)
+			break;
+	}
+	ao_exit();
+}
+
+__xdata struct ao_task ao_pyro_task;
+
+
 static void
 ao_pyro_print_name(uint8_t v)
 {
@@ -449,3 +472,4 @@ ao_pyro_init(void)
 	ao_cmd_register(&ao_pyro_cmds[0]);
 	ao_add_task(&ao_pyro_task, ao_pyro, "pyro");
 }
+#endif
diff --git a/src/test/Makefile b/src/test/Makefile
index 8032a163..3c9ac9c6 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -29,7 +29,7 @@ ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalm
 ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalman.c $(INCS)
 	cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c
 
-ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
+ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS)
 	cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm
 
 ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 99bed7ee..6e53d8e1 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -20,6 +20,7 @@
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <stddef.h>
 #include <string.h>
 #include <getopt.h>
 #include <math.h>
@@ -137,6 +138,18 @@ int	tick_offset;
 
 static int32_t	ao_k_height;
 
+int16_t
+ao_time(void)
+{
+	return ao_data_static.tick;
+}
+
+void
+ao_delay(int16_t interval)
+{
+	return;
+}
+
 void
 ao_ignite(enum ao_igniter igniter)
 {
@@ -200,6 +213,8 @@ struct ao_cmds {
 #include <ao_ms5607.h>
 struct ao_ms5607_prom	ms5607_prom;
 #include "ao_ms5607_convert.c"
+#define AO_PYRO_NUM	4
+#include <ao_pyro.h>
 #else
 #include "ao_convert.c"
 #endif
@@ -210,6 +225,9 @@ struct ao_config {
 	int16_t		accel_minus_g;
 	uint8_t		pad_orientation;
 	uint16_t	apogee_lockout;
+#if TELEMEGA
+	struct ao_pyro	pyro[AO_PYRO_NUM];	/* minor version 12 */
+#endif
 };
 
 #define AO_PAD_ORIENTATION_ANTENNA_UP	0
@@ -246,6 +264,24 @@ uint16_t	prev_tick;
 #include "ao_sqrt.c"
 #include "ao_sample.c"
 #include "ao_flight.c"
+#if TELEMEGA
+#define AO_PYRO_NUM	4
+
+#define AO_PYRO_0	0
+#define AO_PYRO_1	1
+#define AO_PYRO_2	2
+#define AO_PYRO_3	3
+
+static void
+ao_pyro_tell(int pin)
+{
+	printf ("fire pyro %d\n", pin);
+}
+
+#define ao_pyro_fire_port(port,bit,pin) ao_pyro_tell(pin)
+
+#include "ao_pyro.c"
+#endif
 
 #define to_double(f)	((f) / 65536.0)
 
@@ -771,6 +807,10 @@ ao_sleep(void *wchan)
 		char		*words[64];
 		int		nword;
 
+#if TELEMEGA
+		if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed)
+			ao_pyro_check();
+#endif
 		for (;;) {
 			if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
 			{
@@ -883,6 +923,23 @@ ao_sleep(void *wchan)
 				else if (strcmp(words[1], "crc:") == 0)
 					ms5607_prom.crc = strtoul(words[2], NULL, 10);
 				continue;
+			} else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) {
+				int	p = strtoul(words[1], NULL, 10);
+				int	i, j;
+				struct ao_pyro	*pyro = &ao_config.pyro[p];
+
+				for (i = 2; i < nword; i++) {
+					for (j = 0; j < NUM_PYRO_VALUES; j++)
+						if (!strcmp (words[2], ao_pyro_values[j].name))
+							break;
+					if (j == NUM_PYRO_VALUES)
+						continue;
+					pyro->flags |= ao_pyro_values[j].flag;
+					if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) {
+						int16_t	val = strtoul(words[++i], NULL, 10);
+						*((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val;
+					}
+				}
 			}
 #else
 			if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) {
-- 
cgit v1.2.3


From 956f4dff1cc521059434743624b1271fb92b96ae Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sun, 26 May 2013 19:39:13 -0600
Subject: altos: Light pyro charges simultaneously if so configured

Don't try to be nice to the battery, just let the pyro circuit deal
with it and try to get all of the specified circuits going at the same
time if they're configured to do so.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_pyro.c        | 49 ++++++++++++++++++++++++++++-------------------
 src/test/ao_flight_test.c |  6 ++----
 2 files changed, 31 insertions(+), 24 deletions(-)

(limited to 'src/test')

diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c
index b3cda656..84f949dc 100644
--- a/src/core/ao_pyro.c
+++ b/src/core/ao_pyro.c
@@ -135,45 +135,38 @@ ao_pyro_ready(struct ao_pyro *pyro)
 }
 
 #ifndef AO_FLIGHT_TEST
-#define ao_pyro_fire_port(port, bit, pin) do {	\
-		ao_gpio_set(port, bit, pin, 1);	\
-		ao_delay(AO_MS_TO_TICKS(50));	\
-		ao_gpio_set(port, bit, pin, 0);	\
-	} while (0)
-#endif
-
 static void
-ao_pyro_fire(uint8_t p)
+ao_pyro_pin_set(uint8_t p, uint8_t v)
 {
 	switch (p) {
 #if AO_PYRO_NUM > 0
-	case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0); break;
+	case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break;
 #endif
 #if AO_PYRO_NUM > 1
-	case 1: ao_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1); break;
+	case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break;
 #endif
 #if AO_PYRO_NUM > 2
-	case 2: ao_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2); break;
+	case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break;
 #endif
 #if AO_PYRO_NUM > 3
-	case 3: ao_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3); break;
+	case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break;
 #endif
 #if AO_PYRO_NUM > 4
-	case 4: ao_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4); break;
+	case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break;
 #endif
 #if AO_PYRO_NUM > 5
-	case 5: ao_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5); break;
+	case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break;
 #endif
 #if AO_PYRO_NUM > 6
-	case 6: ao_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6); break;
+	case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break;
 #endif
 #if AO_PYRO_NUM > 7
-	case 7: ao_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7); break;
+	case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break;
 #endif
 	default: break;
 	}
-	ao_delay(AO_MS_TO_TICKS(50));
 }
+#endif
 
 uint8_t	ao_pyro_wakeup;
 
@@ -182,6 +175,7 @@ ao_pyro_check(void)
 {
 	struct ao_pyro	*pyro;
 	uint8_t		p, any_waiting;
+	uint16_t	fire = 0;
 	
 	any_waiting = 0;
 	for (p = 0; p < AO_PYRO_NUM; p++) {
@@ -222,10 +216,25 @@ ao_pyro_check(void)
 				continue;
 		}
 
-		ao_pyro_fire(p);
-		pyro->fired = 1;
-		ao_pyro_fired |= (1 << p);
+		fire |= (1 << p);
 	}
+
+	if (fire) {
+		for (p = 0; p < AO_PYRO_NUM; p++) {
+			if (fire & (1 << p))
+				ao_pyro_pin_set(p, 1);
+		}
+		ao_delay(AO_MS_TO_TICKS(50));
+		for (p = 0; p < AO_PYRO_NUM; p++) {
+			if (fire & (1 << p)) {
+				ao_pyro_pin_set(p, 0);
+				ao_config.pyro[p].fired = 1;
+				ao_pyro_fired |= (1 << p);
+			}
+		}
+		ao_delay(AO_MS_TO_TICKS(50));
+	}
+
 	return any_waiting;
 }
 
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 6e53d8e1..faf31aa7 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -273,13 +273,11 @@ uint16_t	prev_tick;
 #define AO_PYRO_3	3
 
 static void
-ao_pyro_tell(int pin)
+ao_pyro_pin_set(uint8_t pin, uint8_t value)
 {
-	printf ("fire pyro %d\n", pin);
+	printf ("set pyro %d %d\n", pin, value);
 }
 
-#define ao_pyro_fire_port(port,bit,pin) ao_pyro_tell(pin)
-
 #include "ao_pyro.c"
 #endif
 
-- 
cgit v1.2.3


From fa0859a51576efe231effcb5995f325f9e7e0fcb Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sat, 17 Aug 2013 16:01:44 +0200
Subject: altos: Make FAT test program link explicitly against libcrypto

For some reason, the MD5_Final symbol isn't resolved when linking only
against libssl.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/test/Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/test')

diff --git a/src/test/Makefile b/src/test/Makefile
index 3c9ac9c6..75b1f848 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -69,7 +69,7 @@ ao_micropeak_test: ao_micropeak_test.c ao_microflight.c ao_kalman.h
 	cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm
 
 ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c
-	cc $(CFLAGS) -o $@ ao_fat_test.c -lssl
+	cc $(CFLAGS) -o $@ ao_fat_test.c -lssl -lcrypto
 
 ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c
 	cc $(CFLAGS) -o $@ ao_aes_test.c
-- 
cgit v1.2.3


From d0b4e926ecececa7499a301b6135189be119512e Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 22 May 2013 13:03:06 -0700
Subject: Initial TeleMini bits

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/cc1111/ao_adc.c                  |   5 ++
 src/cc1111/ao_usb.c                  |   5 ++
 src/core/ao_data.c                   |   2 +
 src/core/ao_data.h                   |   2 +-
 src/core/ao_log_mini.c               |   1 -
 src/core/ao_telemetry.c              |  40 +++++++++-
 src/core/ao_telemetry.h              |  27 +++++++
 src/drivers/ao_ms5607.c              |  40 +++++-----
 src/drivers/ao_ms5607.h              |   2 +-
 src/drivers/ao_ms5607_convert_8051.c |  60 +++++++++++++++
 src/telemini-v2.0/Makefile           | 108 +++++++++++++++++++++++++++
 src/telemini-v2.0/ao_pins.h          | 140 +++++++++++++++++++++++++++++++++++
 src/telemini-v2.0/ao_telemini.c      |  54 ++++++++++++++
 src/test/Makefile                    |   5 +-
 14 files changed, 466 insertions(+), 25 deletions(-)
 create mode 100644 src/drivers/ao_ms5607_convert_8051.c
 create mode 100644 src/telemini-v2.0/Makefile
 create mode 100644 src/telemini-v2.0/ao_pins.h
 create mode 100644 src/telemini-v2.0/ao_telemini.c

(limited to 'src/test')

diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c
index 4a58023d..6cc08399 100644
--- a/src/cc1111/ao_adc.c
+++ b/src/cc1111/ao_adc.c
@@ -162,6 +162,11 @@ ao_adc_isr(void) __interrupt 1
 #define GOT_ADC
 #endif	
 
+#ifdef FETCH_ADC
+	FETCH_ADC()
+#define GOT_ADC
+#endif
+
 #ifndef GOT_ADC
 #error No known ADC configuration set
 #endif
diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c
index a655d1be..b0ab409d 100644
--- a/src/cc1111/ao_usb.c
+++ b/src/cc1111/ao_usb.c
@@ -201,6 +201,11 @@ ao_usb_ep0_setup(void)
 				ao_usb_ep0_queue_byte(0);
 				break;
 			case AO_USB_REQ_SET_ADDRESS:
+#if USB_FORCE_FLIGHT_IDLE
+				/* Go to idle mode if USB is connected
+				 */
+				ao_flight_force_idle = 1;
+#endif
 				ao_usb_set_address(ao_usb_setup.value);
 				break;
 			case AO_USB_REQ_GET_DESCRIPTOR:
diff --git a/src/core/ao_data.c b/src/core/ao_data.c
index 38d2f7ff..6a3d02a1 100644
--- a/src/core/ao_data.c
+++ b/src/core/ao_data.c
@@ -22,6 +22,7 @@ volatile __xdata struct ao_data	ao_data_ring[AO_DATA_RING];
 volatile __data uint8_t		ao_data_head;
 volatile __data uint8_t		ao_data_present;
 
+#ifndef ao_data_count
 void
 ao_data_get(__xdata struct ao_data *packet)
 {
@@ -32,3 +33,4 @@ ao_data_get(__xdata struct ao_data *packet)
 #endif
 	memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data));
 }
+#endif
diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index b0f086f8..080a534f 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -101,7 +101,7 @@ extern volatile __data uint8_t		ao_data_count;
  * signaled by the timer tick
  */
 #define AO_DATA_WAIT() do {				\
-		ao_sleep((void *) &ao_data_count);	\
+		ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count));	\
 	} while (0)
 
 #endif /* AO_DATA_RING */
diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c
index 1273b0e3..46b285f3 100644
--- a/src/core/ao_log_mini.c
+++ b/src/core/ao_log_mini.c
@@ -79,7 +79,6 @@ void
 ao_log(void)
 {
 	__pdata uint16_t	next_sensor, next_other;
-	uint8_t			i;
 
 	ao_storage_setup();
 
diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index 03a8a273..9c673030 100644
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -171,6 +171,36 @@ ao_send_mega_data(void)
 }
 #endif /* AO_SEND_MEGA */
 
+#ifdef AO_SEND_MINI
+
+static void
+ao_send_mini(void)
+{
+	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
+			
+	telemetry.generic.tick = packet->tick;
+	telemetry.generic.type = AO_TELEMETRY_MINI;
+
+	telemetry.mini.state = ao_flight_state;
+
+	telemetry.mini.v_batt = packet->adc.v_batt;
+	telemetry.mini.sense_a = packet->adc.sense_a;
+	telemetry.mini.sense_m = packet->adc.sense_m;
+
+	telemetry.mini.pres = ao_data_pres(packet);
+	telemetry.mini.temp = ao_data_temp(packet);
+
+	telemetry.mini.acceleration = ao_accel;
+	telemetry.mini.speed = ao_speed;
+	telemetry.mini.height = ao_height;
+
+	telemetry.mini.ground_pres = ao_ground_pres;
+
+	ao_radio_send(&telemetry, sizeof (telemetry));
+}
+
+#endif
+
 #ifdef AO_SEND_ALL_BARO
 static uint8_t		ao_baro_sample;
 
@@ -323,12 +353,16 @@ ao_telemetry(void)
 				ao_send_baro();
 #endif
 #if HAS_FLIGHT
-#ifdef AO_SEND_MEGA
+# ifdef AO_SEND_MEGA
 				ao_send_mega_sensor();
 				ao_send_mega_data();
-#else
+# else
+#  ifdef AO_SEND_MINI
+				ao_send_mini();
+#  else
 				ao_send_sensor();
-#endif
+#  endif
+# endif
 #endif
 
 #if HAS_COMPANION
diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h
index f2d201de..77601529 100644
--- a/src/core/ao_telemetry.h
+++ b/src/core/ao_telemetry.h
@@ -207,6 +207,32 @@ struct ao_telemetry_mega_data {
 };
 
 
+#define AO_TELEMETRY_MINI		0x10
+
+struct ao_telemetry_mini {
+	uint16_t	serial;		/*  0 */
+	uint16_t	tick;		/*  2 */
+	uint8_t		type;		/*  4 */
+
+	uint8_t         state;          /*  5 flight state */
+
+	int16_t		v_batt;		/*  6 battery voltage */
+	int16_t		sense_a;	/*  8 apogee continuity */
+	int16_t		sense_m;	/* 10 main continuity */
+
+	int32_t		pres;		/* 12 Pa * 10 */
+	int16_t		temp;		/* 16 °C * 100 */
+
+	int16_t         acceleration;   /* 18 m/s² * 16 */
+	int16_t         speed;          /* 20 m/s * 16 */
+	int16_t         height;         /* 22 m */
+
+	int32_t		ground_pres;	/* 24 average pres on pad */
+
+	int32_t		pad28;		/* 28 */
+	/* 32 */
+};
+
 /* #define AO_SEND_ALL_BARO */
 
 #define AO_TELEMETRY_BARO		0x80
@@ -240,6 +266,7 @@ union ao_telemetry_all {
 	struct ao_telemetry_companion		companion;
 	struct ao_telemetry_mega_sensor		mega_sensor;
 	struct ao_telemetry_mega_data		mega_data;
+	struct ao_telemetry_mini		mini;
 	struct ao_telemetry_baro		baro;
 };
 
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index 8f1dcbe1..5259b265 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -21,7 +21,7 @@
 
 #if HAS_MS5607 || HAS_MS5611
 
-static struct ao_ms5607_prom	ms5607_prom;
+static __xdata struct ao_ms5607_prom	ms5607_prom;
 static uint8_t	  		ms5607_configured;
 
 static void
@@ -40,7 +40,7 @@ ao_ms5607_reset(void) {
 
 	cmd = AO_MS5607_RESET;
 	ao_ms5607_start();
-	ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+	ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
 	ao_delay(AO_MS_TO_TICKS(10));
 	ao_ms5607_stop();
 }
@@ -69,17 +69,17 @@ ao_ms5607_crc(uint8_t *prom)
 }
 
 static void
-ao_ms5607_prom_read(struct ao_ms5607_prom *prom)
+ao_ms5607_prom_read(__xdata struct ao_ms5607_prom *prom)
 {
-	uint8_t		addr;
-	uint8_t		crc;
-	uint16_t	*r;
+	uint8_t			addr;
+	uint8_t			crc;
+	__xdata uint16_t	*r;
 
-	r = (uint16_t *) prom;
+	r = (__xdata uint16_t *) prom;
 	for (addr = 0; addr < 8; addr++) {
 		uint8_t	cmd = AO_MS5607_PROM_READ(addr);
 		ao_ms5607_start();
-		ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+		ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
 		ao_spi_recv(r, 2, AO_MS5607_SPI_INDEX);
 		ao_ms5607_stop();
 		r++;
@@ -114,25 +114,25 @@ ao_ms5607_setup(void)
 	ao_ms5607_prom_read(&ms5607_prom);
 }
 
-static volatile uint8_t	ao_ms5607_done;
+static __xdata volatile uint8_t	ao_ms5607_done;
 
 static void
 ao_ms5607_isr(void)
 {
 	ao_exti_disable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN);
 	ao_ms5607_done = 1;
-	ao_wakeup((void *) &ao_ms5607_done);
+	ao_wakeup((__xdata void *) &ao_ms5607_done);
 }
 
 static uint32_t
 ao_ms5607_get_sample(uint8_t cmd) {
-	uint8_t	reply[3];
-	uint8_t read;
+	__xdata uint8_t	reply[3];
+	__xdata uint8_t read;
 
 	ao_ms5607_done = 0;
 
 	ao_ms5607_start();
-	ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX);
+	ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX);
 
 	ao_exti_enable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN);
 
@@ -179,10 +179,14 @@ ao_ms5607_sample(struct ao_ms5607_sample *sample)
 	sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2);
 }
 
+#ifdef _CC1111_H_
+#include "ao_ms5607_convert_8051.c"
+#else
 #include "ao_ms5607_convert.c"
+#endif
 
 #if HAS_TASK
-struct ao_ms5607_sample	ao_ms5607_current;
+__xdata struct ao_ms5607_sample	ao_ms5607_current;
 
 static void
 ao_ms5607(void)
@@ -191,10 +195,10 @@ ao_ms5607(void)
 	for (;;)
 	{
 		ao_ms5607_sample(&ao_ms5607_current);
-		ao_arch_critical(
-			AO_DATA_PRESENT(AO_DATA_MS5607);
-			AO_DATA_WAIT();
-			);
+		ao_arch_block_interrupts();
+		AO_DATA_PRESENT(AO_DATA_MS5607);
+		AO_DATA_WAIT();
+		ao_arch_release_interrupts();
 	}
 }
 
diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h
index b2f98a59..3fd43fd4 100644
--- a/src/drivers/ao_ms5607.h
+++ b/src/drivers/ao_ms5607.h
@@ -56,7 +56,7 @@ struct ao_ms5607_value {
 	int32_t		temp;	/* in °C * 100 */
 };
 
-extern struct ao_ms5607_sample	ao_ms5607_current;
+extern __xdata struct ao_ms5607_sample	ao_ms5607_current;
 
 void
 ao_ms5607_setup(void);
diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c
new file mode 100644
index 00000000..f47972c9
--- /dev/null
+++ b/src/drivers/ao_ms5607_convert_8051.c
@@ -0,0 +1,60 @@
+/*
+ * 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_ms5607.h>
+
+void
+ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value)
+{
+#if 0
+	int32_t	dT;
+	int32_t TEMP;
+	int64_t OFF;
+	int64_t SENS;
+
+	dT = sample->temp - ((int32_t) ms5607_prom.tref << 8);
+	
+	TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23);
+
+#if HAS_MS5611
+	OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7);
+	SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8);
+#else
+	OFF = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6);
+	SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7);
+#endif
+
+	if (TEMP < 2000) {
+		int32_t	T2 = ((int64_t) dT * (int64_t) dT) >> 31;
+		int32_t TEMPM = TEMP - 2000;
+		int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4;
+		int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM;
+		if (TEMP < 1500) {
+			int32_t TEMPP = TEMP + 1500;
+			int64_t TEMPP2 = TEMPP * TEMPP;
+			OFF2 = OFF2 + 15 * TEMPP2;
+			SENS2 = SENS2 + 8 * TEMPP2;
+		}
+		TEMP -= T2;
+		OFF -= OFF2;
+		SENS -= SENS2;
+	}
+
+	value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15;
+	value->temp = TEMP;
+#endif
+}
diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile
new file mode 100644
index 00000000..afd529f0
--- /dev/null
+++ b/src/telemini-v2.0/Makefile
@@ -0,0 +1,108 @@
+#
+# TeleMini build file
+#
+
+TELEMINI_VER=2.0
+TELEMINI_DEF=2_0
+
+vpath %.c ..:../core:../cc1111:../drivers:../product
+vpath %.h ..:../core:../cc1111:../drivers:../product
+vpath ao-make-product.5c ../util
+
+ifndef VERSION
+include ../Version
+endif
+
+INC = \
+	ao.h \
+	ao_pins.h \
+	ao_arch.h \
+	ao_arch_funcs.h \
+	cc1111.h \
+	ao_product.h
+
+CORE_SRC = \
+	ao_cmd.c \
+	ao_config.c \
+	ao_convert.c \
+	ao_flight.c \
+	ao_kalman.c \
+	ao_log.c \
+	ao_log_mini.c \
+	ao_mutex.c \
+	ao_panic.c \
+	ao_report.c \
+	ao_sample.c \
+	ao_stdio.c \
+	ao_storage.c \
+	ao_task.c \
+	ao_telemetry.c \
+	ao_freq.c
+
+CC1111_SRC = \
+	ao_adc.c \
+	ao_dma.c \
+	ao_ignite.c \
+	ao_led.c \
+	ao_packet.c \
+	ao_packet_slave.c \
+	ao_radio.c \
+	ao_romconfig.c \
+	ao_string.c \
+	ao_spi.c \
+	ao_usb.c \
+	ao_convert_pa.c \
+	ao_data.c \
+	ao_beep.c \
+	ao_timer.c \
+	_bp.c
+
+DRIVER_SRC = \
+	ao_ms5607.c \
+	ao_m25.c
+
+PRODUCT_SRC = \
+	ao_telemini.c
+
+SRC = \
+	$(CORE_SRC) \
+	$(CC1111_SRC) \
+	$(DRIVER_SRC) \
+	$(PRODUCT_SRC)
+
+PROGNAME = telemini-v$(TELEMINI_VER)
+PROG = $(PROGNAME)-$(VERSION).ihx
+PRODUCT=TeleMini-v$(TELEMINI_VER)
+PRODUCT_DEF=-DTELEMINI_V_$(TELEMINI_DEF)
+IDPRODUCT=0x000a
+
+include ../cc1111/Makefile.cc1111
+
+NICKLE=nickle
+CHECK_STACK=sh ../util/check-stack
+
+V=0
+# The user has explicitly enabled quiet compilation.
+ifeq ($(V),0)
+quiet = @printf "  $1 $2 $@\n"; $($1)
+endif
+# Otherwise, print the full command line.
+quiet ?= $($1)
+
+all: ../$(PROG)
+
+../$(PROG): $(REL) Makefile
+	$(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) ..
+	$(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@
+
+ao_product.h: ao-make-product.5c ../Version
+	$(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
+
+distclean:	clean
+
+clean: clean-cc1111
+
+install:
+
+uninstall:
+
diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h
new file mode 100644
index 00000000..386c8dc3
--- /dev/null
+++ b/src/telemini-v2.0/ao_pins.h
@@ -0,0 +1,140 @@
+/*
+ * Copyright © 2010 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_PINS_H_
+#define _AO_PINS_H_
+
+#define HAS_RADIO	1
+
+#define HAS_FLIGHT		1
+#define HAS_USB			1
+#define USB_FORCE_FLIGHT_IDLE	1
+#define HAS_BEEP		1
+#define HAS_GPS			0
+#define HAS_SERIAL_1		0
+#define HAS_EEPROM		1
+#define HAS_LOG			1
+#define USE_INTERNAL_FLASH	0
+#define HAS_DBG			0
+#define PACKET_HAS_SLAVE	1
+#define USE_FAST_ASCENT_LOG	1
+
+#define AO_LED_GREEN		1
+#define AO_LED_RED		2
+#define LEDS_AVAILABLE		(AO_LED_RED|AO_LED_GREEN)
+#define HAS_EXTERNAL_TEMP	0
+#define HAS_ACCEL		0
+#define HAS_IGNITE		1
+#define HAS_IGNITE_REPORT 1
+#define HAS_MONITOR		0
+
+/*
+ * SPI
+ */
+
+#define SPI_CS_PORT		P1
+#define SPI_CS_SEL		P1SEL
+#define SPI_CS_DIR		P1DIR
+
+/*
+ * Flash
+ */
+#define AO_M25_SPI_CS_PORT	SPI_CS_PORT
+#define AO_M25_SPI_CS_MASK	0x04	/* cs_flash is P1_2 */
+#define M25_MAX_CHIPS		1
+
+/*
+ * MS5607
+ */
+
+#define HAS_MS5607		1
+#define HAS_MS5611		0
+#define AO_MS5607_PRIVATE_PINS	0
+#define AO_MS5607_CS_PORT	P1
+#define AO_MS5607_CS_PIN	3
+#define AO_MS5607_CS		P1_3
+#define AO_MS5607_CS_MASK	(1 << AO_MS5607_CS_PIN)
+#define AO_MS5607_MISO_PORT	P0
+#define AO_MS5607_MISO_PIN	2
+#define AO_MS5607_MISO_MASK	(1 << AO_MS5607_MISO_PIN)
+#define AO_MS5607_SPI_INDEX	0
+
+/*
+ * Igniters
+ */
+#define AO_IGNITER_PORT		P2
+#define AO_IGNITER_DROGUE_PORT	AO_IGNITER_PORT
+#define AO_IGNITER_DROGUE	P2_3
+#define AO_IGNITER_MAIN		P2_4
+#define AO_IGNITER_DIR		P2DIR
+#define AO_IGNITER_DROGUE_BIT	(1 << 3)
+#define AO_IGNITER_MAIN_BIT	(1 << 4)
+#define AO_IGNITER_DROGUE_PIN	3
+#define AO_IGNITER_MAIN_PIN	4
+
+#define AO_IGNITER_DROGUE_PORT	AO_IGNITER_PORT
+#define AO_IGNITER_MAIN_PORT	AO_IGNITER_PORT
+
+/* test these values with real igniters */
+#define AO_IGNITER_OPEN		1000
+#define AO_IGNITER_CLOSED	7000
+#define AO_IGNITER_FIRE_TIME	AO_MS_TO_TICKS(50)
+#define AO_IGNITER_CHARGE_TIME	AO_MS_TO_TICKS(2000)
+
+#define AO_SEND_MINI
+
+/*
+ * ADC
+ */
+
+#define HAS_ADC			1
+#define AO_ADC_FIRST_PIN	0
+
+struct ao_adc {
+	int16_t		sense_a;	/* apogee continuity sense */
+	int16_t		sense_m;	/* main continuity sense */
+	int16_t		v_batt;		/* battery voltage */
+};
+
+#define ao_data_count	ao_adc_count
+
+#define AO_SENSE_DROGUE(p)	((p)->adc.sense_a)
+#define AO_SENSE_MAIN(p)	((p)->adc.sense_m)
+
+#define AO_ADC_DUMP(p) \
+	printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \
+	       (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt)
+
+#define FETCH_ADC() 							\
+	a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc); 	\
+	switch (sequence) {						\
+	case 4:								\
+		a += 4;							\
+		sequence = 0;						\
+		break;							\
+	case 1:								\
+		a += 2;							\
+		sequence = 4;						\
+		break;							\
+	case 0:								\
+		sequence = 1;						\
+		break;							\
+	}								\
+	if (sequence)							\
+		;
+
+#endif /* _AO_PINS_H_ */
diff --git a/src/telemini-v2.0/ao_telemini.c b/src/telemini-v2.0/ao_telemini.c
new file mode 100644
index 00000000..84df2b8c
--- /dev/null
+++ b/src/telemini-v2.0/ao_telemini.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright © 2011 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_pins.h"
+
+__xdata uint8_t ao_force_freq;
+
+void
+main(void)
+{
+	/*
+	 * Reduce the transient on the ignite pins at startup by
+	 * pulling the pins low as soon as possible at power up
+	 */
+	ao_ignite_set_pins();
+
+	ao_clock_init();
+
+	/* Turn on the red LED until the system is stable */
+	ao_led_init(LEDS_AVAILABLE);
+	ao_led_on(AO_LED_RED);
+
+	ao_task_init();
+
+	ao_timer_init();
+	ao_adc_init();
+	ao_cmd_init();
+	ao_storage_init();
+	ao_ms5607_init();
+	ao_flight_init();
+	ao_log_init();
+	ao_report_init();
+	ao_telemetry_init();
+	ao_radio_init();
+	ao_packet_slave_init(TRUE);
+	ao_igniter_init();
+	ao_config_init();
+	ao_start_scheduler();
+}
diff --git a/src/test/Makefile b/src/test/Makefile
index 75b1f848..9c304318 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -2,7 +2,7 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes
 
 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_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \
-	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test
+	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test
 
 INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
 
@@ -73,3 +73,6 @@ ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c
 
 ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c
 	cc $(CFLAGS) -o $@ ao_aes_test.c
+
+ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h
+	cc $(CFLAGS) -o $@ ao_int64_test.c
-- 
cgit v1.2.3


From 3114baef45803250a2e5cdd2ee4a9171f2045b0c Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 22 May 2013 14:32:50 -0700
Subject: altos: Add 64-bit add/mul/shift for SDCC

SDCC doeesn't provide a native 64-bit type (sigh), so
implement the minimal operations necessary for the MS5607 conversion
routine.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_int64.c      | 119 +++++++++++++++++++++++++++++++++++++++++++++++
 src/core/ao_int64.h      |  39 ++++++++++++++++
 src/test/ao_int64_test.c |  78 +++++++++++++++++++++++++++++++
 3 files changed, 236 insertions(+)
 create mode 100644 src/core/ao_int64.c
 create mode 100644 src/core/ao_int64.h
 create mode 100644 src/test/ao_int64_test.c

(limited to 'src/test')

diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c
new file mode 100644
index 00000000..1fe717dc
--- /dev/null
+++ b/src/core/ao_int64.c
@@ -0,0 +1,119 @@
+/*
+ * 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.
+ */
+
+#include <ao_int64.h>
+
+void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
+	uint32_t	t;
+
+	r->low = t = a->low + b->low;
+	r->high = a->high + b->high;
+	if (t < a->low)
+		r->high++;
+}
+
+void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
+	if (d < 32) {
+		r->high = (int32_t) a->high >> d;
+		r->low = a->low >> d;
+		if (d)
+			r->low |= a->high << (32 - d);
+	} else {
+		d &= 0x1f;
+		r->high = 0;
+		r->low = (int32_t) a->high >> d;
+	}
+}
+
+void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
+	if (d < 32) {
+		r->high = a->high << d;
+		r->low = a->low << d;
+		if (d)
+			r->high |= a->low >> (32 - d);
+	} else {
+		d &= 0x1f;
+		r->low = 0;
+		r->high = a->low << d;
+	}
+}
+
+void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b)
+{
+	uint32_t	r1;
+	uint32_t	r2, r3, r4;
+	ao_int64_t	s,t,u,v;
+	r1 = (uint32_t) (uint16_t) a * (uint16_t) b;
+	r2 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b;
+	r3 = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16);
+	r4 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16);
+
+	s.low = r1;
+	s.high = r4;
+
+	t.high = (uint32_t) r2 >> 16;
+	t.low = r2 << 16;
+	ao_plus64(&u, &s, &t);
+
+	v.high = (int32_t) r3 >> 16;
+	v.low = r3 << 16;
+	ao_plus64(r, &u, &v);
+}
+
+void ao_neg64(ao_int64_t *r, ao_int64_t *a) {
+	r->high = ~a->high;
+	r->low = ~a->low;
+	if (!++r->low)
+		r->high++;
+}
+
+void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) {
+	uint8_t		negative = 0;
+
+	if (a < 0) {
+		a = -a;
+		negative = 1;
+	}
+	if (b < 0) {
+		b = -b;
+		negative = !negative;
+	}
+	ao_umul64(r, a, b);
+	if (negative)
+		ao_neg64(r, r);
+}
+
+void ao_umul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
+	uint32_t	low = a->low;
+	ao_umul64(r, (uint32_t) low >> 1, (uint32_t) b << 1);
+	if (low & 1) {
+		if ((uint32_t) (r->low += b) < (uint32_t) b)
+			r->high++;
+	}
+	r->high += a->high * b;
+}
+
+void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
+	if ((int32_t) a->high < 0) {
+		ao_int64_t	t;
+
+		ao_neg64(&t, a);
+		ao_umul64_16(r, &t, b);
+		ao_neg64(r, r);
+	} else
+		ao_umul64_16(r, a, b);
+}
diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h
new file mode 100644
index 00000000..93aa87e4
--- /dev/null
+++ b/src/core/ao_int64.h
@@ -0,0 +1,39 @@
+/*
+ * 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_INT64_H_
+#define _AO_INT64_H_
+
+#include <stdint.h>
+
+typedef struct {
+	uint32_t	high;
+	uint32_t	low;
+} ao_int64_t;
+
+void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
+void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
+void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
+void ao_mul64(ao_int64_t *r, int32_t a, int32_t b);
+void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b);
+
+#define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a))
+#define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b))
+
+#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low)
+
+#endif /* _AO_INT64_H_ */
diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c
new file mode 100644
index 00000000..67ba6ec5
--- /dev/null
+++ b/src/test/ao_int64_test.c
@@ -0,0 +1,78 @@
+/*
+ * 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.
+ */
+
+#include <ao_int64.h>
+#include <ao_int64.c>
+#include <stdio.h>
+#include <stdlib.h>
+
+int	errors;
+
+#define test(op,func,a,b,ao_a,ao_b) do {				\
+		r = (a) op (b);						\
+		func(&ao_r, ao_a, ao_b);				\
+		c = ao_cast64(&ao_r);					\
+		if (c != r) {						\
+			printf ("trial %4d: %lld " #func " %lld = %lld (should be %lld)\n", \
+				trial, (int64_t) (a), (int64_t) b, c, r); \
+			++errors;					\
+		}							\
+	} while (0)
+
+
+void
+do_test(int trial, int64_t a, int64_t b)
+{
+	int64_t	r, c;
+	ao_int64_t	ao_a, ao_b, ao_r;
+
+	ao_int64_init64(&ao_a, a >> 32, a);
+	ao_int64_init64(&ao_b, b >> 32, b);
+
+	test(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test(*, ao_mul64,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b);
+	test(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+	test(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+	test(*, ao_mul64_16, a, (uint16_t) b, &ao_a, (uint16_t) b);
+}
+
+#define TESTS	10000000
+
+static int64_t
+random64(void)
+{
+	return (int64_t) random() + ((int64_t) random() << 31) /* + ((int64_t) random() << 33) */;
+}
+
+int
+main (int argc, char **argv)
+{
+	int	i, start;
+
+	if (argv[1])
+		start = atoi(argv[1]);
+	else
+		start = 0;
+	srandom(1000);
+	for (i = 0; i < TESTS; i++) {
+		int64_t a = random64();
+		int64_t b = random64();
+		if (i >= start)
+			do_test(i, a, b);
+	}
+	return errors;
+}
-- 
cgit v1.2.3


From 5ccd902d0fd2adc40c72982babb60fac4da6a087 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 22 May 2013 17:08:55 -0700
Subject: altos: Add 64x64 multiply. Test 64 ops for dest same as either source

The test change is to ensure that the destination may be one of the 64
bit sources.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_int64.c      | 71 +++++++++++++++++++++++++++++++++++-------------
 src/core/ao_int64.h      |  9 ++++--
 src/test/ao_int64_test.c | 36 +++++++++++++++++++++---
 3 files changed, 91 insertions(+), 25 deletions(-)

(limited to 'src/test')

diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c
index 8e3caa24..5307342d 100644
--- a/src/core/ao_int64.c
+++ b/src/core/ao_int64.c
@@ -20,39 +20,40 @@
 void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
 	uint32_t	t;
 
-	r->low = t = a->low + b->low;
 	r->high = a->high + b->high;
+	t = a->low + b->low;
 	if (t < a->low)
 		r->high++;
+	r->low = t;
 }
 
 void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
 	if (d < 32) {
-		r->high = (int32_t) a->high >> d;
 		r->low = a->low >> d;
 		if (d)
 			r->low |= a->high << (32 - d);
+		r->high = (int32_t) a->high >> d;
 	} else {
 		d &= 0x1f;
-		r->high = 0;
 		r->low = (int32_t) a->high >> d;
+		r->high = 0;
 	}
 }
 
 void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
 	if (d < 32) {
 		r->high = a->high << d;
-		r->low = a->low << d;
 		if (d)
 			r->high |= a->low >> (32 - d);
+		r->low = a->low << d;
 	} else {
 		d &= 0x1f;
-		r->low = 0;
 		r->high = a->low << d;
+		r->low = 0;
 	}
 }
 
-void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b)
+static void ao_umul64_32_32(ao_int64_t *r, uint32_t a, uint32_t b)
 {
 	uint32_t	r1;
 	uint32_t	r2, r3, r4;
@@ -65,11 +66,11 @@ void ao_umul64(ao_int64_t *r, uint32_t a, uint32_t b)
 	s.low = r1;
 	s.high = r4;
 
-	t.high = (uint32_t) r2 >> 16;
+	t.high = r2 >> 16;
 	t.low = r2 << 16;
 	ao_plus64(&u, &s, &t);
 
-	v.high = (int32_t) r3 >> 16;
+	v.high = r3 >> 16;
 	v.low = r3 << 16;
 	ao_plus64(r, &u, &v);
 }
@@ -81,7 +82,7 @@ void ao_neg64(ao_int64_t *r, ao_int64_t *a) {
 		r->high++;
 }
 
-void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) {
+void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b) {
 	uint8_t		negative = 0;
 
 	if (a < 0) {
@@ -92,23 +93,55 @@ void ao_mul64(ao_int64_t *r, int32_t a, int32_t b) {
 		b = -b;
 		negative = ~negative;
 	}
-	ao_umul64(r, a, b);
+	ao_umul64_32_32(r, a, b);
 	if (negative)
 		ao_neg64(r, r);
 }
 
-void ao_umul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
-	ao_umul64(r, a->low, b);
-	r->high += a->high * b;
+static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
+	ao_int64_t	r2, r3;
+
+	ao_umul64_32_32(&r2, a->high, b->low);
+	ao_umul64_32_32(&r3, a->low, b->high);
+	ao_umul64_32_32(r, a->low, b->low);
+
+	r->high += r2.low + r3.low;
 }
 
-void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
-	if ((int32_t) a->high < 0) {
-		ao_int64_t	t;
+void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
+	uint8_t	negative = 0;
+	ao_int64_t	ap, bp;
 
-		ao_neg64(&t, a);
-		ao_umul64_16(r, &t, b);
+	if (ao_int64_negativep(a)) {
+		ao_neg64(&ap, a);
+		a = &ap;
+		negative = ~0;
+	}
+	if (ao_int64_negativep(b)) {
+		ao_neg64(&bp, b);
+		b = &bp;
+		negative = ~negative;
+	}
+	ao_umul64(r, a, b);
+	if (negative)
 		ao_neg64(r, r);
+}
+
+void ao_umul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
+	uint32_t h = a->high * b;
+	ao_umul64_32_32(r, a->low, b);
+	r->high += h;
+}
+
+void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
+	ao_int64_t	ap;
+	uint8_t		negative = 0;
+	if ((int32_t) a->high < 0) {
+		ao_neg64(&ap, a);
+		a = &ap;
+		negative = ~0;
 	} else
-		ao_umul64_16(r, a, b);
+		ao_umul64_64_16(r, a, b);
+	if (negative)
+		ao_neg64(r, r);
 }
diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h
index 93aa87e4..e5eee823 100644
--- a/src/core/ao_int64.h
+++ b/src/core/ao_int64.h
@@ -26,14 +26,19 @@ typedef struct {
 } ao_int64_t;
 
 void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
+void ao_neg64(ao_int64_t *r, ao_int64_t *a);
+void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d);
 void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
 void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
-void ao_mul64(ao_int64_t *r, int32_t a, int32_t b);
-void ao_mul64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b);
+void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
+void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b);
+void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b);
 
 #define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a))
 #define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b))
 
 #define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low)
 
+#define ao_int64_negativep(a)	(((int32_t) (a)->high) < 0)
+
 #endif /* _AO_INT64_H_ */
diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c
index 67ba6ec5..4c88b1a1 100644
--- a/src/test/ao_int64_test.c
+++ b/src/test/ao_int64_test.c
@@ -22,17 +22,35 @@
 
 int	errors;
 
-#define test(op,func,a,b,ao_a,ao_b) do {				\
+#define test_o(op,func,mod,a,b,ao_a,ao_b) do {				\
 		r = (a) op (b);						\
 		func(&ao_r, ao_a, ao_b);				\
 		c = ao_cast64(&ao_r);					\
 		if (c != r) {						\
-			printf ("trial %4d: %lld " #func " %lld = %lld (should be %lld)\n", \
+			printf ("trial %4d: %lld " #func mod " %lld = %lld (should be %lld)\n", \
 				trial, (int64_t) (a), (int64_t) b, c, r); \
 			++errors;					\
 		}							\
 	} while (0)
 
+#define test(op,func,a,b,ao_a,ao_b) test_o(op,func,"",a,b,ao_a,ao_b)
+
+#define test_a(op,func,a,b,ao_a,ao_b) do {	\
+		ao_r = *ao_a;			\
+		test_o(op,func,"_a",a,b,&ao_r,ao_b);	\
+	} while (0)
+
+#define test_b(op,func,a,b,ao_a,ao_b) do {	\
+		ao_r = *ao_b;			\
+		test_o(op,func,"_b",a,b,ao_a,&ao_r);	\
+	} while (0)
+
+#define test_x(op,func,a,b,ao_a,ao_b) do {	\
+		ao_r = *ao_a;			\
+		test_o(op,func,"_xa",a,a,&ao_r,&ao_r);	\
+		ao_r = *ao_b;			\
+		test_o(op,func,"_xb",b,b,&ao_r,&ao_r);	\
+	} while (0)
 
 void
 do_test(int trial, int64_t a, int64_t b)
@@ -44,10 +62,20 @@ do_test(int trial, int64_t a, int64_t b)
 	ao_int64_init64(&ao_b, b >> 32, b);
 
 	test(+, ao_plus64, a, b, &ao_a, &ao_b);
-	test(*, ao_mul64,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b);
+	test_a(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test_b(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test_x(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test(*, ao_mul64_32_32,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b);
+	test(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test_a(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test_b(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test_x(*, ao_mul64, a, b, &ao_a, &ao_b);
+	test(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b);
+	test_a(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b);
 	test(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
+	test_a(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
 	test(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
-	test(*, ao_mul64_16, a, (uint16_t) b, &ao_a, (uint16_t) b);
+	test_a(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f);
 }
 
 #define TESTS	10000000
-- 
cgit v1.2.3


From cb844328322fd7d9f4dafb58b322257a70b347e6 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 22 May 2013 19:20:54 -0600
Subject: altos: Add 64-bit subtraction

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_int64.c      | 11 +++++++++++
 src/core/ao_int64.h      |  2 ++
 src/test/ao_int64_test.c |  4 ++++
 3 files changed, 17 insertions(+)

(limited to 'src/test')

diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c
index 5307342d..07cdb357 100644
--- a/src/core/ao_int64.c
+++ b/src/core/ao_int64.c
@@ -27,6 +27,17 @@ void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
 	r->low = t;
 }
 
+void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
+	uint32_t	t;
+
+
+	r->high = a->high - b->high;
+	t = a->low - b->low;
+	if (t > a->low)
+		r->high--;
+	r->low = t;
+}
+
 void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
 	if (d < 32) {
 		r->low = a->low >> d;
diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h
index e5eee823..cf12f0d8 100644
--- a/src/core/ao_int64.h
+++ b/src/core/ao_int64.h
@@ -26,6 +26,7 @@ typedef struct {
 } ao_int64_t;
 
 void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
+void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
 void ao_neg64(ao_int64_t *r, ao_int64_t *a);
 void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d);
 void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
@@ -33,6 +34,7 @@ void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
 void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
 void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b);
 void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b);
+void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
 
 #define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a))
 #define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b))
diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c
index 4c88b1a1..c26a63b0 100644
--- a/src/test/ao_int64_test.c
+++ b/src/test/ao_int64_test.c
@@ -65,6 +65,10 @@ do_test(int trial, int64_t a, int64_t b)
 	test_a(+, ao_plus64, a, b, &ao_a, &ao_b);
 	test_b(+, ao_plus64, a, b, &ao_a, &ao_b);
 	test_x(+, ao_plus64, a, b, &ao_a, &ao_b);
+	test(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test_a(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test_b(-, ao_minus64, a, b, &ao_a, &ao_b);
+	test_x(-, ao_minus64, a, b, &ao_a, &ao_b);
 	test(*, ao_mul64_32_32,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b);
 	test(*, ao_mul64, a, b, &ao_a, &ao_b);
 	test_a(*, ao_mul64, a, b, &ao_a, &ao_b);
-- 
cgit v1.2.3


From 56911f27376b0fe91a464e369bb8aa1531b3c7dc Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Thu, 23 May 2013 02:17:51 -0600
Subject: altos: Make TeleMini v2.0 fit

Mash lots of storage locations and code around to shrink stuff down to size

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao.h                        |   2 +-
 src/core/ao_cmd.c                    |   4 +-
 src/core/ao_int64.c                  |  88 ++++++++++++------------
 src/core/ao_int64.h                  |  22 +++---
 src/core/ao_kalman.c                 |   4 +-
 src/core/ao_log_telem.c              |   2 +-
 src/core/ao_sample.h                 |   4 +-
 src/core/ao_task.h                   |   3 +
 src/drivers/ao_ms5607.c              |   4 +-
 src/drivers/ao_ms5607.h              |   7 +-
 src/drivers/ao_ms5607_convert_8051.c | 126 ++++++++++++++++++++++++++++-------
 src/telemini-v2.0/Makefile           |   9 ++-
 src/telemini-v2.0/ao_pins.h          |   2 +
 src/test/Makefile                    |   6 +-
 src/test/ao_int64_test.c             |   5 ++
 src/test/ao_ms5607_convert_test.c    |  96 ++++++++++++++++++++++++++
 16 files changed, 289 insertions(+), 95 deletions(-)
 create mode 100644 src/test/ao_ms5607_convert_test.c

(limited to 'src/test')

diff --git a/src/core/ao.h b/src/core/ao.h
index caa0ec19..e7320327 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -182,7 +182,7 @@ void
 ao_cmd_hex(void);
 
 void
-ao_cmd_decimal(void);
+ao_cmd_decimal(void) __reentrant;
 
 /* Read a single hex nibble off stdin. */
 uint8_t
diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c
index 5113548b..4ebaa607 100644
--- a/src/core/ao_cmd.c
+++ b/src/core/ao_cmd.c
@@ -206,9 +206,9 @@ ao_cmd_hex(void)
 }
 
 void
-ao_cmd_decimal(void)
+ao_cmd_decimal(void) __reentrant
 {
-	__pdata uint8_t	r = ao_cmd_lex_error;
+	uint8_t	r = ao_cmd_lex_error;
 
 	ao_cmd_lex_u32 = 0;
 	ao_cmd_white();
diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c
index 07cdb357..aa23dbe0 100644
--- a/src/core/ao_int64.c
+++ b/src/core/ao_int64.c
@@ -17,8 +17,10 @@
 
 #include <ao_int64.h>
 
-void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
-	uint32_t	t;
+__pdata ao_int64_t *__data ao_64r, *__data ao_64a, *__data ao_64b;
+
+void ao_plus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR {
+	__LOCAL uint32_t	t;
 
 	r->high = a->high + b->high;
 	t = a->low + b->low;
@@ -27,9 +29,8 @@ void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
 	r->low = t;
 }
 
-void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
-	uint32_t	t;
-
+void ao_minus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR {
+	__LOCAL uint32_t	t;
 
 	r->high = a->high - b->high;
 	t = a->low - b->low;
@@ -38,7 +39,7 @@ void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
 	r->low = t;
 }
 
-void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
+void ao_rshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR {
 	if (d < 32) {
 		r->low = a->low >> d;
 		if (d)
@@ -51,7 +52,7 @@ void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
 	}
 }
 
-void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
+void ao_lshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR {
 	if (d < 32) {
 		r->high = a->high << d;
 		if (d)
@@ -64,53 +65,49 @@ void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d) {
 	}
 }
 
-static void ao_umul64_32_32(ao_int64_t *r, uint32_t a, uint32_t b)
-{
-	uint32_t	r1;
-	uint32_t	r2, r3, r4;
-	ao_int64_t	s,t,u,v;
-	r1 = (uint32_t) (uint16_t) a * (uint16_t) b;
-	r2 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b;
-	r3 = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16);
-	r4 = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16);
-
-	s.low = r1;
-	s.high = r4;
-
-	t.high = r2 >> 16;
-	t.low = r2 << 16;
-	ao_plus64(&u, &s, &t);
-
-	v.high = r3 >> 16;
-	v.low = r3 << 16;
-	ao_plus64(r, &u, &v);
+static void ao_umul64_32_32(__ARG ao_int64_t *r, uint32_t a, uint32_t b) __reentrant {
+	__LOCAL uint32_t	s;
+	__LOCAL ao_int64_t	t;
+	r->low = (uint32_t) (uint16_t) a * (uint16_t) b;
+	r->high = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16);
+
+	s = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b;
+
+	t.high = s >> 16;
+	t.low = s << 16;
+	ao_plus64(r, r, &t);
+
+	s = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16);
+
+	t.high = s >> 16;
+	t.low = s << 16;
+	ao_plus64(r, r, &t);
 }
 
-void ao_neg64(ao_int64_t *r, ao_int64_t *a) {
+void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR {
 	r->high = ~a->high;
-	r->low = ~a->low;
-	if (!++r->low)
+	if (!(r->low = ~a->low + 1))
 		r->high++;
 }
 
-void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b) {
+void ao_mul64_32_32(__ARG ao_int64_t *r, int32_t a, int32_t b) __FATTR {
 	uint8_t		negative = 0;
 
 	if (a < 0) {
 		a = -a;
-		negative = ~0;
+		++negative;
 	}
 	if (b < 0) {
 		b = -b;
-		negative = ~negative;
+		--negative;
 	}
 	ao_umul64_32_32(r, a, b);
 	if (negative)
 		ao_neg64(r, r);
 }
 
-static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
-	ao_int64_t	r2, r3;
+static void ao_umul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __reentrant {
+	__LOCAL ao_int64_t	r2, r3;
 
 	ao_umul64_32_32(&r2, a->high, b->low);
 	ao_umul64_32_32(&r3, a->low, b->high);
@@ -119,38 +116,41 @@ static void ao_umul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
 	r->high += r2.low + r3.low;
 }
 
-void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b) {
+static __ARG ao_int64_t	ap, bp;
+
+void ao_mul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __FATTR {
 	uint8_t	negative = 0;
-	ao_int64_t	ap, bp;
 
 	if (ao_int64_negativep(a)) {
 		ao_neg64(&ap, a);
 		a = &ap;
-		negative = ~0;
+		++negative;
 	}
 	if (ao_int64_negativep(b)) {
 		ao_neg64(&bp, b);
 		b = &bp;
-		negative = ~negative;
+		--negative;
 	}
 	ao_umul64(r, a, b);
 	if (negative)
 		ao_neg64(r, r);
 }
 
-void ao_umul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
-	uint32_t h = a->high * b;
+static void ao_umul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, uint16_t b) __reentrant {
+	__LOCAL uint32_t h;
+
+	h = a->high * b;
 	ao_umul64_32_32(r, a->low, b);
 	r->high += h;
 }
 
-void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b) {
-	ao_int64_t	ap;
+void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR {
 	uint8_t		negative = 0;
+
 	if ((int32_t) a->high < 0) {
 		ao_neg64(&ap, a);
 		a = &ap;
-		negative = ~0;
+		negative++;
 	} else
 		ao_umul64_64_16(r, a, b);
 	if (negative)
diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h
index cf12f0d8..b16db58c 100644
--- a/src/core/ao_int64.h
+++ b/src/core/ao_int64.h
@@ -25,16 +25,18 @@ typedef struct {
 	uint32_t	low;
 } ao_int64_t;
 
-void ao_plus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
-void ao_minus64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
-void ao_neg64(ao_int64_t *r, ao_int64_t *a);
-void ao_lshift64_16(ao_int64_t *r, uint16_t a, uint8_t d);
-void ao_rshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
-void ao_lshift64(ao_int64_t *r, ao_int64_t *a, uint8_t d);
-void ao_mul64_64_64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
-void ao_mul64_32_32(ao_int64_t *r, int32_t a, int32_t b);
-void ao_mul64_64_16(ao_int64_t *r, ao_int64_t *a, uint16_t b);
-void ao_mul64(ao_int64_t *r, ao_int64_t *a, ao_int64_t *b);
+#define __FATTR
+#define __ARG __pdata
+#define __LOCAL static __pdata
+
+void ao_plus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR;
+void ao_minus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR;
+void ao_neg64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a) __FATTR;
+void ao_rshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR;
+void ao_lshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR;
+void ao_mul64_32_32(__ARG ao_int64_t *r, __ARG int32_t a, __ARG int32_t b) __FATTR;
+void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR;
+void ao_mul64(__ARG ao_int64_t * __ARG r, __ARG ao_int64_t * __ARG a, __ARG ao_int64_t *__ARG b) __FATTR;
 
 #define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a))
 #define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b))
diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c
index 59ffd8b2..762b2c0a 100644
--- a/src/core/ao_kalman.c
+++ b/src/core/ao_kalman.c
@@ -40,9 +40,9 @@ static __pdata int32_t		ao_k_accel;
 __pdata int16_t			ao_height;
 __pdata int16_t			ao_speed;
 __pdata int16_t			ao_accel;
-__pdata int16_t			ao_max_height;
+__xdata int16_t			ao_max_height;
 static __pdata int32_t		ao_avg_height_scaled;
-__pdata int16_t			ao_avg_height;
+__xdata int16_t			ao_avg_height;
 
 __pdata int16_t			ao_error_h;
 __pdata int16_t			ao_error_h_sq_avg;
diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c
index 23ebf7dd..095aca37 100644
--- a/src/core/ao_log_telem.c
+++ b/src/core/ao_log_telem.c
@@ -23,7 +23,7 @@ __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY;
 
 static __data uint8_t			ao_log_monitor_pos;
 __pdata enum ao_flight_state		ao_flight_state;
-__pdata int16_t				ao_max_height;	/* max of ao_height */
+__xdata int16_t				ao_max_height;	/* max of ao_height */
 __pdata int16_t				sense_d, sense_m;
 __pdata uint8_t				ao_igniter_present;
 
diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h
index a2dac979..5bd29536 100644
--- a/src/core/ao_sample.h
+++ b/src/core/ao_sample.h
@@ -136,8 +136,8 @@ uint8_t ao_sample(void);
 extern __pdata int16_t			ao_height;	/* meters */
 extern __pdata int16_t			ao_speed;	/* m/s * 16 */
 extern __pdata int16_t			ao_accel;	/* m/s² * 16 */
-extern __pdata int16_t			ao_max_height;	/* max of ao_height */
-extern __pdata int16_t			ao_avg_height;	/* running average of height */
+extern __xdata int16_t			ao_max_height;	/* max of ao_height */
+extern __xdata int16_t			ao_avg_height;	/* running average of height */
 
 extern __pdata int16_t			ao_error_h;
 extern __pdata int16_t			ao_error_h_sq_avg;
diff --git a/src/core/ao_task.h b/src/core/ao_task.h
index 1a4b5b6b..e3a311ed 100644
--- a/src/core/ao_task.h
+++ b/src/core/ao_task.h
@@ -45,7 +45,10 @@ struct ao_task {
 #endif
 };
 
+#ifndef AO_NUM_TASKS
 #define AO_NUM_TASKS		16	/* maximum number of tasks */
+#endif
+
 #define AO_NO_TASK		0	/* no task id */
 
 extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS];
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index 5259b265..4b4403a7 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -173,7 +173,7 @@ ao_ms5607_get_sample(uint8_t cmd) {
 #define AO_CONVERT_D2	token_evaluator(AO_MS5607_CONVERT_D2_, AO_MS5607_TEMP_OVERSAMPLE)
 
 void
-ao_ms5607_sample(struct ao_ms5607_sample *sample)
+ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample)
 {
 	sample->pres = ao_ms5607_get_sample(AO_CONVERT_D1);
 	sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2);
@@ -220,7 +220,7 @@ ao_ms5607_info(void)
 static void
 ao_ms5607_dump(void)
 {
-	struct ao_ms5607_value value;
+	__xdata struct ao_ms5607_value value;
 
 	ao_ms5607_convert(&ao_ms5607_current, &value);
 	printf ("Pressure:    %8u %8d\n", ao_ms5607_current.pres, value.pres);
diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h
index 3fd43fd4..206efd64 100644
--- a/src/drivers/ao_ms5607.h
+++ b/src/drivers/ao_ms5607.h
@@ -68,12 +68,13 @@ void
 ao_ms5607_info(void);
 
 void
-ao_ms5607_sample(struct ao_ms5607_sample *sample);
+ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample);
 
 void
-ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value);
+ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample,
+		  __xdata struct ao_ms5607_value *value);
 
 void
-ao_ms5607_get_prom(struct ao_ms5607_prom *prom);
+ao_ms5607_get_prom(__data struct ao_ms5607_prom *prom);
 
 #endif /* _AO_MS5607_H_ */
diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c
index f47972c9..f3a48c46 100644
--- a/src/drivers/ao_ms5607_convert_8051.c
+++ b/src/drivers/ao_ms5607_convert_8051.c
@@ -16,45 +16,121 @@
  */
 
 #include <ao_ms5607.h>
+#include <ao_int64.h>
+
+#if HAS_MS5611
+#define SHIFT_OFF	16
+#define SHIFT_TCO	7
+#define SHIFT_SENS	15
+#define SHFIT_TCS	8
+#else
+#define SHIFT_OFF	17
+#define SHIFT_TCO	6
+#define SHIFT_SENS	16
+#define SHIFT_TCS	7
+#endif
 
 void
-ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value)
+ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample,
+		  __xdata struct ao_ms5607_value *value)
 {
-#if 0
-	int32_t	dT;
-	int32_t TEMP;
-	int64_t OFF;
-	int64_t SENS;
+	__LOCAL int32_t	dT;
+	__LOCAL int32_t TEMP;
+	__LOCAL ao_int64_t OFF;
+	__LOCAL ao_int64_t SENS;
+	__LOCAL ao_int64_t a;
 
 	dT = sample->temp - ((int32_t) ms5607_prom.tref << 8);
 	
-	TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23);
+	/* TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); */
+	ao_mul64_32_32(&a, dT, ms5607_prom.tempsens);
+	ao_rshift64(&a, &a, 23);
+	TEMP = 2000 + a.low;
+	/* */
 
-#if HAS_MS5611
-	OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7);
-	SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8);
+	/* OFF = ((int64_t) ms5607_prom.off << SHIFT_OFF) + (((int64_t) ms5607_prom.tco * dT) >> SHIFT_TCO);*/
+#if SHIFT_OFF > 16
+	OFF.high = ms5607_prom.off >> (32 - SHIFT_OFF);
 #else
-	OFF = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6);
-	SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7);
+	OFF.high = 0;
 #endif
+	OFF.low = (uint32_t) ms5607_prom.off << SHIFT_OFF;
+	ao_mul64_32_32(&a, ms5607_prom.tco, dT);
+	ao_rshift64(&a, &a, SHIFT_TCO);
+	ao_plus64(&OFF, &OFF, &a);
+	/**/
+
+	/* SENS = ((int64_t) ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ms5607_prom.tcs * dT) >> SHIFT_TCS); */
+	SENS.high = 0;
+	SENS.low = (uint32_t) ms5607_prom.sens << SHIFT_SENS;
+	ao_mul64_32_32(&a, ms5607_prom.tcs, dT);
+	ao_rshift64(&a, &a, SHIFT_TCS);
+	ao_plus64(&SENS, &SENS, &a);
+	/**/
 
 	if (TEMP < 2000) {
-		int32_t	T2 = ((int64_t) dT * (int64_t) dT) >> 31;
-		int32_t TEMPM = TEMP - 2000;
-		int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4;
-		int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM;
-		if (TEMP < 1500) {
-			int32_t TEMPP = TEMP + 1500;
-			int64_t TEMPP2 = TEMPP * TEMPP;
-			OFF2 = OFF2 + 15 * TEMPP2;
-			SENS2 = SENS2 + 8 * TEMPP2;
+		__LOCAL int32_t	T2;
+		__LOCAL int32_t TEMPM;
+		__LOCAL ao_int64_t OFF2;
+		__LOCAL ao_int64_t SENS2;
+
+		/* T2 = ((int64_t) dT * (int64_t) dT) >> 31; */
+		ao_mul64_32_32(&a, dT, dT);
+		T2 = (a.low >> 31) | (a.high << 1);
+		/**/
+
+		TEMPM = TEMP - 2000;
+
+		/* OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; */
+		ao_mul64_32_32(&OFF2, TEMPM, TEMPM);
+		ao_mul64_64_16(&OFF2, &OFF2, 61);
+		ao_rshift64(&OFF2, &OFF2, 4);
+		/**/
+		
+		/* SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; */
+		ao_mul64_32_32(&SENS2, TEMPM, TEMPM);
+		ao_lshift64(&SENS2, &SENS2, 1);
+		/**/
+
+		if (TEMP < -1500) {
+			int32_t TEMPP;
+			int32_t TEMPP2;
+
+			TEMPP = TEMP + 1500;
+			TEMPP2 = TEMPP * TEMPP;
+
+			/* OFF2 = OFF2 + 15 * TEMPP2; */
+			ao_mul64_32_32(&a, 15, TEMPP2);
+			ao_plus64(&OFF2, &OFF2, &a);
+			/**/
+
+			/* SENS2 = SENS2 + 8 * TEMPP2; */
+			a.high = 0;
+			a.low = TEMPP2;
+			ao_lshift64(&a, &a, 3);
+			ao_plus64(&SENS2, &SENS2, &a);
+			/**/
 		}
 		TEMP -= T2;
-		OFF -= OFF2;
-		SENS -= SENS2;
+
+		/* OFF -= OFF2; */
+		ao_minus64(&OFF, &OFF, &OFF2);
+		/**/
+
+		/* SENS -= SENS2; */
+		ao_minus64(&SENS, &SENS, &SENS2);
+		/**/
 	}
 
-	value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15;
+	/* value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; */
+	a.high = 0;
+	a.low = sample->pres;
+	ao_mul64(&a, &a, &SENS);
+	ao_rshift64(&a, &a, 21);
+	ao_minus64(&a, &a, &OFF);
+	ao_rshift64(&a, &a, 15);
+	value->pres = a.low;
+	/**/
+	
 	value->temp = TEMP;
-#endif
 }
diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile
index afd529f0..984406a9 100644
--- a/src/telemini-v2.0/Makefile
+++ b/src/telemini-v2.0/Makefile
@@ -19,7 +19,11 @@ INC = \
 	ao_arch.h \
 	ao_arch_funcs.h \
 	cc1111.h \
-	ao_product.h
+	ao_ms5607.h \
+	ao_ms5607_convert_8051.c \
+	ao_product.h \
+	ao_int64.h \
+	ao_sample.h
 
 CORE_SRC = \
 	ao_cmd.c \
@@ -37,7 +41,8 @@ CORE_SRC = \
 	ao_storage.c \
 	ao_task.c \
 	ao_telemetry.c \
-	ao_freq.c
+	ao_freq.c \
+	ao_int64.c
 
 CC1111_SRC = \
 	ao_adc.c \
diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h
index 386c8dc3..fad029e2 100644
--- a/src/telemini-v2.0/ao_pins.h
+++ b/src/telemini-v2.0/ao_pins.h
@@ -115,6 +115,8 @@ struct ao_adc {
 #define AO_SENSE_DROGUE(p)	((p)->adc.sense_a)
 #define AO_SENSE_MAIN(p)	((p)->adc.sense_m)
 
+#define AO_NUM_TASKS	10
+
 #define AO_ADC_DUMP(p) \
 	printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \
 	       (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt)
diff --git a/src/test/Makefile b/src/test/Makefile
index 9c304318..5eee6bbb 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -2,7 +2,8 @@ vpath % ..:../core:../drivers:../util:../micropeak:../aes
 
 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_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \
-	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test
+	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \
+	ao_ms5607_convert_test
 
 INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
 
@@ -76,3 +77,6 @@ ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c
 
 ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h
 	cc $(CFLAGS) -o $@ ao_int64_test.c
+
+ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h
+	cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c
diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c
index c26a63b0..8557a1c7 100644
--- a/src/test/ao_int64_test.c
+++ b/src/test/ao_int64_test.c
@@ -15,6 +15,11 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+#define __data
+#define __pdata
+#define __xdata
+#define __reentrant
+
 #include <ao_int64.h>
 #include <ao_int64.c>
 #include <stdio.h>
diff --git a/src/test/ao_ms5607_convert_test.c b/src/test/ao_ms5607_convert_test.c
new file mode 100644
index 00000000..ad593204
--- /dev/null
+++ b/src/test/ao_ms5607_convert_test.c
@@ -0,0 +1,96 @@
+/*
+ * 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.
+ */
+
+#define __xdata
+#define __data
+#define __pdata
+#define __reentrant
+
+#include <stdint.h>
+#include <ao_ms5607.h>
+
+struct ao_ms5607_prom ms5607_prom = {
+	0x002c,
+	0xa6e0,
+	0x988e,
+	0x6814,
+	0x5eff,
+	0x8468,
+	0x6c86,
+	0xa271,
+};
+
+int32_t D1_mm = 6179630;
+int32_t D2_mm = 8933155;
+
+#include <ao_ms5607_convert.c>
+#define ao_ms5607_convert ao_ms5607_convert_8051
+#include <ao_ms5607_convert_8051.c>
+#include <ao_int64.c>
+#include <stdio.h>
+#include <stdlib.h>
+
+struct ao_ms5607_sample ao_sample = {
+	6179630,
+	8933155
+};
+
+int errors;
+
+void test(int trial, struct ao_ms5607_sample *sample)
+{
+	struct ao_ms5607_value	value, value_8051;
+
+	ao_ms5607_convert(sample, &value);
+	ao_ms5607_convert_8051(sample, &value_8051);
+	if (value.temp != value_8051.temp || value.pres != value_8051.pres) {
+		++errors;
+		printf ("trial %d: %d, %d -> %d, %d (should be %d, %d)\n",
+			trial,
+			sample->pres, sample->temp,
+			value_8051.pres, value_8051.temp,
+			value.pres, value.temp);
+	}
+}
+
+#define TESTS	10000000
+
+#include <stdlib.h>
+
+static int32_t rand24(void) { return random() & 0xffffff; }
+
+int
+main(int argc, char **argv)
+{
+	struct ao_ms5607_sample sample;
+	int	i, start;
+
+	if (argv[1])
+		start = atoi(argv[1]);
+	else
+		start = 0;
+
+	srandom(10000);
+	test(-1, &ao_sample);
+	for (i = 0; i < TESTS; i++) {
+		sample.pres = rand24();
+		sample.temp = rand24();
+		if (i >= start)
+			test(i, &sample);
+	}
+	return errors;
+}
-- 
cgit v1.2.3


From 039446f54ef6968a3f0b37ce32ca6bdcdbe62546 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Mon, 14 Oct 2013 22:41:43 -0700
Subject: altos: Merge GPS logging into a single function

Create a new global, ao_gps_new, which indicates new GPS position and
satellite data.

Use ao_gps_new as the new sleep/wakeup address.

Merge the separate gps position/satellite logging tasks into a single
function which waits for new data and writes out the changed values.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao.h                     |   4 ++
 src/core/ao_flight.c              |   4 +-
 src/core/ao_gps_report.c          | 102 ++++++++++++++++-------------------
 src/core/ao_gps_report_mega.c     |  88 +++++++++++++------------------
 src/core/ao_gps_report_metrum.c   | 108 +++++++++++++++++---------------------
 src/drivers/ao_gps_sirf.c         |   7 ++-
 src/drivers/ao_gps_skytraq.c      |   7 ++-
 src/drivers/ao_gps_ublox.c        |   5 +-
 src/product/ao_terraui.c          |   2 +-
 src/teleballoon-v1.1/ao_balloon.c |   4 +-
 src/test/ao_gps_test.c            |  13 +++--
 src/test/ao_gps_test_skytraq.c    |   2 +-
 src/test/ao_gps_test_ublox.c      |   2 +-
 13 files changed, 164 insertions(+), 184 deletions(-)

(limited to 'src/test')

diff --git a/src/core/ao.h b/src/core/ao.h
index e7320327..ea37885e 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -342,6 +342,10 @@ ao_spi_slave(void);
 #define AO_GPS_DATE_VALID	(1 << 6)
 #define AO_GPS_COURSE_VALID	(1 << 7)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
+extern __xdata uint8_t ao_gps_new;
 extern __pdata uint16_t ao_gps_tick;
 extern __xdata uint8_t ao_gps_mutex;
 extern __xdata struct ao_telemetry_location ao_gps_data;
diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 88dc816d..2495a44b 100644
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -194,8 +194,8 @@ ao_flight(void)
 
 #if HAS_GPS
 				/* Record current GPS position by waking up GPS log tasks */
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 #endif
 
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c
index c52ef621..8d15c083 100644
--- a/src/core/ao_gps_report.c
+++ b/src/core/ao_gps_report.c
@@ -22,78 +22,68 @@ ao_gps_report(void)
 {
 	static __xdata struct ao_log_record		gps_log;
 	static __xdata struct ao_telemetry_location	gps_data;
+	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
 	uint8_t	date_reported = 0;
+	uint8_t new;
 
 	for (;;) {
-		ao_sleep(&ao_gps_data);
 		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		while ((new = ao_gps_new) == 0)
+			ao_sleep(&ao_gps_new);
+		if (new & AO_GPS_NEW_DATA)
+			ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_TRACKING)
+			ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_gps_new = 0;
 		ao_mutex_put(&ao_gps_mutex);
 
-		if (!(gps_data.flags & AO_GPS_VALID))
-			continue;
-
-		gps_log.tick = ao_gps_tick;
-		gps_log.type = AO_LOG_GPS_TIME;
-		gps_log.u.gps_time.hour = gps_data.hour;
-		gps_log.u.gps_time.minute = gps_data.minute;
-		gps_log.u.gps_time.second = gps_data.second;
-		gps_log.u.gps_time.flags = gps_data.flags;
-		ao_log_data(&gps_log);
-		gps_log.type = AO_LOG_GPS_LAT;
-		gps_log.u.gps_latitude = gps_data.latitude;
-		ao_log_data(&gps_log);
-		gps_log.type = AO_LOG_GPS_LON;
-		gps_log.u.gps_longitude = gps_data.longitude;
-		ao_log_data(&gps_log);
-		gps_log.type = AO_LOG_GPS_ALT;
-		gps_log.u.gps_altitude.altitude = gps_data.altitude;
-		gps_log.u.gps_altitude.unused = 0xffff;
-		ao_log_data(&gps_log);
-		if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
-			gps_log.type = AO_LOG_GPS_DATE;
-			gps_log.u.gps_date.year = gps_data.year;
-			gps_log.u.gps_date.month = gps_data.month;
-			gps_log.u.gps_date.day = gps_data.day;
-			gps_log.u.gps_date.extra = 0;
-			date_reported = ao_log_data(&gps_log);
+		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_TIME;
+			gps_log.u.gps_time.hour = gps_data.hour;
+			gps_log.u.gps_time.minute = gps_data.minute;
+			gps_log.u.gps_time.second = gps_data.second;
+			gps_log.u.gps_time.flags = gps_data.flags;
+			ao_log_data(&gps_log);
+			gps_log.type = AO_LOG_GPS_LAT;
+			gps_log.u.gps_latitude = gps_data.latitude;
+			ao_log_data(&gps_log);
+			gps_log.type = AO_LOG_GPS_LON;
+			gps_log.u.gps_longitude = gps_data.longitude;
+			ao_log_data(&gps_log);
+			gps_log.type = AO_LOG_GPS_ALT;
+			gps_log.u.gps_altitude.altitude = gps_data.altitude;
+			gps_log.u.gps_altitude.unused = 0xffff;
+			ao_log_data(&gps_log);
+			if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) {
+				gps_log.type = AO_LOG_GPS_DATE;
+				gps_log.u.gps_date.year = gps_data.year;
+				gps_log.u.gps_date.month = gps_data.month;
+				gps_log.u.gps_date.day = gps_data.day;
+				gps_log.u.gps_date.extra = 0;
+				date_reported = ao_log_data(&gps_log);
+			}
 		}
-	}
-}
-
-void
-ao_gps_tracking_report(void)
-{
-	static __xdata struct ao_log_record		gps_log;
-	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
-	uint8_t	c, n;
-
-	for (;;) {
-		ao_sleep(&ao_gps_tracking_data);
-		ao_mutex_get(&ao_gps_mutex);
-		gps_log.tick = ao_gps_tick;
-		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
-		ao_mutex_put(&ao_gps_mutex);
+		if (new & AO_GPS_NEW_TRACKING) {
+			uint8_t c, n;
 
-		if (!(n = gps_tracking_data.channels))
-			continue;
-
-		gps_log.type = AO_LOG_GPS_SAT;
-		for (c = 0; c < n; c++)
-			if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid))
-			{
-				gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1;
-				ao_log_data(&gps_log);
+			if ((n = gps_tracking_data.channels) != 0) {
+				gps_log.type = AO_LOG_GPS_SAT;
+				for (c = 0; c < n; c++)
+					if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid))
+					{
+						gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1;
+						ao_log_data(&gps_log);
+					}
 			}
+		}
 	}
 }
 
 __xdata struct ao_task ao_gps_report_task;
-__xdata struct ao_task ao_gps_tracking_report_task;
 
 void
 ao_gps_report_init(void)
 {
 	ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report");
-	ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report");
 }
diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c
index e3af4307..e2adbfbc 100644
--- a/src/core/ao_gps_report_mega.c
+++ b/src/core/ao_gps_report_mega.c
@@ -23,66 +23,55 @@ ao_gps_report_mega(void)
 {
 	static __xdata struct ao_log_mega		gps_log;
 	static __xdata struct ao_telemetry_location	gps_data;
-	uint8_t	date_reported = 0;
-
-	for (;;) {
-		ao_sleep(&ao_gps_data);
-		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
-		ao_mutex_put(&ao_gps_mutex);
-
-		if (!(gps_data.flags & AO_GPS_VALID))
-			continue;
-
-		gps_log.tick = ao_gps_tick;
-		gps_log.type = AO_LOG_GPS_TIME;
-		gps_log.u.gps.latitude = gps_data.latitude;
-		gps_log.u.gps.longitude = gps_data.longitude;
-		gps_log.u.gps.altitude = gps_data.altitude;
-
-		gps_log.u.gps.hour = gps_data.hour;
-		gps_log.u.gps.minute = gps_data.minute;
-		gps_log.u.gps.second = gps_data.second;
-		gps_log.u.gps.flags = gps_data.flags;
-		gps_log.u.gps.year = gps_data.year;
-		gps_log.u.gps.month = gps_data.month;
-		gps_log.u.gps.day = gps_data.day;
-		ao_log_mega(&gps_log);
-	}
-}
-
-void
-ao_gps_tracking_report_mega(void)
-{
-	static __xdata struct ao_log_mega		gps_log;
 	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
+	uint8_t	date_reported = 0;
+	uint8_t	new;
 	uint8_t	c, n, i;
 
 	for (;;) {
-		ao_sleep(&ao_gps_tracking_data);
 		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		while (!(new = ao_gps_new))
+			ao_sleep(&ao_gps_new);
+		if (new & AO_GPS_NEW_DATA)
+			ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_TRACKING)
+			ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_gps_new = 0;
 		ao_mutex_put(&ao_gps_mutex);
 
-		if (!(n = gps_tracking_data.channels))
-			continue;
+		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_TIME;
+			gps_log.u.gps.latitude = gps_data.latitude;
+			gps_log.u.gps.longitude = gps_data.longitude;
+			gps_log.u.gps.altitude = gps_data.altitude;
 
-		gps_log.type = AO_LOG_GPS_SAT;
-		gps_log.tick = ao_gps_tick;
-		i = 0;
-		for (c = 0; c < n; c++)
-			if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid))
-			{
-				gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
-				i++;
-			}
-		gps_log.u.gps_sat.channels = i;
-		ao_log_mega(&gps_log);
+			gps_log.u.gps.hour = gps_data.hour;
+			gps_log.u.gps.minute = gps_data.minute;
+			gps_log.u.gps.second = gps_data.second;
+			gps_log.u.gps.flags = gps_data.flags;
+			gps_log.u.gps.year = gps_data.year;
+			gps_log.u.gps.month = gps_data.month;
+			gps_log.u.gps.day = gps_data.day;
+			ao_log_mega(&gps_log);
+		}
+		if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_SAT;
+			i = 0;
+			for (c = 0; c < n; c++)
+				if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid))
+				{
+					gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
+					i++;
+				}
+			gps_log.u.gps_sat.channels = i;
+			ao_log_mega(&gps_log);
+		}
 	}
 }
 
 __xdata struct ao_task ao_gps_report_mega_task;
-__xdata struct ao_task ao_gps_tracking_report_mega_task;
 
 void
 ao_gps_report_mega_init(void)
@@ -90,7 +79,4 @@ ao_gps_report_mega_init(void)
 	ao_add_task(&ao_gps_report_mega_task,
 		    ao_gps_report_mega,
 		    "gps_report");
-	ao_add_task(&ao_gps_tracking_report_mega_task,
-		    ao_gps_tracking_report_mega,
-		    "gps_tracking_report");
 }
diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c
index 4fefe223..b82936dd 100644
--- a/src/core/ao_gps_report_metrum.c
+++ b/src/core/ao_gps_report_metrum.c
@@ -23,80 +23,69 @@ ao_gps_report_metrum(void)
 {
 	static __xdata struct ao_log_metrum		gps_log;
 	static __xdata struct ao_telemetry_location	gps_data;
-	uint8_t	date_reported = 0;
-
-	for (;;) {
-		ao_sleep(&ao_gps_data);
-		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
-		ao_mutex_put(&ao_gps_mutex);
-
-		if (!(gps_data.flags & AO_GPS_VALID))
-			continue;
-
-		gps_log.tick = ao_gps_tick;
-		gps_log.type = AO_LOG_GPS_POS;
-		gps_log.u.gps.latitude = gps_data.latitude;
-		gps_log.u.gps.longitude = gps_data.longitude;
-		gps_log.u.gps.altitude = gps_data.altitude;
-		ao_log_metrum(&gps_log);
-
-		gps_log.type = AO_LOG_GPS_TIME;
-		gps_log.u.gps_time.hour = gps_data.hour;
-		gps_log.u.gps_time.minute = gps_data.minute;
-		gps_log.u.gps_time.second = gps_data.second;
-		gps_log.u.gps_time.flags = gps_data.flags;
-		gps_log.u.gps_time.year = gps_data.year;
-		gps_log.u.gps_time.month = gps_data.month;
-		gps_log.u.gps_time.day = gps_data.day;
-		ao_log_metrum(&gps_log);
-	}
-}
-
-void
-ao_gps_tracking_report_metrum(void)
-{
-	static __xdata struct ao_log_metrum		gps_log;
 	static __xdata struct ao_telemetry_satellite	gps_tracking_data;
 	uint8_t	c, n, i, p, valid, packets;
 	uint8_t svid;
+	uint8_t	date_reported = 0;
 
 	for (;;) {
-		ao_sleep(&ao_gps_tracking_data);
 		ao_mutex_get(&ao_gps_mutex);
-		ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		while (!(new = ao_gps_new))
+			ao_sleep(&ao_gps_new);
+		if (new & AO_GPS_NEW_DATA)
+			ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
+		if (new & AO_GPS_NEW_TRACKING)
+			ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
+		ao_gps_new = 0;
 		ao_mutex_put(&ao_gps_mutex);
 
-		if (!(n = gps_tracking_data.channels))
-			continue;
+		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+			gps_log.tick = ao_gps_tick;
+			gps_log.type = AO_LOG_GPS_POS;
+			gps_log.u.gps.latitude = gps_data.latitude;
+			gps_log.u.gps.longitude = gps_data.longitude;
+			gps_log.u.gps.altitude = gps_data.altitude;
+			ao_log_metrum(&gps_log);
 
-		gps_log.type = AO_LOG_GPS_SAT;
-		gps_log.tick = ao_gps_tick;
-		i = 0;
-		for (c = 0; c < n; c++) {
-			svid = gps_tracking_data.sats[c].svid;
-			if (svid != 0) {
-				if (i == 4) {
-					gps_log.u.gps_sat.channels = i;
-					gps_log.u.gps_sat.more = 1;
-					ao_log_metrum(&gps_log);
-					i = 0;
+			gps_log.type = AO_LOG_GPS_TIME;
+			gps_log.u.gps_time.hour = gps_data.hour;
+			gps_log.u.gps_time.minute = gps_data.minute;
+			gps_log.u.gps_time.second = gps_data.second;
+			gps_log.u.gps_time.flags = gps_data.flags;
+			gps_log.u.gps_time.year = gps_data.year;
+			gps_log.u.gps_time.month = gps_data.month;
+			gps_log.u.gps_time.day = gps_data.day;
+			ao_log_metrum(&gps_log);
+		}
+
+		if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels)) {
+			gps_log.type = AO_LOG_GPS_SAT;
+			gps_log.tick = ao_gps_tick;
+			i = 0;
+			for (c = 0; c < n; c++) {
+				svid = gps_tracking_data.sats[c].svid;
+				if (svid != 0) {
+					if (i == 4) {
+						gps_log.u.gps_sat.channels = i;
+						gps_log.u.gps_sat.more = 1;
+						ao_log_metrum(&gps_log);
+						i = 0;
+					}
+					gps_log.u.gps_sat.sats[i].svid = svid;
+					gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
+					i++;
 				}
-				gps_log.u.gps_sat.sats[i].svid = svid;
-				gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1;
-				i++;
 			}
-		}
-		if (i) {
-			gps_log.u.gps_sat.channels = i;
-			gps_log.u.gps_sat.more = 0;
-			ao_log_metrum(&gps_log);
+			if (i) {
+				gps_log.u.gps_sat.channels = i;
+				gps_log.u.gps_sat.more = 0;
+				ao_log_metrum(&gps_log);
+			}
 		}
 	}
 }
 
 __xdata struct ao_task ao_gps_report_metrum_task;
-__xdata struct ao_task ao_gps_tracking_report_metrum_task;
 
 void
 ao_gps_report_metrum_init(void)
@@ -104,7 +93,4 @@ ao_gps_report_metrum_init(void)
 	ao_add_task(&ao_gps_report_metrum_task,
 		    ao_gps_report_metrum,
 		    "gps_report");
-	ao_add_task(&ao_gps_tracking_report_metrum_task,
-		    ao_gps_tracking_report_metrum,
-		    "gps_tracking_report");
 }
diff --git a/src/drivers/ao_gps_sirf.c b/src/drivers/ao_gps_sirf.c
index 91fc948b..d89435b9 100644
--- a/src/drivers/ao_gps_sirf.c
+++ b/src/drivers/ao_gps_sirf.c
@@ -19,6 +19,7 @@
 #include "ao.h"
 #endif
 
+__xdata uint8_t ao_gps_new;
 __xdata uint8_t ao_gps_mutex;
 __pdata uint16_t ao_gps_tick;
 __xdata struct ao_telemetry_location	ao_gps_data;
@@ -422,8 +423,9 @@ ao_gps(void) __reentrant
 			else
 				ao_gps_data.v_error = ao_sirf_data.v_error / 100;
 #endif
+			ao_gps_new |= AO_GPS_NEW_DATA;
 			ao_mutex_put(&ao_gps_mutex);
-			ao_wakeup(&ao_gps_data);
+			ao_wakeup(&ao_gps_new);
 			break;
 		case 4:
 			ao_mutex_get(&ao_gps_mutex);
@@ -432,8 +434,9 @@ ao_gps(void) __reentrant
 				ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid;
 				ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1;
 			}
+			ao_gps_new |= AO_GPS_NEW_TRACKING;
 			ao_mutex_put(&ao_gps_mutex);
-			ao_wakeup(&ao_gps_tracking_data);
+			ao_wakeup(&ao_gps_new);
 			break;
 		}
 	}
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index 9a9dff75..944a37f9 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -32,6 +32,7 @@
 #define ao_gps_set_speed	ao_serial1_set_speed
 #endif
 
+__xdata uint8_t ao_gps_new;
 __xdata uint8_t ao_gps_mutex;
 static __data char ao_gps_char;
 static __data uint8_t ao_gps_cksum;
@@ -293,10 +294,11 @@ ao_nmea_gga(void)
 
 	if (!ao_gps_error) {
 		ao_mutex_get(&ao_gps_mutex);
+		ao_gps_new |= AO_GPS_NEW_DATA;
 		ao_gps_tick = ao_gps_next_tick;
 		ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
 		ao_mutex_put(&ao_gps_mutex);
-		ao_wakeup(&ao_gps_data);
+		ao_wakeup(&ao_gps_new);
 	}
 }
 
@@ -352,9 +354,10 @@ ao_nmea_gsv(void)
 		ao_gps_tracking_next.channels = 0;
 	else if (done) {
 		ao_mutex_get(&ao_gps_mutex);
+		ao_gps_new |= AO_GPS_NEW_TRACKING;
 		ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
 		ao_mutex_put(&ao_gps_mutex);
-		ao_wakeup(&ao_gps_tracking_data);
+		ao_wakeup(&ao_gps_new);
 	}
 }
 
diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index e9168348..3582d6e0 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -25,6 +25,7 @@
 
 #include <stdarg.h>
 
+__xdata uint8_t ao_gps_new;
 __xdata uint8_t ao_gps_mutex;
 __pdata uint16_t ao_gps_tick;
 __xdata struct ao_telemetry_location	ao_gps_data;
@@ -760,8 +761,8 @@ ao_gps(void) __reentrant
 				}
 
 				ao_mutex_put(&ao_gps_mutex);
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 				break;
 			}
 			break;
diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c
index 1866eb0c..8fd97033 100644
--- a/src/product/ao_terraui.c
+++ b/src/product/ao_terraui.c
@@ -629,7 +629,7 @@ ao_terragps(void)
 
 	for (;;) {
 		while (ao_gps_tick == gps_tick)
-			ao_sleep(&ao_gps_data);
+			ao_sleep(&ao_gps_new);
 		gps_tick = ao_gps_tick;
 		ao_gps_progress = (ao_gps_progress + 1) & 3;
 	}
diff --git a/src/teleballoon-v1.1/ao_balloon.c b/src/teleballoon-v1.1/ao_balloon.c
index e8972655..12752d1f 100644
--- a/src/teleballoon-v1.1/ao_balloon.c
+++ b/src/teleballoon-v1.1/ao_balloon.c
@@ -105,8 +105,8 @@ ao_flight(void)
 
 #if HAS_GPS
 				/* Record current GPS position by waking up GPS log tasks */
-				ao_wakeup(&ao_gps_data);
-				ao_wakeup(&ao_gps_tracking_data);
+				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
+				ao_wakeup(&ao_gps_new);
 #endif
 
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c
index 3844a326..b6cc9ba7 100644
--- a/src/test/ao_gps_test.c
+++ b/src/test/ao_gps_test.c
@@ -427,11 +427,18 @@ void
 ao_dump_state(void *wchan)
 {
 	int	i;
-	if (wchan == &ao_gps_data)
+
+	if (wchan != &ao_gps_new)
+		return;
+	
+	if (ao_gps_new & AO_GPS_NEW_DATA) {
 		ao_gps_print(&ao_gps_data);
-	else
+		putchar('\n');
+	}
+	if (ao_gps_new & AO_GPS_NEW_TRACKING) {
 		ao_gps_tracking_print(&ao_gps_tracking_data);
-	putchar('\n');
+		putchar('\n');
+	}
 	return;
 	printf ("%02d:%02d:%02d",
 		ao_gps_data.hour, ao_gps_data.minute,
diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c
index 89cbd767..bf2ab5b8 100644
--- a/src/test/ao_gps_test_skytraq.c
+++ b/src/test/ao_gps_test_skytraq.c
@@ -443,7 +443,7 @@ uint8_t	ao_task_minimize_latency;
 void
 ao_dump_state(void *wchan)
 {
-	if (wchan == &ao_gps_data)
+	if (wchan == &ao_gps_new)
 		ao_gps_show();
 }
 
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
index a0e04cb6..31c7af60 100644
--- a/src/test/ao_gps_test_ublox.c
+++ b/src/test/ao_gps_test_ublox.c
@@ -347,7 +347,7 @@ check_ublox_message(char *which, uint8_t *msg)
 void
 ao_dump_state(void *wchan)
 {
-	if (wchan == &ao_gps_data)
+	if (wchan == &ao_gps_new)
 		ao_gps_show();
 	return;
 }
-- 
cgit v1.2.3


From ba99630f33440b993c69830856d2a7741ffdef71 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Fri, 25 Oct 2013 04:03:39 -0700
Subject: altos: Fix GPS test frameworks to handle shared ao_gps_new variable

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/drivers/ao_gps_ublox.c     | 2 +-
 src/test/ao_gps_test.c         | 3 +++
 src/test/ao_gps_test_skytraq.c | 3 +++
 src/test/ao_gps_test_ublox.c   | 3 +++
 4 files changed, 10 insertions(+), 1 deletion(-)

(limited to 'src/test')

diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c
index 3582d6e0..4fb90746 100644
--- a/src/drivers/ao_gps_ublox.c
+++ b/src/drivers/ao_gps_ublox.c
@@ -21,7 +21,7 @@
 
 #include "ao_gps_ublox.h"
 
-#define AO_UBLOX_DEBUG 1
+#define AO_UBLOX_DEBUG 0
 
 #include <stdarg.h>
 
diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c
index b6cc9ba7..e799ab0f 100644
--- a/src/test/ao_gps_test.c
+++ b/src/test/ao_gps_test.c
@@ -26,6 +26,9 @@
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
 #define AO_GPS_NUM_SAT_SHIFT	(0)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
 #define AO_GPS_VALID		(1 << 4)
 #define AO_GPS_RUNNING		(1 << 5)
 #define AO_GPS_DATE_VALID	(1 << 6)
diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c
index bf2ab5b8..1b590d5e 100644
--- a/src/test/ao_gps_test_skytraq.c
+++ b/src/test/ao_gps_test_skytraq.c
@@ -26,6 +26,9 @@
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
 #define AO_GPS_NUM_SAT_SHIFT	(0)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
 #define AO_GPS_VALID		(1 << 4)
 #define AO_GPS_RUNNING		(1 << 5)
 #define AO_GPS_DATE_VALID	(1 << 6)
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
index 31c7af60..4eb4b837 100644
--- a/src/test/ao_gps_test_ublox.c
+++ b/src/test/ao_gps_test_ublox.c
@@ -26,6 +26,9 @@
 #define AO_GPS_NUM_SAT_MASK	(0xf << 0)
 #define AO_GPS_NUM_SAT_SHIFT	(0)
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
 #define AO_GPS_VALID		(1 << 4)
 #define AO_GPS_RUNNING		(1 << 5)
 #define AO_GPS_DATE_VALID	(1 << 6)
-- 
cgit v1.2.3


From 08143a922fe27bc50a19924f46538f9476ab5fd1 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Fri, 25 Oct 2013 04:05:09 -0700
Subject: altos: Add gyro-based orientation tracking

This tracks the angle-from-vertical as an additional input to the pyro
channels.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_data.h            |   6 +-
 src/core/ao_flight.c          |   4 +
 src/core/ao_kalman.c          |   3 -
 src/core/ao_pyro.c            |   4 +-
 src/core/ao_quaternion.h      | 116 +++++++++++++++++++
 src/core/ao_sample.c          |  98 +++++++++++++++-
 src/core/ao_sample.h          |   3 +-
 src/core/ao_telemetry.c       |   1 +
 src/core/ao_telemetry.h       |   2 +-
 src/drivers/ao_mpu6000.c      |   2 +
 src/drivers/ao_mpu6000.h      |  17 ++-
 src/stm/Makefile.defs         |   4 +-
 src/telemega-v0.3/Makefile    |  16 +++
 src/test/Makefile             |   9 +-
 src/test/ao_flight_test.c     | 259 ++----------------------------------------
 src/test/ao_quaternion_test.c |  67 +++++++++++
 16 files changed, 343 insertions(+), 268 deletions(-)
 create mode 100644 src/core/ao_quaternion.h
 create mode 100644 src/test/ao_quaternion_test.c

(limited to 'src/test')

diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 339afe69..5a232885 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -18,6 +18,8 @@
 #ifndef _AO_DATA_H_
 #define _AO_DATA_H_
 
+#define GRAVITY 9.80665
+
 #if HAS_ADC
 #define AO_DATA_ADC	(1 << 0)
 #else
@@ -300,8 +302,8 @@ typedef int16_t accel_t;
 
 #define HAS_GYRO	1
 
-typedef int16_t	gyro_t;
-typedef int32_t angle_t;
+typedef int16_t	gyro_t;		/* in raw sample units */
+typedef int16_t angle_t;	/* in degrees */
 
 /* Y axis is aligned with the direction of motion (along) */
 /* X axis is aligned in the other board axis (across) */
diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 2495a44b..240b348a 100644
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -20,6 +20,10 @@
 #include <ao_log.h>
 #endif
 
+#if HAS_MPU6000
+#include <ao_quaternion.h>
+#endif
+
 #ifndef HAS_ACCEL
 #error Please define HAS_ACCEL
 #endif
diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c
index 762b2c0a..7fd4f889 100644
--- a/src/core/ao_kalman.c
+++ b/src/core/ao_kalman.c
@@ -292,7 +292,4 @@ ao_kalman(void)
 	else 
 #endif
 		ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
-#ifdef AO_FLIGHT_TEST
-	ao_sample_prev_tick = ao_sample_tick;
-#endif
 }
diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c
index 531e1f50..24c9fe99 100644
--- a/src/core/ao_pyro.c
+++ b/src/core/ao_pyro.c
@@ -113,7 +113,7 @@ ao_pyro_ready(struct ao_pyro *pyro)
 				continue;
 			break;
 
-#if HAS_ORIENT
+#if HAS_GYRO
 		case ao_pyro_orient_less:
 			if (ao_orient <= pyro->orient_less)
 				continue;
@@ -310,7 +310,7 @@ const struct {
 	{ "h<",	ao_pyro_height_less,	offsetof(struct ao_pyro, height_less), HELP("height less (m)") },
 	{ "h>",	ao_pyro_height_greater,	offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") },
 
-#if HAS_ORIENT
+#if HAS_GYRO
 	{ "o<",	ao_pyro_orient_less,	offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") },
 	{ "o>",	ao_pyro_orient_greater,	offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)")  },
 #endif
diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h
new file mode 100644
index 00000000..f4b8aaa4
--- /dev/null
+++ b/src/core/ao_quaternion.h
@@ -0,0 +1,116 @@
+/*
+ * 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_QUATERNION_H_
+#define _AO_QUATERNION_H_
+
+#include <math.h>
+
+struct ao_quaternion {
+	float	r;		/* real bit */
+	float	x, y, z;	/* imaginary bits */
+};
+
+static inline void ao_quaternion_multiply(struct ao_quaternion *r,
+					  struct ao_quaternion *a,
+					  struct ao_quaternion *b)
+{
+	struct ao_quaternion	t;
+#define T(_a,_b)	(((a)->_a) * ((b)->_b))
+	t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z);
+	t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y);
+	t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x);
+	t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r);
+#undef T
+	*r = t;
+}
+
+static inline void ao_quaternion_conjugate(struct ao_quaternion *r,
+					   struct ao_quaternion *a)
+{
+	r->r = a->r;
+	r->x = -a->x;
+	r->y = -a->y;
+	r->z = -a->z;
+}
+
+static inline float ao_quaternion_normal(struct ao_quaternion *a)
+{
+#define S(_a)	(((a)->_a) * ((a)->_a))
+	return S(r) + S(x) + S(y) + S(z);
+#undef S
+}
+
+static inline void ao_quaternion_scale(struct ao_quaternion *r,
+				       struct ao_quaternion *a,
+				       float b)
+{
+	r->r = a->r * b;
+	r->x = a->x * b;
+	r->y = a->y * b;
+	r->z = a->z * b;
+}
+
+static inline void ao_quaternion_normalize(struct ao_quaternion *r,
+					   struct ao_quaternion *a)
+{
+	float	n = ao_quaternion_normal(a);
+
+	if (n > 0)
+		ao_quaternion_scale(r, a, 1/sqrtf(n));
+	else
+		*r = *a;
+}
+
+static inline void ao_quaternion_rotate(struct ao_quaternion *r,
+					struct ao_quaternion *a,
+					struct ao_quaternion *b)
+{
+	struct ao_quaternion	c;
+	struct ao_quaternion	t;
+
+	ao_quaternion_conjugate(&c, b);
+	ao_quaternion_multiply(&t, b, a);
+	ao_quaternion_multiply(r, &t, &c);
+}
+
+static inline void ao_quaternion_init_vector(struct ao_quaternion *r,
+					     float x, float y, float z)
+{
+	r->r = 0;
+	r->x = x;
+	r->y = y;
+	r->z = z;
+}
+
+static inline void ao_quaternion_init_rotation(struct ao_quaternion *r,
+					       float x, float y, float z,
+					       float s, float c)
+{
+	r->r = c;
+	r->x = s * x;
+	r->y = s * y;
+	r->z = s * z;
+}
+
+static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r)
+{
+	r->r = 1;
+	r->x = r->y = r->z = 0;
+}
+
+#endif /* _AO_QUATERNION_H_ */
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index dec44f9f..676e0ffd 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -20,6 +20,10 @@
 #include <ao_data.h>
 #endif
 
+#if HAS_GYRO
+#include <ao_quaternion.h>
+#endif
+
 /*
  * Current sensor values
  */
@@ -44,8 +48,7 @@ __pdata accel_t		ao_sample_accel_through;
 __pdata gyro_t		ao_sample_roll;
 __pdata gyro_t		ao_sample_pitch;
 __pdata gyro_t		ao_sample_yaw;
-__pdata angle_t		ao_sample_angle;
-__pdata angle_t		ao_sample_roll_angle;
+__pdata angle_t		ao_orient;
 #endif
 
 __data uint8_t		ao_sample_data;
@@ -86,6 +89,8 @@ __pdata int32_t	ao_sample_accel_through_sum;
 __pdata int32_t ao_sample_pitch_sum;
 __pdata int32_t ao_sample_yaw_sum;
 __pdata int32_t	ao_sample_roll_sum;
+static struct ao_quaternion ao_rotation;
+static struct ao_quaternion ao_pad_orientation;
 #endif
 
 static void
@@ -129,11 +134,91 @@ ao_sample_preflight_set(void)
 	ao_sample_pitch_sum = 0;
 	ao_sample_yaw_sum = 0;
 	ao_sample_roll_sum = 0;
-	ao_sample_angle = 0;
+	ao_orient = 0;
+
+	/* No rotation yet */
+	ao_quaternion_init_zero_rotation(&ao_rotation);
+
+	/* XXX Assume we're pointing straight up for now */
+	ao_quaternion_init_vector(&ao_pad_orientation,
+				  ao_ground_accel_across,
+				  ao_ground_accel_through,
+				  -ao_ground_accel_along);
+	ao_quaternion_normalize(&ao_pad_orientation,
+				&ao_pad_orientation);
+				  
+	printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n",
+		ao_pad_orientation.r,
+		ao_pad_orientation.x,
+		ao_pad_orientation.y,
+		ao_pad_orientation.z);
 #endif	
 	nsamples = 0;
 }
 
+#if HAS_GYRO
+static void
+ao_sample_rotate(void)
+{
+#ifdef AO_FLIGHT_TEST
+	float	dt = (ao_sample_tick - ao_sample_prev_tick) / 100.0;
+#else
+	static const float dt = 1/100.0;
+#endif
+	float	x = ao_mpu6000_gyro(ao_sample_pitch - ao_ground_pitch) * dt;
+	float	y = ao_mpu6000_gyro(ao_sample_yaw - ao_ground_yaw) * dt;
+	float	z = ao_mpu6000_gyro(ao_sample_roll - ao_ground_roll) * dt;
+
+	float			n_2, n;
+	float			s, c;
+	
+	struct ao_quaternion	rot;
+	struct ao_quaternion	point;
+
+	/* The amount of rotation is just the length of the vector. Now,
+	 * here's the trick -- assume that the rotation amount is small. In this case,
+	 * sin(x) ≃ x, so we can just make this the sin.
+	 */
+
+	n_2 = x*x + y*y + z*z;
+	n = sqrtf(n_2);
+	s = n / 2;
+	if (s > 1)
+		s = 1;
+	c = sqrtf(1 - s*s);
+
+	/* Make unit vector */
+	if (n > 0) {
+		x /= n;
+		y /= n;
+		z /= n;
+	}
+
+	/* Now compute the unified rotation quaternion */
+
+	ao_quaternion_init_rotation(&rot,
+				    x, y, z,
+				    s, c);
+
+	/* Integrate with the previous rotation amount */
+	ao_quaternion_multiply(&ao_rotation, &ao_rotation, &rot);
+
+	/* And normalize to make sure it remains a unit vector */
+	ao_quaternion_normalize(&ao_rotation, &ao_rotation);
+
+	/* Compute pitch angle from vertical by taking the pad
+	 * orientation vector and rotating it by the current total
+	 * rotation value. That will be a unit vector pointing along
+	 * the airframe axis. The Z value will be the cosine of the
+	 * change in the angle from vertical since boost
+	 */
+
+	ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation);
+
+	ao_orient = acosf(point.z) * (float) (180.0/M_PI);
+}
+#endif
+
 static void
 ao_sample_preflight(void)
 {
@@ -232,9 +317,12 @@ ao_sample(void)
 				ao_sample_preflight_update();
 			ao_kalman();
 #if HAS_GYRO
-			/* do quaternion stuff here... */
+			ao_sample_rotate();
 #endif
 		}
+#ifdef AO_FLIGHT_TEST
+		ao_sample_prev_tick = ao_sample_tick;
+#endif
 		ao_sample_data = ao_data_ring_next(ao_sample_data);
 	}
 	return !ao_preflight;
@@ -264,7 +352,7 @@ ao_sample_init(void)
 	ao_sample_pitch = 0;
 	ao_sample_yaw = 0;
 	ao_sample_roll = 0;
-	ao_sample_angle = 0;
+	ao_orient = 0;
 #endif
 	ao_sample_data = ao_data_head;
 	ao_preflight = TRUE;
diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h
index 5bd29536..1d1bcc7c 100644
--- a/src/core/ao_sample.h
+++ b/src/core/ao_sample.h
@@ -64,8 +64,6 @@
  * for all further flight computations
  */
 
-#define GRAVITY 9.80665
-
 /*
  * Above this height, the baro sensor doesn't work
  */
@@ -118,6 +116,7 @@ extern __pdata accel_t	ao_ground_accel_through;
 extern __pdata gyro_t	ao_ground_pitch;
 extern __pdata gyro_t	ao_ground_yaw;
 extern __pdata gyro_t	ao_ground_roll;
+extern __pdata angle_t	ao_orient;
 #endif
 
 void ao_sample_init(void);
diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index 6b47a06a..a2726016 100644
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -115,6 +115,7 @@ ao_send_mega_sensor(void)
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR;
 
+	telemetry.mega_sensor.orient = ao_orient;
 	telemetry.mega_sensor.accel = ao_data_accel(packet);
 	telemetry.mega_sensor.pres = ao_data_pres(packet);
 	telemetry.mega_sensor.temp = ao_data_temp(packet);
diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h
index fd6b76b3..237a35ab 100644
--- a/src/core/ao_telemetry.h
+++ b/src/core/ao_telemetry.h
@@ -162,7 +162,7 @@ struct ao_telemetry_mega_sensor {
 	uint16_t	tick;		/*  2 */
 	uint8_t		type;		/*  4 */
 
-	uint8_t		pad5;		/*  5 */
+	uint8_t		orient;		/*  5 angle from vertical */
 	int16_t		accel;		/*  6 Z axis */
 
 	int32_t		pres;		/*  8 Pa * 10 */
diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 73057820..5e75b78a 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -118,6 +118,7 @@ _ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
 
 #define G	981	/* in cm/s² */
 
+#if 0
 static int16_t /* cm/s² */
 ao_mpu6000_accel(int16_t v)
 {
@@ -129,6 +130,7 @@ ao_mpu6000_gyro(int16_t v)
 {
 	return (int16_t) ((v * (int32_t) 20000) / 32767);
 }
+#endif
 
 static uint8_t
 ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index a42f69c4..2241bf80 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -18,6 +18,10 @@
 #ifndef _AO_MPU6000_H_
 #define _AO_MPU6000_H_
 
+#ifndef M_PI
+#define M_PI 3.1415926535897832384626433
+#endif
+
 #define MPU6000_ADDR_WRITE	0xd0
 #define MPU6000_ADDR_READ	0xd1
 
@@ -166,9 +170,20 @@
 /* Self test gyro is approximately 50°/s */
 #define MPU6000_ST_GYRO(full_scale)	((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale)))
 
-#define MPU6000_GYRO_FULLSCALE	2000
+#define MPU6000_GYRO_FULLSCALE	((float) 2000 * M_PI/180.0)
+
+static inline float
+ao_mpu6000_gyro(int16_t sensor) {
+	return (float) sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0));
+}
+
 #define MPU6000_ACCEL_FULLSCALE	16
 
+static inline float
+ao_mpu6000_accel(int16_t sensor) {
+	return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0));
+}
+
 struct ao_mpu6000_sample {
 	int16_t		accel_x;
 	int16_t		accel_y;
diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs
index ee1cb658..0710d747 100644
--- a/src/stm/Makefile.defs
+++ b/src/stm/Makefile.defs
@@ -1,4 +1,4 @@
-vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:..
+vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:..
 vpath make-altitude ../util
 vpath make-kalman ../util
 vpath kalman.5c ../kalman
@@ -24,7 +24,7 @@ include $(TOPDIR)/Makedefs
 CC=$(ARM_CC)
 LIBS=-lpdclib-cortex-m3 -lgcc
 
-AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I..
+AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I..
 STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS)
 
 LDFLAGS=-L../stm -Wl,-Taltos.ld
diff --git a/src/telemega-v0.3/Makefile b/src/telemega-v0.3/Makefile
index 6e5da721..2f460105 100644
--- a/src/telemega-v0.3/Makefile
+++ b/src/telemega-v0.3/Makefile
@@ -27,6 +27,7 @@ INC = \
 	ao_sample_profile.h \
 	ao_mpu.h \
 	stm32l.h \
+	math.h \
 	Makefile
 
 #
@@ -44,6 +45,20 @@ INC = \
 #STACK_GUARD=ao_mpu_stm.c
 #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1
 
+MATH_SRC=\
+	ef_acos.c \
+	ef_sqrt.c
+
+#	ef_rem_pio2.c \
+#	kf_cos.c \
+#	kf_sin.c \
+#	kf_rem_pio2.c \
+#	sf_copysign.c \
+#	sf_cos.c \
+#	sf_fabs.c \
+#	sf_floor.c \
+#	sf_scalbn.c
+
 ALTOS_SRC = \
 	ao_boot_chain.c \
 	ao_interrupt.c \
@@ -93,6 +108,7 @@ ALTOS_SRC = \
 	ao_companion.c \
 	ao_pyro.c \
 	ao_aprs.c \
+	$(MATH_SRC) \
 	$(PROFILE) \
 	$(SAMPLE_PROFILE) \
 	$(STACK_GUARD)
diff --git a/src/test/Makefile b/src/test/Makefile
index 5eee6bbb..f64a9531 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,15 +1,15 @@
-vpath % ..:../core:../drivers:../util:../micropeak:../aes
+vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product
 
 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_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \
 	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \
-	ao_ms5607_convert_test
+	ao_ms5607_convert_test ao_quaternion_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 -I../micropeak -O0 -g -Wall
+CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall
 
 all: $(PROGS) ao_aprs_data.wav
 
@@ -80,3 +80,6 @@ ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h
 
 ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h
 	cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c
+
+ao_quaternion_test: ao_quaternion_test.c ao_quaternion.h
+	cc $(CFLAGS) -o $@ ao_quaternion_test.c -lm
\ No newline at end of file
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index faf31aa7..e2f63e34 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -25,6 +25,8 @@
 #include <getopt.h>
 #include <math.h>
 
+#define GRAVITY 9.80665
+
 #define AO_HERTZ	100
 
 #define HAS_ADC 1
@@ -36,6 +38,11 @@
 #define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
 #define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
 
+#define AO_GPS_NEW_DATA		1
+#define AO_GPS_NEW_TRACKING	2
+
+int ao_gps_new;
+
 #if TELEMEGA
 #define AO_ADC_NUM_SENSE	6
 #define HAS_MS5607		1
@@ -240,7 +247,6 @@ struct ao_config ao_config;
 #define DATA_TO_XDATA(x) (x)
 
 
-#define GRAVITY 9.80665
 extern int16_t ao_ground_accel, ao_flight_accel;
 extern int16_t ao_accel_2g;
 
@@ -339,20 +345,6 @@ ao_test_exit(void)
 	exit(0);
 }
 
-#if HAS_MPU6000
-static double
-ao_mpu6000_accel(int16_t sensor)
-{
-	return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY;
-}
-
-static double
-ao_mpu6000_gyro(int32_t sensor)
-{
-	return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
-}
-#endif
-
 void
 ao_insert(void)
 {
@@ -408,23 +400,23 @@ ao_insert(void)
 		if (!ao_summary) {
 			printf("%7.2f height %8.2f accel %8.3f "
 #if TELEMEGA
-			       "roll %8.3f angle %8.3f qangle %8.3f "
-			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
+			       "angle %5d "
+/*			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */
 #endif
 			       "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
 			       time,
 			       height,
 			       accel,
 #if TELEMEGA
-			       ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
-			       ao_mpu6000_gyro(ao_sample_angle) / 100.0,
-			       ao_sample_qangle,
+			       ao_orient,
+/*
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z),
+*/
 #endif
 			       ao_state_names[ao_flight_state],
 			       ao_k_height / 65536.0,
@@ -589,207 +581,6 @@ int32(uint8_t *bytes, int off)
 
 static int log_format;
 
-#if TELEMEGA
-
-static double
-ao_vec_norm(double x, double y, double z)
-{
-	return x*x + y*y + z*z;
-}
-
-static void
-ao_vec_normalize(double *x, double *y, double *z)
-{
-	double	scale = 1/sqrt(ao_vec_norm(*x, *y, *z));
-
-	*x *= scale;
-	*y *= scale;
-	*z *= scale;
-}
-
-struct ao_quat {
-	double	q0, q1, q2, q3;
-};
-
-static void
-ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b)
-{
-	r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
-	r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
-	r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
-	r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
-}
-
-#if 0
-static void
-ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s)
-{
-	r->q0 = a->q0 * s;
-	r->q1 = a->q1 * s;
-	r->q2 = a->q2 * s;
-	r->q3 = a->q3 * s;
-}
-#endif
-
-static void
-ao_quat_conj(struct ao_quat *r, struct ao_quat *a)
-{
-	r->q0 =  a->q0;
-	r->q1 = -a->q1;
-	r->q2 = -a->q2;
-	r->q3 = -a->q3;
-}
-
-static void
-ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q)
-{
-	struct ao_quat	t;
-	struct ao_quat	c;
-	ao_quat_mul(&t, q, a);
-	ao_quat_conj(&c, q);
-	ao_quat_mul(r, &t, &c);
-}
-
-static void
-ao_quat_from_angle(struct ao_quat *r,
-		   double x_rad,
-		   double y_rad,
-		   double z_rad)
-{
-	double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad);
-	double s = sin(angle/2);
-	double c = cos(angle/2);
-
-	r->q0 = c;
-	r->q1 = x_rad * s / angle;
-	r->q2 = y_rad * s / angle;
-	r->q3 = z_rad * s / angle;
-}
-
-static void
-ao_quat_from_vector(struct ao_quat *r, double x, double y, double z)
-{
-	ao_vec_normalize(&x, &y, &z);
-	double	x_rad = atan2(z, y);
-	double	y_rad = atan2(x, z);
-	double	z_rad = atan2(y, x);
-
-	ao_quat_from_angle(r, x_rad, y_rad, z_rad);
-}
-
-static double
-ao_quat_norm(struct ao_quat *a)
-{
-	return (a->q0 * a->q0 +
-		a->q1 * a->q1 +
-		a->q2 * a->q2 +
-		a->q3 * a->q3);
-}
-
-static void
-ao_quat_normalize(struct ao_quat *a)
-{
-	double	norm = ao_quat_norm(a);
-
-	if (norm) {
-		double m = 1/sqrt(norm);
-
-		a->q0 *= m;
-		a->q1 *= m;
-		a->q2 *= m;
-		a->q3 *= m;
-	}
-}
-
-static struct ao_quat	ao_up, ao_current;
-static struct ao_quat	ao_orient;
-static int		ao_orient_tick;
-
-void
-set_orientation(double x, double y, double z, int tick)
-{
-	struct ao_quat	t;
-
-	printf ("set_orientation %g %g %g\n", x, y, z);
-	ao_quat_from_vector(&ao_orient, x, y, z);
-	ao_up.q1 = ao_up.q2 = 0;
-	ao_up.q0 = ao_up.q3 = sqrt(2)/2;
-	ao_orient_tick = tick;
-
-	ao_orient.q0 = 1;
-	ao_orient.q1 = 0;
-	ao_orient.q2 = 0;
-	ao_orient.q3 = 0;
-
-	printf ("orient (%g) %g %g %g up (%g) %g %g %g\n",
-		ao_orient.q0,
-		ao_orient.q1,
-		ao_orient.q2,
-		ao_orient.q3,
-		ao_up.q0,
-		ao_up.q1,
-		ao_up.q2,
-		ao_up.q3);
-
-	ao_quat_rot(&t, &ao_up, &ao_orient);
-	printf ("pad orient (%g) %g %g %g\n",
-		t.q0,
-		t.q1,
-		t.q2,
-		t.q3);
-
-}
-
-void
-update_orientation (double rate_x, double rate_y, double rate_z, int tick)
-{
-	struct ao_quat	q_dot;
-	double		lambda;
-	double		dt = (tick - ao_orient_tick) / 100.0;
-
-	ao_orient_tick = tick;
- 
-//	lambda = 1 - ao_quat_norm(&ao_orient);
-	lambda = 0;
-
-	q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0;
-	q_dot.q1 =  0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1;
-	q_dot.q2 =  0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
-	q_dot.q3 =  0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
-
-#if 0
-	printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
-	printf ("q_dot (%g) %g %g %g\n",
-		q_dot.q0,
-		q_dot.q1,
-		q_dot.q2,
-		q_dot.q3);
-#endif
-
-	ao_orient.q0 += q_dot.q0 * dt;
-	ao_orient.q1 += q_dot.q1 * dt;
-	ao_orient.q2 += q_dot.q2 * dt;
-	ao_orient.q3 += q_dot.q3 * dt;
-
-	ao_quat_normalize(&ao_orient);
-
-	ao_quat_rot(&ao_current, &ao_up, &ao_orient);
-
-	ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2));
-#if 0
-	printf ("orient (%g) %g %g %g current (%g) %g %g %g\n",
-		ao_orient.q0,
-		ao_orient.q1,
-		ao_orient.q2,
-		ao_orient.q3,
-		ao_current.q0,
-		ao_current.q1,
-		ao_current.q2,
-		ao_current.q3);
-#endif
-}
-#endif
-
 void
 ao_sleep(void *wchan)
 {
@@ -872,32 +663,6 @@ ao_sleep(void *wchan)
 #if HAS_MMA655X
 					ao_data_static.mma655x = int16(bytes, 26);
 #endif
-					if (ao_records_read == 0)
-						ao_ground_mpu6000 = ao_data_static.mpu6000;
-					else if (ao_records_read < 10) {
-#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2)
-						f(accel_x);
-						f(accel_y);
-						f(accel_z);
-						f(gyro_x);
-						f(gyro_y);
-						f(gyro_z);
-
-						double		accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x);
-						double		accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y);
-						double		accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z);
-
-						/* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes
-						 * Z is normal to the ground, the MPU y axis
-						 */
-						set_orientation(accel_x, accel_z, accel_y, tick);
-					} else {
-						double		rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x);
-						double		rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
-						double		rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
-
-						update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
-					}
 					ao_records_read++;
 					ao_insert();
 					return;
diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c
new file mode 100644
index 00000000..0e4b5b07
--- /dev/null
+++ b/src/test/ao_quaternion_test.c
@@ -0,0 +1,67 @@
+/*
+ * 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.
+ */
+
+#define _GNU_SOURCE
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stddef.h>
+#include <string.h>
+#include <getopt.h>
+#include <math.h>
+
+#include "ao_quaternion.h"
+
+static void
+print_q(char *name, struct ao_quaternion *q)
+{
+	printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name,
+		q->r, q->x, q->y, q->z);
+}
+
+int main(int argc, char **argv)
+{
+	struct ao_quaternion	position;
+	struct ao_quaternion	rotation;
+	struct ao_quaternion	little_rotation;
+	int			i;
+
+	/* unit x vector */
+	ao_quaternion_init_vector(&position, 1, 0, 0);
+
+	/* zero rotation */
+	ao_quaternion_init_zero_rotation(&rotation);
+
+	/* π/16 rotation around Z axis */
+	ao_quaternion_init_rotation(&little_rotation, 0, 0, 1,
+				    sin((M_PI/16)/2),
+				    cos((M_PI/16)/2));
+	for (i = 0; i <= 16; i++) {
+		struct ao_quaternion	rotated;
+
+		ao_quaternion_rotate(&rotated, &position, &rotation);
+		print_q("position", &position);
+		print_q("rotated", &rotated);
+		print_q("rotation", &rotation);
+		printf ("\n");
+		ao_quaternion_multiply(&rotation, &rotation, &little_rotation);
+		ao_quaternion_normalize(&rotation, &rotation);
+	}
+	return 0;
+}
+
-- 
cgit v1.2.3


From 351d53836e201834a2d89773a08ab7c2dab2b2f4 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Fri, 25 Oct 2013 04:34:16 -0700
Subject: altos: Calibrate IMU accelerometers too

Average the IMU accelerometer values pointing up and down so that we
have a zero-g offset for all three axes. This can then be used to
compute which direction the rocket is pointing while sitting on the pad.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao.h             |  7 +++++-
 src/core/ao_config.c      | 61 +++++++++++++++++++++++++++++++++++++++++++++++
 src/core/ao_sample.c      | 15 +++++-------
 src/test/ao_flight_test.c | 10 ++++++++
 4 files changed, 83 insertions(+), 10 deletions(-)

(limited to 'src/test')

diff --git a/src/core/ao.h b/src/core/ao.h
index ea37885e..d12f13a0 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -739,7 +739,7 @@ extern __xdata uint8_t ao_force_freq;
 #endif
 
 #define AO_CONFIG_MAJOR	1
-#define AO_CONFIG_MINOR	14
+#define AO_CONFIG_MINOR	15
 
 #define AO_AES_LEN 16
 
@@ -773,6 +773,11 @@ struct ao_config {
 #if HAS_RADIO_AMP
 	uint8_t		radio_amp;		/* minor version 14 */
 #endif
+#if HAS_GYRO
+	uint16_t	accel_zero_along;	/* minor version 15 */
+	uint16_t	accel_zero_across;	/* minor version 15 */
+	uint16_t	accel_zero_through;	/* minor version 15 */
+#endif
 };
 
 #define AO_IGNITE_MODE_DUAL		0
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index b480e14c..82faf32b 100644
--- a/src/core/ao_config.c
+++ b/src/core/ao_config.c
@@ -155,6 +155,19 @@ _ao_config_get(void)
 #if HAS_RADIO_AMP
 		if (minor  < 14)
 			ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP;
+#endif
+#if HAS_GYRO
+		if (minor < 15) {
+			ao_config.accel_zero_along = 0;
+			ao_config.accel_zero_across = 0;
+			ao_config.accel_zero_through = 0;
+
+			/* Reset the main accel offsets to force
+			 * re-calibration
+			 */
+			ao_config.accel_plus_g = 0;
+			ao_config.accel_minus_g = 0;
+		}
 #endif
 		ao_config.minor = AO_CONFIG_MINOR;
 		ao_config_dirty = 1;
@@ -275,17 +288,34 @@ ao_config_accel_calibrate_show(void) __reentrant
 {
 	printf("Accel cal +1g: %d -1g: %d\n",
 	       ao_config.accel_plus_g, ao_config.accel_minus_g);
+#if HAS_GYRO
+	printf ("IMU cal along %d across %d through %d\n",
+		ao_config.accel_zero_along,
+		ao_config.accel_zero_across,
+		ao_config.accel_zero_through);
+#endif
 }
 
 #define ACCEL_CALIBRATE_SAMPLES	1024
 #define ACCEL_CALIBRATE_SHIFT	10
 
+#if HAS_GYRO
+static int16_t accel_cal_along;
+static int16_t accel_cal_across;
+static int16_t accel_cal_through;
+#endif
+
 static int16_t
 ao_config_accel_calibrate_auto(char *orientation) __reentrant
 {
 	uint16_t	i;
 	int32_t		accel_total;
 	uint8_t		cal_data_ring;
+#if HAS_GYRO
+	int32_t		accel_along_total = 0;
+	int32_t		accel_across_total = 0;
+	int32_t		accel_through_total = 0;
+#endif
 
 	printf("Orient antenna %s and press a key...", orientation);
 	flush();
@@ -299,10 +329,20 @@ ao_config_accel_calibrate_auto(char *orientation) __reentrant
 		ao_sleep(DATA_TO_XDATA(&ao_sample_data));
 		while (i && cal_data_ring != ao_sample_data) {
 			accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]);
+#if HAS_GYRO
+			accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]);
+			accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]);
+			accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]);
+#endif
 			cal_data_ring = ao_data_ring_next(cal_data_ring);
 			i--;
 		}
 	}
+#if HAS_GYRO
+	accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT;
+	accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT;
+	accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT;
+#endif
 	return accel_total >> ACCEL_CALIBRATE_SHIFT;
 }
 
@@ -310,12 +350,28 @@ void
 ao_config_accel_calibrate_set(void) __reentrant
 {
 	int16_t	up, down;
+#if HAS_GYRO
+	int16_t	accel_along_up, accel_along_down;
+	int16_t	accel_across_up, accel_across_down;
+	int16_t	accel_through_up, accel_through_down;
+#endif
+	
 	ao_cmd_decimal();
 	if (ao_cmd_status != ao_cmd_success)
 		return;
 	if (ao_cmd_lex_i == 0) {
 		up = ao_config_accel_calibrate_auto("up");
+#if HAS_GYRO
+		accel_along_up = accel_cal_along;
+		accel_across_up = accel_cal_across;
+		accel_through_up = accel_cal_through;
+#endif
 		down = ao_config_accel_calibrate_auto("down");
+#if HAS_GYRO
+		accel_along_down = accel_cal_along;
+		accel_across_down = accel_cal_across;
+		accel_through_down = accel_cal_through;
+#endif
 	} else {
 		up = ao_cmd_lex_i;
 		ao_cmd_decimal();
@@ -331,6 +387,11 @@ ao_config_accel_calibrate_set(void) __reentrant
 	_ao_config_edit_start();
 	ao_config.accel_plus_g = up;
 	ao_config.accel_minus_g = down;
+#if HAS_GYRO
+	ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2;
+	ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2;
+	ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2;
+#endif
 	_ao_config_edit_finish();
 }
 #endif /* HAS_ACCEL */
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 676e0ffd..a9d50cb2 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -139,19 +139,16 @@ ao_sample_preflight_set(void)
 	/* No rotation yet */
 	ao_quaternion_init_zero_rotation(&ao_rotation);
 
-	/* XXX Assume we're pointing straight up for now */
+	/* Take the pad IMU acceleration values and compute our current direction
+	 */
 	ao_quaternion_init_vector(&ao_pad_orientation,
-				  ao_ground_accel_across,
-				  ao_ground_accel_through,
-				  -ao_ground_accel_along);
+				  ao_ground_accel_across - ao_config.accel_zero_across,
+				  ao_ground_accel_through - ao_config.accel_zero_through,
+				  -ao_ground_accel_along - ao_config.accel_zero_along);
+
 	ao_quaternion_normalize(&ao_pad_orientation,
 				&ao_pad_orientation);
 				  
-	printf ("pad r%8.5f x%8.5f y%8.5f z%8.5f\n",
-		ao_pad_orientation.r,
-		ao_pad_orientation.x,
-		ao_pad_orientation.y,
-		ao_pad_orientation.z);
 #endif	
 	nsamples = 0;
 }
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index e2f63e34..7f18c80e 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -234,6 +234,9 @@ struct ao_config {
 	uint16_t	apogee_lockout;
 #if TELEMEGA
 	struct ao_pyro	pyro[AO_PYRO_NUM];	/* minor version 12 */
+	int16_t		accel_zero_along;
+	int16_t		accel_zero_across;
+	int16_t		accel_zero_through;
 #endif
 };
 
@@ -719,6 +722,13 @@ ao_sleep(void *wchan)
 			} else if (nword >= 6 && strcmp(words[0], "Accel") == 0) {
 				ao_config.accel_plus_g = atoi(words[3]);
 				ao_config.accel_minus_g = atoi(words[5]);
+#ifdef TELEMEGA
+			} else if (nword >= 8 && strcmp(words[0], "IMU") == 0) {
+				ao_config.accel_zero_along = atoi(words[3]);
+				ao_config.accel_zero_across = atoi(words[5]);
+				ao_config.accel_zero_through = atoi(words[7]);
+				printf ("%d %d %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, ao_config.accel_zero_through);
+#endif
 			} else if (nword >= 4 && strcmp(words[0], "Main") == 0) {
 				ao_config.main_deploy = atoi(words[2]);
 			} else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 &&
-- 
cgit v1.2.3


From 5d9e715d570b24ac124c30772b11923bd26ed670 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sun, 27 Oct 2013 23:44:47 -0700
Subject: altos: Update quaternion tests to check vectors_to_rotation

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/test/Makefile             |   2 +-
 src/test/ao_flight_test.c     |   3 +-
 src/test/ao_quaternion_test.c | 121 ++++++++++++++++++++++++++++++++++++------
 src/test/plotmm               |  22 ++++++--
 src/test/run-mm               |  59 ++++++++++----------
 5 files changed, 158 insertions(+), 49 deletions(-)

(limited to 'src/test')

diff --git a/src/test/Makefile b/src/test/Makefile
index f64a9531..686fde0c 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -5,7 +5,7 @@ PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noi
 	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \
 	ao_ms5607_convert_test ao_quaternion_test
 
-INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
+INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h ao_quaternion.h
 
 KALMAN=make-kalman 
 
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 7f18c80e..952a811a 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -128,6 +128,7 @@ int	ao_summary = 0;
 #define ao_rdf_set(rdf)
 #define ao_packet_slave_start()
 #define ao_packet_slave_stop()
+#define flush()
 
 enum ao_igniter {
 	ao_igniter_drogue = 0,
@@ -411,7 +412,7 @@ ao_insert(void)
 			       height,
 			       accel,
 #if TELEMEGA
-			       ao_orient,
+			       ao_sample_orient,
 /*
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c
index 0e4b5b07..b630f1d3 100644
--- a/src/test/ao_quaternion_test.c
+++ b/src/test/ao_quaternion_test.c
@@ -27,41 +27,128 @@
 
 #include "ao_quaternion.h"
 
+#if 0
 static void
 print_q(char *name, struct ao_quaternion *q)
 {
 	printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name,
 		q->r, q->x, q->y, q->z);
 }
+#endif
+
+#define STEPS	16
+
+#define DEG	(1.0f * 3.1415926535f / 180.0f)
+
+struct ao_rotation {
+	int	steps;
+	float	x, y, z;
+};
+
+static struct ao_rotation ao_path[] = {
+	{ .steps = 45, .x =  2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = 2*DEG, .z = 0 },
+	
+	{ .steps = 45, .x = -2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = -2*DEG, .z = 0 },
+};
+
+#define NUM_PATH	(sizeof ao_path / sizeof ao_path[0])
+
+static int close(float a, float b) {
+	return fabsf (a - b) < 1e-5;
+}
+
+static int check_quaternion(char *where, struct ao_quaternion *got, struct ao_quaternion *expect) {
+	if (!close (got->r, expect->r) ||
+	    !close (got->x, expect->x) ||
+	    !close (got->y, expect->y) ||
+	    !close (got->z, expect->z))
+	{
+		printf ("%s: got r%8.5f x%8.5f y%8.5f z%8.5f expect r%8.5f x%8.5f y%8.5f z%8.5f\n",
+			where,
+			got->r, got->x, got->y, got->z,
+			expect->r, expect->x, expect->y, expect->z);
+		return 1;
+	}
+	return 0;
+}
 
 int main(int argc, char **argv)
 {
 	struct ao_quaternion	position;
+	struct ao_quaternion 	position_expect;
 	struct ao_quaternion	rotation;
+	struct ao_quaternion	rotated;
 	struct ao_quaternion	little_rotation;
 	int			i;
+	int			p;
+	int			ret = 0;
 
-	/* unit x vector */
-	ao_quaternion_init_vector(&position, 1, 0, 0);
+	/* vector */
+	ao_quaternion_init_vector(&position, 1, 1, 1);
+	ao_quaternion_init_vector(&position_expect, -1, -1, 1);
 
 	/* zero rotation */
 	ao_quaternion_init_zero_rotation(&rotation);
 
-	/* π/16 rotation around Z axis */
-	ao_quaternion_init_rotation(&little_rotation, 0, 0, 1,
-				    sin((M_PI/16)/2),
-				    cos((M_PI/16)/2));
-	for (i = 0; i <= 16; i++) {
-		struct ao_quaternion	rotated;
-
-		ao_quaternion_rotate(&rotated, &position, &rotation);
-		print_q("position", &position);
-		print_q("rotated", &rotated);
-		print_q("rotation", &rotation);
-		printf ("\n");
-		ao_quaternion_multiply(&rotation, &rotation, &little_rotation);
-		ao_quaternion_normalize(&rotation, &rotation);
+#define dump() do {							\
+									\
+		ao_quaternion_rotate(&rotated, &position, &rotation);	\
+		print_q("rotated", &rotated);				\
+		print_q("rotation", &rotation);				\
+		printf ("\n");						\
+	} while (0)
+
+//	dump();
+
+	for (p = 0; p < NUM_PATH; p++) {
+		ao_quaternion_init_half_euler(&little_rotation,
+					      ao_path[p].x / 2.0f,
+					      ao_path[p].y / 2.0f,
+					      ao_path[p].z / 2.0f);
+//		printf ("\t\tx: %8.4f, y: %8.4f, z: %8.4f ", ao_path[p].x, ao_path[p].y, ao_path[p].z);
+//		print_q("step", &little_rotation);
+//		printf("\n");
+		for (i = 0; i < ao_path[p].steps; i++) {
+			ao_quaternion_multiply(&rotation, &little_rotation, &rotation);
+
+			ao_quaternion_normalize(&rotation, &rotation);
+
+//			dump();
+		}
 	}
-	return 0;
+
+	ao_quaternion_rotate(&rotated, &position, &rotation);
+
+	ret += check_quaternion("rotation", &rotated, &position_expect);
+
+	struct ao_quaternion	vertical;
+	struct ao_quaternion	angle;
+	struct ao_quaternion	rot;
+
+	ao_quaternion_init_vector(&vertical, 0, 0, 1);
+	ao_quaternion_init_vector(&angle, 0, 0, 1);
+
+	ao_quaternion_init_half_euler(&rot,
+				      M_PI * 3.0 / 8.0 , 0, 0);
+
+	ao_quaternion_rotate(&angle, &angle, &rot);
+
+	struct ao_quaternion	rot_compute;
+
+	ao_quaternion_vectors_to_rotation(&rot_compute, &vertical, &angle);
+
+	ret += check_quaternion("vector rotation", &rot_compute, &rot);
+
+	struct ao_quaternion	rotd;
+
+	ao_quaternion_rotate(&rotd, &vertical, &rot_compute);
+
+	ret += check_quaternion("vector rotated", &rotd, &angle);
+
+	return ret;
 }
 
diff --git a/src/test/plotmm b/src/test/plotmm
index 5f5bd2ca..bfe15f4c 100755
--- a/src/test/plotmm
+++ b/src/test/plotmm
@@ -1,3 +1,19 @@
+#!/bin/sh
+
+case $# in
+1)
+	file="$1"
+	title="$1"
+	;;
+2)
+	file="$1"
+	title="$2"
+	;;
+*)
+	echo "Usage: $0 <data-file> <title>"
+	exit 1
+esac
+
 gnuplot -persist << EOF
 set ylabel "altitude (m)"
 set y2label "angle (d)"
@@ -5,7 +21,7 @@ set xlabel "time (s)"
 set xtics border out nomirror
 set ytics border out nomirror
 set y2tics border out nomirror
-plot "$1" using 1:3 with lines axes x1y1 title "raw height",\
-"$1" using 1:9 with lines axes x1y2 title "angle",\
-"$1" using 1:11 with lines axes x1y2 title "qangle"
+set title "$title"
+plot "$file" using 1:3 with lines axes x1y1 title "raw height",\
+"$file" using 1:7 with lines axes x1y2 title "angle"
 EOF
diff --git a/src/test/run-mm b/src/test/run-mm
index 6f3d97a2..ae5e5f42 100755
--- a/src/test/run-mm
+++ b/src/test/run-mm
@@ -3,15 +3,20 @@
 DIR=~/misc/rockets/flights
 
 for i in "$@"; do
-case "$i" in
-    */*)
-    file="$i"
-    ;;
-    *)
-    file="$DIR/$i"
-    ;;
-esac
-./ao_flight_test_mm "$file" > run-out.mm
+    case "$i" in
+	*/*)
+	file="$i"
+	;;
+	*)
+	file="$DIR/$i"
+	;;
+    esac
+    base=`basename "$i" .eeprom`
+
+    ./ao_flight_test_mm "$file" > $base.plot
+
+    sh ./plotmm $base.plot `basename "$file"`
+done
 
 #./ao_flight_test_accel "$file" > run-out.accel
 #"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\
@@ -21,21 +26,21 @@ esac
 #"run-out.accel" using 1:17 with lines lt 4 axes x1y1 title "accel main",\
 #
 
-gnuplot << EOF
-set ylabel "altitude (m)"
-set y2label "velocity (m/s), acceleration(m/s²)"
-set xlabel "time (s)"
-set xtics border out nomirror
-set ytics border out nomirror
-set y2tics border out nomirror
-set title "$i"
-plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
-"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
-"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
-"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
-"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
-"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
-"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
-pause mouse close
-EOF
-done
\ No newline at end of file
+#gnuplot << EOF
+#set ylabel "altitude (m)"
+#set y2label "velocity (m/s), acceleration(m/s²)"
+#set xlabel "time (s)"
+#set xtics border out nomirror
+#set ytics border out nomirror
+#set y2tics border out nomirror
+#set title "$i"
+#plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
+#"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
+#"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
+#"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
+#"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
+#"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
+#"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
+#pause mouse close
+#EOF
+#done
\ No newline at end of file
-- 
cgit v1.2.3


From bdd6244d8b4a55c9aa4fb79b0cb1a0727afbc2ac Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Tue, 12 Nov 2013 14:01:55 +0900
Subject: altos: Add orientation tracking to ao_flight_test

Shows calculated offset from vertical in ao_flight_test output

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/core/ao_data.h        |  12 +++++
 src/core/ao_quaternion.h  |   9 ++++
 src/test/ao_flight_test.c | 111 +++++++++++++++++++++++++++++++++++++++++++---
 3 files changed, 127 insertions(+), 5 deletions(-)

(limited to 'src/test')

diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 5a232885..e1d8a139 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -319,4 +319,16 @@ typedef int16_t angle_t;	/* in degrees */
 
 #endif
 
+#if !HAS_MAG && HAS_HMC5883
+
+#define HAS_MAG		1
+
+typedef int16_t ao_mag_t;		/* in raw sample units */
+
+#define ao_data_mag_along(packet)	((packet)->hmc5883.x)
+#define ao_data_mag_across(packet)	((packet)->hmc5883.y)
+#define ao_data_mag_through(packet)	((packet)->hmc5883.z)
+
+#endif
+
 #endif /* _AO_DATA_H_ */
diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h
index 6c885500..044f1607 100644
--- a/src/core/ao_quaternion.h
+++ b/src/core/ao_quaternion.h
@@ -109,6 +109,15 @@ static inline void ao_quaternion_normalize(struct ao_quaternion *r,
 		*r = *a;
 }
 
+static inline float ao_quaternion_dot(const struct ao_quaternion *a,
+				      const struct ao_quaternion *b)
+{
+#define T(_a)	(((a)->_a) * ((b)->_a))
+	return T(r) + T(x) + T(y) + T(z);
+#undef T
+}
+				     
+
 static inline void ao_quaternion_rotate(struct ao_quaternion *r,
 					const struct ao_quaternion *a,
 					const struct ao_quaternion *b)
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 952a811a..452f5b75 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -48,6 +48,7 @@ int ao_gps_new;
 #define HAS_MS5607		1
 #define HAS_MPU6000		1
 #define HAS_MMA655X		1
+#define HAS_HMC5883 		1
 
 struct ao_adc {
 	int16_t			sense[AO_ADC_NUM_SENSE];
@@ -349,6 +350,23 @@ ao_test_exit(void)
 	exit(0);
 }
 
+#ifdef TELEMEGA
+struct ao_azel {
+	int	az;
+	int	el;
+};
+
+static void
+azel (struct ao_azel *r, struct ao_quaternion *q)
+{
+	double	v;
+
+	r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5);
+	v = sqrt (q->x*q->x + q->y*q->y);
+	r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5);
+}
+#endif
+
 void
 ao_insert(void)
 {
@@ -402,10 +420,86 @@ ao_insert(void)
 		}
 
 		if (!ao_summary) {
+#if TELEMEGA
+			static struct ao_quaternion	ao_ground_mag;
+			static int			ao_ground_mag_set;
+
+			if (!ao_ground_mag_set) {
+				ao_quaternion_init_vector (&ao_ground_mag,
+							   ao_data_mag_across(&ao_data_static),
+							   ao_data_mag_through(&ao_data_static),
+							   ao_data_mag_along(&ao_data_static));
+				ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag);
+				ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation);
+				ao_ground_mag_set = 1;
+			}
+
+			struct ao_quaternion		ao_mag, ao_mag_rot;
+
+			ao_quaternion_init_vector(&ao_mag,
+						  ao_data_mag_across(&ao_data_static),
+						  ao_data_mag_through(&ao_data_static),
+						  ao_data_mag_along(&ao_data_static));
+
+			ao_quaternion_normalize(&ao_mag, &ao_mag);
+			ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation);
+			
+			float				ao_dot;
+			int				ao_mag_angle;
+
+			ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag);
+
+			struct ao_azel			ground_azel, mag_azel, rot_azel;
+
+			azel(&ground_azel, &ao_ground_mag);
+			azel(&mag_azel, &ao_mag);
+			azel(&rot_azel, &ao_mag_rot);
+
+			ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5);
+
+			static struct ao_quaternion	ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 };
+			struct ao_quaternion		ao_out;
+
+			ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation);
+
+			int	out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI);
+
+			printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n",
+				time,
+				ao_state_names[ao_flight_state],
+				ao_k_height / 65536.0,
+				ao_sample_orient, out,
+				mag_azel.el,
+				mag_azel.az);
+			
+
+#if 0
+			printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n",
+				ao_state_names[ao_flight_state],
+				ground_azel.az, ground_azel.el,
+				mag_azel.az, mag_azel.el,
+				rot_azel.az, rot_azel.el,
+				ground_azel.el - rot_azel.el,
+				ground_azel.az - rot_azel.az,
+				ao_mag_angle,
+				ao_sample_orient,
+				ao_ground_mag.x,
+				ao_ground_mag.y,
+				ao_ground_mag.z,
+				ao_mag.x,
+				ao_mag.y,
+				ao_mag.z,
+				ao_mag_rot.x,
+				ao_mag_rot.y,
+				ao_mag_rot.z);
+#endif
+#endif
+
+#if 0
 			printf("%7.2f height %8.2f accel %8.3f "
 #if TELEMEGA
 			       "angle %5d "
-/*			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " */
+			       "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d "
 #endif
 			       "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
 			       time,
@@ -413,14 +507,17 @@ ao_insert(void)
 			       accel,
 #if TELEMEGA
 			       ao_sample_orient,
-/*
+
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y),
 			       ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z),
-*/
+			       ao_data_static.hmc5883.x,
+			       ao_data_static.hmc5883.y,
+			       ao_data_static.hmc5883.z,
+			       ao_mag_angle,
 #endif
 			       ao_state_names[ao_flight_state],
 			       ao_k_height / 65536.0,
@@ -430,6 +527,7 @@ ao_insert(void)
 			       drogue_height,
 			       main_height,
 			       ao_error_h_sq_avg);
+#endif
 			
 //			if (ao_flight_state == ao_flight_landed)
 //				ao_test_exit();
@@ -659,11 +757,14 @@ ao_sleep(void *wchan)
 					ao_data_static.ms5607_raw.temp = int32(bytes, 4);
 					ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
 					ao_data_static.mpu6000.accel_x = int16(bytes, 8);
-					ao_data_static.mpu6000.accel_y = -int16(bytes, 10);
+					ao_data_static.mpu6000.accel_y = int16(bytes, 10);
 					ao_data_static.mpu6000.accel_z = int16(bytes, 12);
 					ao_data_static.mpu6000.gyro_x = int16(bytes, 14);
-					ao_data_static.mpu6000.gyro_y = -int16(bytes, 16);
+					ao_data_static.mpu6000.gyro_y = int16(bytes, 16);
 					ao_data_static.mpu6000.gyro_z = int16(bytes, 18);
+					ao_data_static.hmc5883.x = int16(bytes, 20);
+					ao_data_static.hmc5883.y = int16(bytes, 22);
+					ao_data_static.hmc5883.z = int16(bytes, 24);
 #if HAS_MMA655X
 					ao_data_static.mma655x = int16(bytes, 26);
 #endif
-- 
cgit v1.2.3


From 40d3575a9365d77ca507ebee226d51d081e1ecc6 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Tue, 12 Nov 2013 14:54:57 +0900
Subject: altos: Clean up .gitignore and add a few random files

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/.gitignore                 |  1 +
 src/lpcxpresso/.gitignore      |  3 +++
 src/telefire-v0.2/.gitignore   |  2 ++
 src/telefire-v0.2/.sdcdbrc     |  2 ++
 src/telegps-v0.3/.gitignore    |  3 +++
 src/telemini-v2.0/.gitignore   |  2 ++
 src/telemini-v2.0/.sdcdbrc     |  2 ++
 src/telescience-pwm/.gitignore |  2 +-
 src/test/.gitignore            |  4 ++++
 src/test/mega.flights          | 12 ++++++++++++
 src/test/plot-rot              | 30 ++++++++++++++++++++++++++++++
 src/test/run-all-mm            |  2 ++
 12 files changed, 64 insertions(+), 1 deletion(-)
 create mode 100644 src/lpcxpresso/.gitignore
 create mode 100644 src/telefire-v0.2/.gitignore
 create mode 100644 src/telefire-v0.2/.sdcdbrc
 create mode 100644 src/telegps-v0.3/.gitignore
 create mode 100644 src/telemini-v2.0/.gitignore
 create mode 100644 src/telemini-v2.0/.sdcdbrc
 create mode 100644 src/test/mega.flights
 create mode 100755 src/test/plot-rot
 create mode 100755 src/test/run-all-mm

(limited to 'src/test')

diff --git a/src/.gitignore b/src/.gitignore
index cae36ae6..a8628fae 100644
--- a/src/.gitignore
+++ b/src/.gitignore
@@ -1,3 +1,4 @@
+Makedefs
 altitude.h
 altitude-pa.h
 ao_whiten.h
diff --git a/src/lpcxpresso/.gitignore b/src/lpcxpresso/.gitignore
new file mode 100644
index 00000000..892c3acc
--- /dev/null
+++ b/src/lpcxpresso/.gitignore
@@ -0,0 +1,3 @@
+ao_product.h
+ao_serial_lpc.h
+*.elf
diff --git a/src/telefire-v0.2/.gitignore b/src/telefire-v0.2/.gitignore
new file mode 100644
index 00000000..4d4f4200
--- /dev/null
+++ b/src/telefire-v0.2/.gitignore
@@ -0,0 +1,2 @@
+telefire-*
+ao_product.h
diff --git a/src/telefire-v0.2/.sdcdbrc b/src/telefire-v0.2/.sdcdbrc
new file mode 100644
index 00000000..b9f6129c
--- /dev/null
+++ b/src/telefire-v0.2/.sdcdbrc
@@ -0,0 +1,2 @@
+--directory=../cc1111:../product:../core:../drivers:.
+
diff --git a/src/telegps-v0.3/.gitignore b/src/telegps-v0.3/.gitignore
new file mode 100644
index 00000000..892c3acc
--- /dev/null
+++ b/src/telegps-v0.3/.gitignore
@@ -0,0 +1,3 @@
+ao_product.h
+ao_serial_lpc.h
+*.elf
diff --git a/src/telemini-v2.0/.gitignore b/src/telemini-v2.0/.gitignore
new file mode 100644
index 00000000..568925a2
--- /dev/null
+++ b/src/telemini-v2.0/.gitignore
@@ -0,0 +1,2 @@
+ao_product.h
+telemini-v2.0*
diff --git a/src/telemini-v2.0/.sdcdbrc b/src/telemini-v2.0/.sdcdbrc
new file mode 100644
index 00000000..b9f6129c
--- /dev/null
+++ b/src/telemini-v2.0/.sdcdbrc
@@ -0,0 +1,2 @@
+--directory=../cc1111:../product:../core:../drivers:.
+
diff --git a/src/telescience-pwm/.gitignore b/src/telescience-pwm/.gitignore
index dfccadf8..5d9c0970 100644
--- a/src/telescience-pwm/.gitignore
+++ b/src/telescience-pwm/.gitignore
@@ -1,2 +1,2 @@
-telescience-v0.1*
+telescience-pwm*
 ao_product.h
diff --git a/src/test/.gitignore b/src/test/.gitignore
index 90af6517..b8b2513e 100644
--- a/src/test/.gitignore
+++ b/src/test/.gitignore
@@ -5,6 +5,10 @@ ao_flight_test_baro
 ao_flight_test_accel
 ao_gps_test
 ao_gps_test_skytraq
+ao_gps_test_ublox
+ao_int64_test
+ao_ms5607_convert_test
+ao_quaternion_test
 ao_convert_test
 ao_convert_pa_test
 ao_fat_test
diff --git a/src/test/mega.flights b/src/test/mega.flights
new file mode 100644
index 00000000..71c44e65
--- /dev/null
+++ b/src/test/mega.flights
@@ -0,0 +1,12 @@
+2013-10-12-serial-0094-flight-0001.eeprom
+2013-09-02-serial-094-flight-002.eeprom
+2013-03-02-serial-069-flight-002.eeprom
+2013-03-02-serial-069-flight-001.eeprom
+2013-05-27-serial-084-flight-001.mega
+2013-05-26-serial-085-flight-002.mega
+2013-05-19-serial-084-flight-003.mega
+2013-04-21-serial-069-flight-002.mega
+2012-10-20-serial-068-flight-004.mega
+2012-07-03-serial-058-flight-007.mega
+2012-06-30-serial-058-flight-006.mega
+2012-06-30-serial-058-flight-005.mega
diff --git a/src/test/plot-rot b/src/test/plot-rot
new file mode 100755
index 00000000..b6b77c93
--- /dev/null
+++ b/src/test/plot-rot
@@ -0,0 +1,30 @@
+#!/bin/sh
+
+case $# in
+1)
+	file="$1"
+	title="$1"
+	;;
+2)
+	file="$1"
+	title="$2"
+	;;
+*)
+	echo "Usage: $0 <data-file> <title>"
+	exit 1
+esac
+
+gnuplot -persist << EOF
+set ylabel "altitude (m)"
+set y2label "angle (d)"
+set xlabel "time (s)"
+set xtics border out nomirror
+set ytics border out nomirror
+set y2tics border out nomirror
+set title "$title"
+plot "$file" using 1:5 with lines axes x1y1 title "height",\
+"$file" using 1:9 with lines axes x1y2 title "gyro rot", \
+"$file" using 1:7 with lines axes x1y2 title "gyro tilt", \
+"$file" using 1:13 with lines axes x1y2 title "mag rot", \
+"$file" using 1:11 with lines axes x1y2 title "mag tilt"
+EOF
diff --git a/src/test/run-all-mm b/src/test/run-all-mm
new file mode 100755
index 00000000..d9c2043d
--- /dev/null
+++ b/src/test/run-all-mm
@@ -0,0 +1,2 @@
+#!/bin/sh
+./run-mm `cat mega.flights`
-- 
cgit v1.2.3


From cdb32b1717db4e8cb8cf94d810e74ce2b569566b Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sat, 7 Dec 2013 09:47:45 -0800
Subject: altos/test: Compute and plot tilt based on GPS track

This lets us compare the gyro-computed tilt angle against the actual
flight path.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/test/ao_flight_test.c | 233 ++++++++++++++++++++++------------------------
 src/test/plotmm           |   8 +-
 2 files changed, 118 insertions(+), 123 deletions(-)

(limited to 'src/test')

diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 452f5b75..0abb4090 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -92,6 +92,95 @@ struct ao_adc {
 
 #include <ao_data.h>
 #include <ao_log.h>
+#include <ao_telemetry.h>
+
+#if TELEMEGA
+int ao_gps_count;
+struct ao_telemetry_location ao_gps_first;
+struct ao_telemetry_location ao_gps_prev;
+struct ao_telemetry_location ao_gps_static;
+
+struct ao_telemetry_satellite ao_gps_tracking;
+
+static inline double sqr(double a) { return a * a; }
+
+void
+cc_great_circle (double start_lat, double start_lon,
+		 double end_lat, double end_lon,
+		 double *dist, double *bearing)
+{
+	const double rad = M_PI / 180;
+	const double earth_radius = 6371.2 * 1000;	/* in meters */
+	double lat1 = rad * start_lat;
+	double lon1 = rad * -start_lon;
+	double lat2 = rad * end_lat;
+	double lon2 = rad * -end_lon;
+
+//	double d_lat = lat2 - lat1;
+	double d_lon = lon2 - lon1;
+
+	/* From http://en.wikipedia.org/wiki/Great-circle_distance */
+	double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
+			  sqr(cos(lat1) * sin(lat2) -
+			      sin(lat1) * cos(lat2) * cos(d_lon)));
+	double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
+	double d = atan2(vdn,vdd);
+	double course;
+
+	if (cos(lat1) < 1e-20) {
+		if (lat1 > 0)
+			course = M_PI;
+		else
+			course = -M_PI;
+	} else {
+		if (d < 1e-10)
+			course = 0;
+		else
+			course = acos((sin(lat2)-sin(lat1)*cos(d)) /
+				      (sin(d)*cos(lat1)));
+		if (sin(lon2-lon1) > 0)
+			course = 2 * M_PI-course;
+	}
+	*dist = d * earth_radius;
+	*bearing = course * 180/M_PI;
+}
+
+double
+ao_distance_from_pad(void)
+{
+	double	dist, bearing;
+	if (!ao_gps_count)
+		return 0;
+	
+	cc_great_circle(ao_gps_first.latitude / 1e7,
+			ao_gps_first.longitude / 1e7,
+			ao_gps_static.latitude / 1e7,
+			ao_gps_static.longitude / 1e7,
+			&dist, &bearing);
+	return dist;
+}
+
+double
+ao_gps_angle(void)
+{
+	double	dist, bearing;
+	double	height;
+	double	angle;
+
+	if (ao_gps_count < 2)
+		return 0;
+
+	cc_great_circle(ao_gps_prev.latitude / 1e7,
+			ao_gps_prev.longitude / 1e7,
+			ao_gps_static.latitude / 1e7,
+			ao_gps_static.longitude / 1e7,
+			&dist, &bearing);
+	height = ao_gps_static.altitude - ao_gps_prev.altitude;
+
+	angle = atan2(dist, height);
+	return angle * 180/M_PI;
+}
+#endif
 
 #define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
 #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
@@ -390,6 +479,7 @@ ao_insert(void)
 		double	height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
 #endif
 
+		(void) accel;
 		if (!tick_offset)
 			tick_offset = -ao_data_static.tick;
 		if ((prev_tick - ao_data_static.tick) > 0x400)
@@ -457,6 +547,8 @@ ao_insert(void)
 
 			ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5);
 
+			(void) ao_mag_angle;
+
 			static struct ao_quaternion	ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 };
 			struct ao_quaternion		ao_out;
 
@@ -464,6 +556,7 @@ ao_insert(void)
 
 			int	out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI);
 
+#if 0
 			printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n",
 				time,
 				ao_state_names[ao_flight_state],
@@ -471,7 +564,15 @@ ao_insert(void)
 				ao_sample_orient, out,
 				mag_azel.el,
 				mag_azel.az);
-			
+#endif
+			printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n",
+				time,
+				ao_state_names[ao_flight_state],
+				ao_k_height / 65536.0,
+				ao_sample_orient, out,
+				ao_distance_from_pad(),
+				(int) floor (ao_gps_angle() + 0.5),
+				(ao_gps_static.flags & 0xf) * 10);
 
 #if 0
 			printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n",
@@ -535,125 +636,6 @@ ao_insert(void)
 	}
 }
 
-#define AO_MAX_CALLSIGN			8
-#define AO_MAX_VERSION			8
-#define AO_MAX_TELEMETRY		128
-
-struct ao_telemetry_generic {
-	uint16_t	serial;		/* 0 */
-	uint16_t	tick;		/* 2 */
-	uint8_t		type;		/* 4 */
-	uint8_t		payload[27];	/* 5 */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_SENSOR_TELEMETRUM	0x01
-#define AO_TELEMETRY_SENSOR_TELEMINI	0x02
-#define AO_TELEMETRY_SENSOR_TELENANO	0x03
-
-struct ao_telemetry_sensor {
-	uint16_t	serial;		/*  0 */
-	uint16_t	tick;		/*  2 */
-	uint8_t		type;		/*  4 */
-
-	uint8_t         state;          /*  5 flight state */
-	int16_t		accel;		/*  6 accelerometer (TM only) */
-	int16_t		pres;		/*  8 pressure sensor */
-	int16_t		temp;		/* 10 temperature sensor */
-	int16_t		v_batt;		/* 12 battery voltage */
-	int16_t		sense_d;	/* 14 drogue continuity sense (TM/Tm) */
-	int16_t		sense_m;	/* 16 main continuity sense (TM/Tm) */
-
-	int16_t         acceleration;   /* 18 m/s² * 16 */
-	int16_t         speed;          /* 20 m/s * 16 */
-	int16_t         height;         /* 22 m */
-
-	int16_t		ground_pres;	/* 24 average pres on pad */
-	int16_t		ground_accel;	/* 26 average accel on pad */
-	int16_t		accel_plus_g;	/* 28 accel calibration at +1g */
-	int16_t		accel_minus_g;	/* 30 accel calibration at -1g */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_CONFIGURATION	0x04
-
-struct ao_telemetry_configuration {
-	uint16_t	serial;				/*  0 */
-	uint16_t	tick;				/*  2 */
-	uint8_t		type;				/*  4 */
-
-	uint8_t         device;         		/*  5 device type */
-	uint16_t        flight;				/*  6 flight number */
-	uint8_t		config_major;			/*  8 Config major version */
-	uint8_t		config_minor;			/*  9 Config minor version */
-	uint16_t	apogee_delay;			/* 10 Apogee deploy delay in seconds */
-	uint16_t	main_deploy;			/* 12 Main deploy alt in meters */
-	uint16_t	flight_log_max;			/* 14 Maximum flight log size in kB */
-	char		callsign[AO_MAX_CALLSIGN];	/* 16 Radio operator identity */
-	char		version[AO_MAX_VERSION];	/* 24 Software version */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_LOCATION		0x05
-
-#define AO_GPS_MODE_NOT_VALID		'N'
-#define AO_GPS_MODE_AUTONOMOUS		'A'
-#define AO_GPS_MODE_DIFFERENTIAL	'D'
-#define AO_GPS_MODE_ESTIMATED		'E'
-#define AO_GPS_MODE_MANUAL		'M'
-#define AO_GPS_MODE_SIMULATED		'S'
-
-struct ao_telemetry_location {
-	uint16_t	serial;		/*  0 */
-	uint16_t	tick;		/*  2 */
-	uint8_t		type;		/*  4 */
-
-	uint8_t         flags;		/*  5 Number of sats and other flags */
-	int16_t         altitude;	/*  6 GPS reported altitude (m) */
-	int32_t         latitude;	/*  8 latitude (degrees * 10⁷) */
-	int32_t         longitude;	/* 12 longitude (degrees * 10⁷) */
-	uint8_t         year;		/* 16 (- 2000) */
-	uint8_t         month;		/* 17 (1-12) */
-	uint8_t         day;		/* 18 (1-31) */
-	uint8_t         hour;		/* 19 (0-23) */
-	uint8_t         minute;		/* 20 (0-59) */
-	uint8_t         second;		/* 21 (0-59) */
-	uint8_t         pdop;		/* 22 (m * 5) */
-	uint8_t         hdop;		/* 23 (m * 5) */
-	uint8_t         vdop;		/* 24 (m * 5) */
-	uint8_t         mode;		/* 25 */
-	uint16_t	ground_speed;	/* 26 cm/s */
-	int16_t		climb_rate;	/* 28 cm/s */
-	uint8_t		course;		/* 30 degrees / 2 */
-	uint8_t		unused[1];	/* 31 */
-	/* 32 */
-};
-
-#define AO_TELEMETRY_SATELLITE		0x06
-
-struct ao_telemetry_satellite_info {
-	uint8_t		svid;
-	uint8_t		c_n_1;
-};
-
-struct ao_telemetry_satellite {
-	uint16_t				serial;		/*  0 */
-	uint16_t				tick;		/*  2 */
-	uint8_t					type;		/*  4 */
-	uint8_t					channels;	/*  5 number of reported sats */
-
-	struct ao_telemetry_satellite_info	sats[12];	/* 6 */
-	uint8_t					unused[2];	/* 30 */
-	/* 32 */
-};
-
-union ao_telemetry_all {
-	struct ao_telemetry_generic		generic;
-	struct ao_telemetry_sensor		sensor;
-	struct ao_telemetry_configuration	configuration;
-	struct ao_telemetry_location		location;
-	struct ao_telemetry_satellite		satellite;
-};
 
 uint16_t
 uint16(uint8_t *bytes, int off)
@@ -771,6 +753,17 @@ ao_sleep(void *wchan)
 					ao_records_read++;
 					ao_insert();
 					return;
+				case 'G':
+					ao_gps_prev = ao_gps_static;
+					ao_gps_static.tick = tick;
+					ao_gps_static.latitude = int32(bytes, 0);
+					ao_gps_static.longitude = int32(bytes, 4);
+					ao_gps_static.altitude = int32(bytes, 8);
+					ao_gps_static.flags = bytes[13];
+					if (!ao_gps_count)
+						ao_gps_first = ao_gps_static;
+					ao_gps_count++;
+					break;
 				}
 				continue;
 			} else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
diff --git a/src/test/plotmm b/src/test/plotmm
index bfe15f4c..27f8ddcd 100755
--- a/src/test/plotmm
+++ b/src/test/plotmm
@@ -15,13 +15,15 @@ case $# in
 esac
 
 gnuplot -persist << EOF
-set ylabel "altitude (m)"
+set ylabel "distance (m)"
 set y2label "angle (d)"
 set xlabel "time (s)"
 set xtics border out nomirror
 set ytics border out nomirror
 set y2tics border out nomirror
 set title "$title"
-plot "$file" using 1:3 with lines axes x1y1 title "raw height",\
-"$file" using 1:7 with lines axes x1y2 title "angle"
+plot "$file" using 1:5 with lines axes x1y1 title "height",\
+"$file" using 1:7 with lines axes x1y2 title "angle",\
+"$file" using 1:13 with lines axes x1y2 title "gps angle",\
+"$file" using 1:15 with lines axes x1y2 title "sats"
 EOF
-- 
cgit v1.2.3