altos: Average MPU6000 values on ground for later use

Having long-term ground averages recorded to the eeprom file will make
post-flight analysis of the data better.

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

4 files changed, 127 insertions, 5 deletions

diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 6fdd19cb..7e2f85d8 100644
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -288,4 +288,25 @@ typedef int16_t accel_t;
 
 #endif
 
+#if !HAS_GYRO && HAS_MPU6000
+
+#define HAS_GYRO	1
+
+typedef int16_t	gyro_t;
+typedef int32_t angle_t;
+
+/* Y axis is aligned with the direction of motion (along) */
+/* X axis is aligned in the other board axis (across) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+#define ao_data_along(packet)	((packet)->mpu6000.accel_y)
+#define ao_data_across(packet)	((packet)->mpu6000.accel_x)
+#define ao_data_through(packet)	((packet)->mpu6000.accel_z)
+
+#define ao_data_roll(packet)	((packet)->mpu6000.gyro_y)
+#define ao_data_pitch(packet)	((packet)->mpu6000.gyro_x)
+#define ao_data_yaw(packet)	((packet)->mpu6000.gyro_z)
+
+#endif
+
 #endif /* _AO_DATA_H_ */
diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index 4eaed420..93b01778 100644
--- a/src/core/ao_log.h
+++ b/src/core/ao_log.h
@@ -199,10 +199,16 @@ struct ao_log_mega {
 	union {						/* 4 */
 		/* AO_LOG_FLIGHT */
 		struct {
-			uint16_t	flight;		/* 4 */
-			int16_t		ground_accel;	/* 6 */
-			uint32_t	ground_pres;	/* 8 */
-		} flight;				/* 12 */
+			uint16_t	flight;			/* 4 */
+			int16_t		ground_accel;		/* 6 */
+			uint32_t	ground_pres;		/* 8 */
+			int16_t		ground_accel_along;	/* 16 */
+			int16_t		ground_accel_across;	/* 12 */
+			int16_t		ground_accel_through;	/* 14 */
+			int16_t		ground_gyro_roll;	/* 18 */
+			int16_t		ground_gyro_pitch;	/* 20 */
+			int16_t		ground_gyro_yaw;	/* 22 */
+		} flight;					/* 24 */
 		/* AO_LOG_STATE */
 		struct {
 			uint16_t	state;
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 985c0940..7a1eff8e 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -37,6 +37,16 @@ __pdata alt_t		ao_sample_height;
 #if HAS_ACCEL
 __pdata accel_t		ao_sample_accel;
 #endif
+#if HAS_GYRO
+__pdata accel_t		ao_sample_accel_along;
+__pdata accel_t		ao_sample_accel_across;
+__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;
+#endif
 
 __data uint8_t		ao_sample_data;
 
@@ -53,6 +63,15 @@ __pdata accel_t		ao_accel_2g;		/* factory accel calibration */
 __pdata int32_t		ao_accel_scale;		/* sensor to m/s² conversion */
 #endif
 
+#if HAS_GYRO
+__pdata accel_t		ao_ground_accel_along;
+__pdata accel_t		ao_ground_accel_across;
+__pdata accel_t		ao_ground_accel_through;
+__pdata gyro_t		ao_ground_pitch;
+__pdata gyro_t		ao_ground_yaw;
+__pdata gyro_t		ao_ground_roll;
+#endif
+
 static __pdata uint8_t	ao_preflight;		/* in preflight mode */
 
 static __pdata uint16_t	nsamples;
@@ -60,6 +79,14 @@ __pdata int32_t ao_sample_pres_sum;
 #if HAS_ACCEL
 __pdata int32_t ao_sample_accel_sum;
 #endif
+#if HAS_GYRO
+__pdata int32_t ao_sample_accel_along_sum;
+__pdata int32_t ao_sample_accel_across_sum;
+__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;
+#endif
 
 static void
 ao_sample_preflight_add(void)
@@ -68,6 +95,14 @@ ao_sample_preflight_add(void)
 	ao_sample_accel_sum += ao_sample_accel;
 #endif
 	ao_sample_pres_sum += ao_sample_pres;
+#if HAS_GYRO
+	ao_sample_accel_along_sum += ao_sample_accel_along;
+	ao_sample_accel_across_sum += ao_sample_accel_across;
+	ao_sample_accel_through_sum += ao_sample_accel_through;
+	ao_sample_pitch_sum += ao_sample_pitch;
+	ao_sample_yaw_sum += ao_sample_yaw;
+	ao_sample_roll_sum += ao_sample_roll;
+#endif
 	++nsamples;
 }
 
@@ -80,8 +115,23 @@ ao_sample_preflight_set(void)
 #endif
 	ao_ground_pres = ao_sample_pres_sum >> 9;
 	ao_ground_height = pres_to_altitude(ao_ground_pres);
-	nsamples = 0;
 	ao_sample_pres_sum = 0;
+#if HAS_GYRO
+	ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
+	ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
+	ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
+	ao_ground_pitch = ao_sample_pitch_sum >> 9;
+	ao_ground_yaw = ao_sample_yaw_sum >> 9;
+	ao_ground_roll = ao_sample_roll_sum >> 9;
+	ao_sample_accel_along_sum = 0;
+	ao_sample_accel_across_sum = 0;
+	ao_sample_accel_through_sum = 0;
+	ao_sample_pitch_sum = 0;
+	ao_sample_yaw_sum = 0;
+	ao_sample_roll_sum = 0;
+	ao_sample_angle = 0;
+#endif	
+	nsamples = 0;
 }
 
 static void
