#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;
}
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",\
#"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
projects / fw / altos / commitdiff