altos: Update quaternion tests to check vectors_to_rotation

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

diff --git a/src/test/Makefile b/src/test/Makefile
index f64a95318ede3ebee656326f13d90d7769ed64b3..686fde0c7404a063815f85814a15e1a5b7518ba7 100644 (file)
--- 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
 
        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 
 
 
 KALMAN=make-kalman 
 

diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 7f18c80e7d3db0551fbedf219073c85b81a7006e..952a811af2f27e24facc8f5776e40fde0900dfdc 100644 (file)
--- 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 ao_rdf_set(rdf)
 #define ao_packet_slave_start()
 #define ao_packet_slave_stop()

+#define flush()

 
 enum ao_igniter {
        ao_igniter_drogue = 0,
 
 enum ao_igniter {
        ao_igniter_drogue = 0,


@@ -411,7 +412,7 @@ ao_insert(void)
                               height,
                               accel,
 #if TELEMEGA
                               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),
 /*
                               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 0e4b5b071fd01e443e42166c3f4ac4ce9909ae37..b630f1d315a110ba75e45a25218b8a2cfb1be562 100644 (file)
--- a/src/test/ao_quaternion_test.c
+++ b/src/test/ao_quaternion_test.c
@@ -27,41 +27,128 @@
 
 #include "ao_quaternion.h"
 
 
 #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);
 }
 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;
 
 int main(int argc, char **argv)
 {
        struct ao_quaternion    position;

+       struct ao_quaternion    position_expect;

        struct ao_quaternion    rotation;
        struct ao_quaternion    rotation;

+       struct ao_quaternion    rotated;

        struct ao_quaternion    little_rotation;
        int                     i;
        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);
 
 
        /* 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 5f5bd2ca745cba12e56e36a7753b9a207a09a223..bfe15f4caffa4dc2f6f0c89e3d9fe942a7f3900f 100755 (executable)
--- 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)"
 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
 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
 EOF

diff --git a/src/test/run-mm b/src/test/run-mm
index 6f3d97a23e2565c32b5869dd839e8e4a649ed97c..ae5e5f4226252f6e9ea7d6973a72e478189edbc4 100755 (executable)
--- a/src/test/run-mm
+++ b/src/test/run-mm
@@ -3,15 +3,20 @@
 DIR=~/misc/rockets/flights
 
 for i in "$@"; do
 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",\
 
 #./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",\
 #
 
 #"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