altos: Update quaternion tests to check vectors_to_rotation

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

5 files changed, 158 insertions, 49 deletions

diff --git a/src/test/Makefile b/src/test/Makefile
index f64a9531..686fde0c 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -5,7 +5,7 @@ PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noi
 	ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \
 	ao_ms5607_convert_test ao_quaternion_test
 
-INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h
+INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h ao_quaternion.h
 
 KALMAN=make-kalman 
 
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 7f18c80e..952a811a 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -128,6 +128,7 @@ int	ao_summary = 0;
 #define ao_rdf_set(rdf)
 #define ao_packet_slave_start()
 #define ao_packet_slave_stop()
+#define flush()
 
 enum ao_igniter {
 	ao_igniter_drogue = 0,
@@ -411,7 +412,7 @@ ao_insert(void)
 			       height,
 			       accel,
 #if TELEMEGA
-			       ao_orient,
+			       ao_sample_orient,
 /*
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 			       ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c
index 0e4b5b07..b630f1d3 100644
--- a/src/test/ao_quaternion_test.c
+++ b/src/test/ao_quaternion_test.c
@@ -27,41 +27,128 @@
 
 #include "ao_quaternion.h"
 
+#if 0
 static void
 print_q(char *name, struct ao_quaternion *q)
 {
 	printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name,
 		q->r, q->x, q->y, q->z);
 }
+#endif
+
+#define STEPS	16
+
+#define DEG	(1.0f * 3.1415926535f / 180.0f)
+
+struct ao_rotation {
+	int	steps;
+	float	x, y, z;
+};
+
+static struct ao_rotation ao_path[] = {
+	{ .steps = 45, .x =  2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = 2*DEG, .z = 0 },
+	
+	{ .steps = 45, .x = -2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = -2*DEG, .z = 0 },
+};
+
+#define NUM_PATH	(sizeof ao_path / sizeof ao_path[0])
+
+static int close(float a, float b) {
+	return fabsf (a - b) < 1e-5;
+}
+
+static int check_quaternion(char *where, struct ao_quaternion *got, struct ao_quaternion *expect) {
+	if (!close (got->r, expect->r) ||
+	    !close (got->x, expect->x) ||
+	    !close (got->y, expect->y) ||
+	    !close (got->z, expect->z))
+	{
+		printf ("%s: got r%8.5f x%8.5f y%8.5f z%8.5f expect r%8.5f x%8.5f y%8.5f z%8.5f\n",
+			where,
+			got->r, got->x, got->y, got->z,
+			expect->r, expect->x, expect->y, expect->z);
+		return 1;
+	}
+	return 0;
+}
 
 int main(int argc, char **argv)
 {
 	struct ao_quaternion	position;
+	struct ao_quaternion 	position_expect;
 	struct ao_quaternion	rotation;
+	struct ao_quaternion	rotated;
 	struct ao_quaternion	little_rotation;
 	int			i;
+	int			p;
+	int			ret = 0;
 
-	/* unit x vector */
-	ao_quaternion_init_vector(&position, 1, 0, 0);
+	/* vector */
+	ao_quaternion_init_vector(&position, 1, 1, 1);
+	ao_quaternion_init_vector(&position_expect, -1, -1, 1);
 
 	/* zero rotation */
 	ao_quaternion_init_zero_rotation(&rotation);
 
-	/* π/16 rotation around Z axis */
-	ao_quaternion_init_rotation(&little_rotation, 0, 0, 1,
-				    sin((M_PI/16)/2),
-				    cos((M_PI/16)/2));
-	for (i = 0; i <= 16; i++) {
-		struct ao_quaternion	rotated;
-
-		ao_quaternion_rotate(&rotated, &position, &rotation);
-		print_q("position", &position);
-		print_q("rotated", &rotated);
-		print_q("rotation", &rotation);
-		printf ("\n");
-		ao_quaternion_multiply(&rotation, &rotation, &little_rotation);
-		ao_quaternion_normalize(&rotation, &rotation);
+#define dump() do {							\
+									\
+		ao_quaternion_rotate(&rotated, &position, &rotation);	\
+		print_q("rotated", &rotated);				\
+		print_q("rotation", &rotation);				\
+		printf ("\n");						\
+	} while (0)
+
+//	dump();
+
+	for (p = 0; p < NUM_PATH; p++) {
+		ao_quaternion_init_half_euler(&little_rotation,
+					      ao_path[p].x / 2.0f,
+					      ao_path[p].y / 2.0f,
+					      ao_path[p].z / 2.0f);
+//		printf ("\t\tx: %8.4f, y: %8.4f, z: %8.4f ", ao_path[p].x, ao_path[p].y, ao_path[p].z);
+//		print_q("step", &little_rotation);
+//		printf("\n");
+		for (i = 0; i < ao_path[p].steps; i++) {
+			ao_quaternion_multiply(&rotation, &little_rotation, &rotation);
+
+			ao_quaternion_normalize(&rotation, &rotation);
+
+//			dump();
+		}
 	}
-	return 0;
+
+	ao_quaternion_rotate(&rotated, &position, &rotation);
+
+	ret += check_quaternion("rotation", &rotated, &position_expect);
+
+	struct ao_quaternion	vertical;
+	struct ao_quaternion	angle;
+	struct ao_quaternion	rot;
+
+	ao_quaternion_init_vector(&vertical, 0, 0, 1);
+	ao_quaternion_init_vector(&angle, 0, 0, 1);
+
+	ao_quaternion_init_half_euler(&rot,
+				      M_PI * 3.0 / 8.0 , 0, 0);
+
+	ao_quaternion_rotate(&angle, &angle, &rot);
+
+	struct ao_quaternion	rot_compute;
+
+	ao_quaternion_vectors_to_rotation(&rot_compute, &vertical, &angle);
+
+	ret += check_quaternion("vector rotation", &rot_compute, &rot);
+
+	struct ao_quaternion	rotd;
+
+	ao_quaternion_rotate(&rotd, &vertical, &rot_compute);
+
+	ret += check_quaternion("vector rotated", &rotd, &angle);
+
+	return ret;
 }
 
