altos: Add orientation tracking to ao_flight_test

Shows calculated offset from vertical in ao_flight_test output

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

1 files changed, 9 insertions, 0 deletions

diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h
index 6c885500..044f1607 100644
--- a/src/core/ao_quaternion.h
+++ b/src/core/ao_quaternion.h
@@ -109,6 +109,15 @@ static inline void ao_quaternion_normalize(struct ao_quaternion *r,
 		*r = *a;
 }
 
+static inline float ao_quaternion_dot(const struct ao_quaternion *a,
+				      const struct ao_quaternion *b)
+{
+#define T(_a)	(((a)->_a) * ((b)->_a))
+	return T(r) + T(x) + T(y) + T(z);
+#undef T
+}
+				     
+
 static inline void ao_quaternion_rotate(struct ao_quaternion *r,
 					const struct ao_quaternion *a,
 					const struct ao_quaternion *b)