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>

diff --git a/src/test/Makefile b/src/test/Makefile
index 37949e86aa127fb5ac33f1fd20551ebbc01e72ff..44cee904ab642ee24d386fe37464966e6660a25f 100644 (file)
--- 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
 
 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
 KALMAN=make-kalman 
 
 CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall


@@ -14,19 +16,19 @@ clean:
 
 install:
 
 
 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 $@ $<
 
        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 $@ $<
 
        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
 
        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
 
        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
        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 6c0bb4da944f2c8565c98051ff7df8e02bb1ffee..0df9a5d79c0b9df7ca0a73be546f3029d59c1a82 100644 (file)
--- 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;
 
 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)
 {
 void
 ao_test_exit(void)
 {


@@ -297,6 +301,20 @@ ao_test_exit(void)
        exit(0);
 }
 
        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)
 {
 void
 ao_insert(void)
 {


@@ -350,10 +368,22 @@ ao_insert(void)
                }
 
                if (!ao_summary) {
                }
 
                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,
                               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,
                               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
                        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
 #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_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;
                                        ao_records_read++;
                                        ao_insert();
                                        return;