@@ -122,6 +172,24 @@ ao_sample_preflight_update(void)
 		ao_sample_preflight_set();
 }
 
+#if HAS_GYRO
+
+static int32_t	p_filt;
+static int32_t	y_filt;
+
+static gyro_t inline ao_gyro(void) {
+	gyro_t	p = ao_sample_pitch - ao_ground_pitch;
+	gyro_t	y = ao_sample_yaw - ao_ground_yaw;
+
+	p_filt = p_filt - (p_filt >> 6) + p;
+	y_filt = y_filt - (y_filt >> 6) + y;
+
+	p = p_filt >> 6;
+	y = y_filt >> 6;
+	return ao_sqrt(p*p + y*y);
+}
+#endif
+
 uint8_t
 ao_sample(void)
 {
@@ -147,6 +215,12 @@ ao_sample(void)
 			ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
 		ao_data_set_accel(ao_data, ao_sample_accel);
 #endif
+#if HAS_GYRO
+		ao_sample_accel_along = ao_data_along(ao_data);
+		ao_sample_pitch = ao_data_pitch(ao_data);
+		ao_sample_yaw = ao_data_yaw(ao_data);
+		ao_sample_roll = ao_data_roll(ao_data);
+#endif
 
 		if (ao_preflight)
 			ao_sample_preflight();
@@ -154,6 +228,10 @@ ao_sample(void)
 			if (ao_flight_state < ao_flight_boost)
 				ao_sample_preflight_update();
 			ao_kalman();
+#if HAS_GYRO
+			ao_sample_angle += ao_gyro();
+			ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll);
+#endif
 		}
 		ao_sample_data = ao_data_ring_next(ao_sample_data);
 	}
@@ -171,6 +249,21 @@ ao_sample_init(void)
 	ao_sample_accel_sum = 0;
 	ao_sample_accel = 0;
 #endif
+#if HAS_GYRO
+	ao_sample_accel_along_sum = 0;
+	ao_sample_accel_across_sum = 0;
+	ao_sample_accel_through_sum = 0;
+	ao_sample_accel_along = 0;
+	ao_sample_accel_across = 0;
+	ao_sample_accel_through = 0;
+	ao_sample_pitch_sum = 0;
+	ao_sample_yaw_sum = 0;
+	ao_sample_roll_sum = 0;
+	ao_sample_pitch = 0;
+	ao_sample_yaw = 0;
+	ao_sample_roll = 0;
+	ao_sample_angle = 0;
+#endif
 	ao_sample_data = ao_data_head;
 	ao_preflight = TRUE;
 }
diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c
index 09c2e319..3a550eaa 100644
--- a/src/core/ao_sqrt.c
+++ b/src/core/ao_sqrt.c
@@ -15,7 +15,9 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+#ifndef AO_FLIGHT_TEST
 #include "ao.h"
+#endif
 
 /* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */
 /**