diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/test/Makefile | 12 | ||||
| -rw-r--r-- | src/test/ao_flight_test.c | 52 |
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; |