altos: Fix up flight code testing

This automates flight code testing by reporting mis-detected apogee or
main events.

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

4 files changed, 131 insertions, 68 deletions

diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c
index e7bfbdd2..51a5965c 100644
--- a/src/ao_flight_test.c
+++ b/src/ao_flight_test.c
@@ -22,6 +22,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <getopt.h>
+#include <math.h>
 
 #define AO_HERTZ	100
 
@@ -87,11 +88,27 @@ enum ao_igniter {
 
 struct ao_adc ao_adc_static;
 
+int	drogue_height;
+double	drogue_time;
+int	main_height;
+double	main_time;
+
+int	tick_offset;
+
+static int32_t	ao_k_height;
+
 void
 ao_ignite(enum ao_igniter igniter)
 {
-	printf ("ignite %s at %7.2f\n", igniter == ao_igniter_drogue ? "drogue" : "main",
-		(double) ao_adc_static.tick / 100.0);
+	double time = (double) (ao_adc_static.tick + tick_offset) / 100;
+
+	if (igniter == ao_igniter_drogue) {
+		drogue_time = time;
+		drogue_height = ao_k_height >> 16;
+	} else {
+		main_time = time;
+		main_height = ao_k_height >> 16;
+	}
 }
 
 struct ao_task {
@@ -108,7 +125,12 @@ struct ao_task {
 
 #define AO_FLIGHT_TEST
 
+int	ao_flight_debug;
+
 FILE *emulator_in;
+char *emulator_app;
+char *emulator_name;
+double emulator_error_max = 10;
 
 void
 ao_dump_state(void);
@@ -157,51 +179,84 @@ struct ao_config ao_config;
 extern int16_t ao_ground_accel, ao_raw_accel;
 extern int16_t ao_accel_2g;
 
-int32_t	drogue_height;
-int32_t main_height;
-
-int		tick_offset;
 uint16_t	prev_tick;
 static int	ao_records_read = 0;
 static int	ao_eof_read = 0;
 static int	ao_flight_ground_accel;
 static int	ao_flight_started = 0;
+static int	ao_test_max_height;
+static double	ao_test_max_height_time;
+static int	ao_test_main_height;
+static double	ao_test_main_height_time;
 
 void
 ao_insert(void)
 {
+	double	time;
+
 	ao_adc_ring[ao_adc_head] = ao_adc_static;
 	ao_adc_head = ao_adc_ring_next(ao_adc_head);
-	if (ao_summary)
-		return;
-	if (ao_flight_state == ao_flight_startup)
-		return;
-	{
+	if (ao_flight_state != ao_flight_startup) {
 		double	height = ao_pres_to_altitude(ao_raw_pres) - ao_ground_height;
 		double  accel = ((ao_flight_ground_accel - ao_adc_static.accel) * GRAVITY * 2.0) /
 			(ao_config.accel_minus_g - ao_config.accel_plus_g);
 
 		if (!tick_offset)
-			tick_offset = ao_adc_static.tick;
-		if (!drogue_height && ao_flight_state >= ao_flight_drogue)
-			drogue_height = ao_k_height;
-		if (!main_height && ao_flight_state >= ao_flight_main)
-			main_height = ao_k_height;
-		if ((prev_tick - ao_adc_static.tick) > 0)
+			tick_offset = -ao_adc_static.tick;
+		if ((prev_tick - ao_adc_static.tick) > 0x400)
 			tick_offset += 65536;
 		prev_tick = ao_adc_static.tick;
-		printf("%7.2f height %g accel %g state %s k_height %g k_speed %g k_accel %g drogue %g main %g error %d\n",
-		       (double) (ao_adc_static.tick + tick_offset) / 100,
-		       height,
-		       accel,
-		       ao_state_names[ao_flight_state],
-		       ao_k_height / 65536.0,
-		       ao_k_speed / 65536.0 / 16.0,
-		       ao_k_accel / 65536.0 / 16.0,
-		       drogue_height / 65536.0,
-		       main_height / 65536.0,
-		       ao_error_h_sq_avg);
+		time = (double) (ao_adc_static.tick + tick_offset) / 100;
+		if (!ao_summary) {
+			printf("%7.2f height %g accel %g state %s k_height %g k_speed %g k_accel %g drogue %d main %d error %d\n",
+			       time,
+			       height,
+			       accel,
+			       ao_state_names[ao_flight_state],
+			       ao_k_height / 65536.0,
+			       ao_k_speed / 65536.0 / 16.0,
+			       ao_k_accel / 65536.0 / 16.0,
+			       drogue_height,
+			       main_height,
+			       ao_error_h_sq_avg);
+		}
+
+		if (ao_test_max_height < height) {
+			ao_test_max_height = height;
+			ao_test_max_height_time = time;
+		}
+		if (height > ao_config.main_deploy) {
+			ao_test_main_height_time = time;
+			ao_test_main_height = height;
+		}
+	}
+}
+
+void
+ao_test_exit(void)
+{
+	double	drogue_error;
+	double	main_error;
+
+	if (!ao_test_main_height_time) {
+		ao_test_main_height_time = ao_test_max_height_time;
+		ao_test_main_height = ao_test_max_height;
+	}
+	drogue_error = fabs(ao_test_max_height_time - drogue_time);
+	main_error = fabs(ao_test_main_height_time - main_time);
+	if (drogue_error > emulator_error_max || main_error > emulator_error_max) {
+		printf ("%s %s\n",
+			emulator_app, emulator_name);
+		printf ("\tApogee error %g\n", drogue_error);
+		printf ("\tMain error %g\n", main_error);
+		printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f\n",
+			ao_test_max_height, ao_test_max_height_time,
+			ao_test_main_height, ao_test_main_height_time);
+		printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f\n",
+			drogue_height, drogue_time, main_height, main_time);
+		exit (1);
 	}
+	exit(0);
 }
 
 void
@@ -228,8 +283,9 @@ ao_sleep(void *wchan)
 
 			if (!fgets(line, sizeof (line), emulator_in)) {
 				if (++ao_eof_read >= 1000) {
-					printf ("no more data, exiting simulation\n");
-					exit(0);
+					if (!ao_summary)
+						printf ("no more data, exiting simulation\n");
+					ao_test_exit();
 				}
 				ao_adc_static.tick += 10;
 				ao_insert();
@@ -247,10 +303,10 @@ ao_sleep(void *wchan)
 				tick = strtoul(words[1], NULL, 16);
 				a = strtoul(words[2], NULL, 16);
 				b = strtoul(words[3], NULL, 16);
-			} else if (nword >= 6 && strcmp(words[0], "Accel")) {
+			} 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]);
-			} else if (nword >= 4 && strcmp(words[0], "Main")) {
+			} else if (nword >= 4 && strcmp(words[0], "Main") == 0) {
 				ao_config.main_deploy = atoi(words[2]);
 			} else if (nword >= 36 && strcmp(words[0], "CALL") == 0) {
 				tick = atoi(words[10]);
@@ -308,21 +364,17 @@ ao_sleep(void *wchan)
 void
 ao_dump_state(void)
 {
-	if (ao_flight_state == ao_flight_startup)
-		return;
-	if (ao_summary)
-		return;
-	if (ao_flight_state == ao_flight_landed)
-		exit(0);
 }
 
 static const struct option options[] = {
 	{ .name = "summary", .has_arg = 0, .val = 's' },
+	{ .name = "debug", .has_arg = 0, .val = 'd' },
 	{ 0, 0, 0, 0},
 };
 
 void run_flight_fixed(char *name, FILE *f, int summary)
 {
+	emulator_name = name;
 	emulator_in = f;
 	ao_summary = summary;
 	ao_flight_init();
@@ -336,11 +388,19 @@ main (int argc, char **argv)
 	int	c;
 	int	i;
 
-	while ((c = getopt_long(argc, argv, "s", options, NULL)) != -1) {
+#if HAS_ACCEL
+	emulator_app="full";
+#else
+	emulator_app="baro";
+#endif
+	while ((c = getopt_long(argc, argv, "sd", options, NULL)) != -1) {
 		switch (c) {
 		case 's':
 			summary = 1;
 			break;
+		case 'd':
+			ao_flight_debug = 1;
+			break;
 		}
 	}
 
diff --git a/src/test/run-one b/src/test/run-one
index f9d21576..d661abec 100755
--- a/src/test/run-one
+++ b/src/test/run-one
@@ -1,17 +1,24 @@
 #!/bin/sh
 
-./ao_flight_test "$1" > run-out.full
-./ao_flight_test_baro "$1" > run-out.baro
-./ao_flight_test_accel "$1" > run-out.accel
+for i in "$@"; do
+./ao_flight_test "$i" > run-out.full
+./ao_flight_test_baro "$i" > run-out.baro
+#./ao_flight_test_accel "$i" > run-out.accel
 
-gnuplot -persist << EOF
+#"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\
+#"run-out.accel" using 1:11 with lines lt 4 axes x1y2 title "accel speed",\
+#"run-out.accel" using 1:13 with lines lt 4 axes x1y2 title "accel accel",\
+#"run-out.accel" using 1:15 with lines lt 4 axes x1y1 title "accel drogue",\
+#"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 "$1"
+set title "$i"
 plot "run-out.full" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
 "run-out.full" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
 "run-out.full" using 1:9 with lines lt 2 axes x1y1 title "full height",\
@@ -23,10 +30,7 @@ plot "run-out.full" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",
 "run-out.baro" using 1:11 with lines lt 3 axes x1y2 title "baro speed",\
 "run-out.baro" using 1:13 with lines lt 3 axes x1y2 title "baro accel",\
 "run-out.baro" using 1:15 with lines lt 3 axes x1y1 title "baro drogue",\
-"run-out.baro" using 1:17 with lines lt 3 axes x1y1 title "baro main",\
-"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\
-"run-out.accel" using 1:11 with lines lt 4 axes x1y2 title "accel speed",\
-"run-out.accel" using 1:13 with lines lt 4 axes x1y2 title "accel accel",\
-"run-out.accel" using 1:15 with lines lt 4 axes x1y1 title "accel drogue",\
-"run-out.accel" using 1:17 with lines lt 4 axes x1y1 title "accel main"
+"run-out.baro" using 1:17 with lines lt 3 axes x1y1 title "baro main"
+pause mouse close
 EOF
+done
\ No newline at end of file
diff --git a/src/test/run-tests b/src/test/run-tests
index ec279776..11b4c95c 100755
--- a/src/test/run-tests
+++ b/src/test/run-tests
@@ -2,21 +2,21 @@
 
 DIR=~/src/cc1111/flights
 
+bad_baro=0
+bad_full=0
 while read flight; do
-	baro=`./ao_flight_test_baro -s $DIR/$flight |
-		awk '/drogue/ {	printf "%s ", $4 }
-		/main/ { printf "%s\n", $4 }'`
-	full=`./ao_flight_test -s $DIR/$flight |
-		awk '/drogue/ {	printf "%s ", $4 }
-		/main/ { printf "%s\n", $4 }'`
-	echo $flight $baro $full
-done < test-flights |
-awk '{ name = $1;
-	drogue_error = $2 - $4;
-	if (drogue_error < 0) drogue_error = -drogue_error;
-	main_error = $3 - $5;
-	if (main_error < 0) main_error = -main_error;
-	if (drogue_error > 4 || main_error > 4)
-		printf ("%s: baro drogue %f main %f. full drogue %f main %f\n",
-			name, $2, $3, $4, $5);
-	}'
+    if ./ao_flight_test_baro -s $DIR/$flight; then
+	:
+    else
+	((bad_baro++))
+    fi
+    if ./ao_flight_test -s $DIR/$flight; then
+	:
+    else
+	((bad_full++))
+    fi
+done < test-flights
+echo baro errors $bad_baro
+echo full errors $bad_full
+((bad = bad_baro + bad_full))
+exit $bad
\ No newline at end of file
diff --git a/src/test/test-flights b/src/test/test-flights
index afdaba5a..0b90d9e0 100644
--- a/src/test/test-flights
+++ b/src/test/test-flights
@@ -63,7 +63,6 @@
 2010-09-24-serial-236-flight-006.eeprom
 2010-09-25-serial-223-flight-001.eeprom
 2010-10-17-serial-215-flight-006.eeprom
-2010-10-23-serial-236-flight-008.eeprom
 2011-01-30-serial-056-flight-001.eeprom
 2011-01-30-serial-250-flight-002.eeprom
 2011-02-19-serial-215-flight-007.eeprom