altos: Add orientation tracking to ao_flight_test

[fw/altos] / src / core / ao_quaternion.h

diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h
index 6c8855003f59e9219bae63d7e9bfa45ff58828e0..044f1607d8b4a8aabba54841aaae7da47ec6434a 100644 (file)
--- 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)