diff --git a/src/test/plotmm b/src/test/plotmm
index 5f5bd2ca..bfe15f4c 100755
--- a/src/test/plotmm
+++ b/src/test/plotmm
@@ -1,3 +1,19 @@
+#!/bin/sh
+
+case $# in
+1)
+	file="$1"
+	title="$1"
+	;;
+2)
+	file="$1"
+	title="$2"
+	;;
+*)
+	echo "Usage: $0 <data-file> <title>"
+	exit 1
+esac
+
 gnuplot -persist << EOF
 set ylabel "altitude (m)"
 set y2label "angle (d)"
@@ -5,7 +21,7 @@ set xlabel "time (s)"
 set xtics border out nomirror
 set ytics border out nomirror
 set y2tics border out nomirror
-plot "$1" using 1:3 with lines axes x1y1 title "raw height",\
-"$1" using 1:9 with lines axes x1y2 title "angle",\
-"$1" using 1:11 with lines axes x1y2 title "qangle"
+set title "$title"
+plot "$file" using 1:3 with lines axes x1y1 title "raw height",\
+"$file" using 1:7 with lines axes x1y2 title "angle"
 EOF
diff --git a/src/test/run-mm b/src/test/run-mm
index 6f3d97a2..ae5e5f42 100755
--- a/src/test/run-mm
+++ b/src/test/run-mm
@@ -3,15 +3,20 @@
 DIR=~/misc/rockets/flights
 
 for i in "$@"; do
-case "$i" in
-    */*)
-    file="$i"
-    ;;
-    *)
-    file="$DIR/$i"
-    ;;
-esac
-./ao_flight_test_mm "$file" > run-out.mm
+    case "$i" in
+	*/*)
+	file="$i"
+	;;
+	*)
+	file="$DIR/$i"
+	;;
+    esac
+    base=`basename "$i" .eeprom`
+
+    ./ao_flight_test_mm "$file" > $base.plot
+
+    sh ./plotmm $base.plot `basename "$file"`
+done
 
 #./ao_flight_test_accel "$file" > run-out.accel
 #"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\
@@ -21,21 +26,21 @@ esac
 #"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 "$i"
-plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
-"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
-"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
-"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
-"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
-"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
-"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
-pause mouse close
-EOF
-done
\ No newline at end of file
+#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 "$i"
+#plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
+#"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
+#"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
+#"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
+#"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
+#"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
+#"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
+#pause mouse close
+#EOF
+#done
\ No newline at end of file