diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/test/Makefile | 2 | ||||
| -rw-r--r-- | src/test/ao_flight_test.c | 3 | ||||
| -rw-r--r-- | src/test/ao_quaternion_test.c | 121 | ||||
| -rwxr-xr-x | src/test/plotmm | 22 | ||||
| -rwxr-xr-x | src/test/run-mm | 59 |
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 |