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/test/ao_flight_test.c | 57 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 57 insertions(+)

(limited to 'src/test/ao_flight_test.c')

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/ao_flight_test.c')

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 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/ao_flight_test.c')

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/ao_flight_test.c')

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/ao_flight_test.c')

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/ao_flight_test.c')

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 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/ao_flight_test.c')

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