altos/test: Display MPU6000 values in ao_flight_test_mm output

No computation yet, just making the values visible in the output

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

2 files changed, 56 insertions, 8 deletions

diff --git a/src/test/Makefile b/src/test/Makefile
index 37949e86..44cee904 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -3,6 +3,8 @@ vpath % ..:../core:../drivers:../util
 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_convert_test ao_convert_pa_test ao_fec_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 -O0 -g -Wall
@@ -14,19 +16,19 @@ clean:
 
 install:
 
-ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h $(INCS)
 	cc $(CFLAGS) -o $@ $<
 
-ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude-pa.h ao_kalman.h
+ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
 	cc -DNOISY_ACCEL=1 $(CFLAGS) -o $@ $<
 
-ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalman.c $(INCS)
 	cc $(CFLAGS) -o $@ -DHAS_ACCEL=0 ao_flight_test.c
 
-ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c  ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+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 altitude.h ao_kalman.h
+ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
 	cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $<
 
 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 6c0bb4da..0df9a5d7 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -260,6 +260,10 @@ static int	landed_set;
 static double	landed_time;
 static double	landed_height;
 
+#if HAS_MPU6000
+static struct ao_mpu6000_sample	ao_ground_mpu6000;
+#endif
+
 void
 ao_test_exit(void)
 {
@@ -297,6 +301,20 @@ 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(int16_t sensor)
+{
+	return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
+}
+#endif
+
 void
 ao_insert(void)
 {
@@ -350,10 +368,22 @@ ao_insert(void)
 		}
 
 		if (!ao_summary) {
-			printf("%7.2f height %8.2f accel %8.3f 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",
+			printf("%7.2f height %8.2f accel %8.3f "
+#if MEGAMETRUM
+			       "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 MEGAMETRUM
+			       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,
 			       ao_k_speed / 65536.0 / 16.0,
@@ -536,6 +566,7 @@ ao_sleep(void *wchan)
 			if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
 			{
 #if MEGAMETRUM
+				ao_data_static.mpu6000 = ao_ground_mpu6000;
 #else
 				ao_data_static.adc.accel = ao_flight_ground_accel;
 #endif
@@ -585,8 +616,23 @@ ao_sleep(void *wchan)
 					ao_data_static.ms5607_raw.pres = int32(bytes, 0);
 					ao_data_static.ms5607_raw.temp = int32(bytes, 4);
 					ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
-					ao_data_set_accel(&ao_data_static, -int16(bytes, 10));
-//					printf ("accel %d pres %d\n", ao_data_accel_cook(&ao_data_static), value.pres);
+					ao_data_static.mpu6000.accel_x = int16(bytes, 8);
+					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_z = int16(bytes, 18);
+					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);
+					}
 					ao_records_read++;
 					ao_insert();
 					return;