Некоторая реализация логики перенесена в main

Source/INS/geometry/quaternion.c

@@ -1,6 +1,8 @@
 #include "quaternion.h"
 #include <math.h>
 
+#define PI		3.14159265359f
+
 Quaternion QuatNormalize(const Quaternion* q, const float gain)
 {
   Quaternion res = {};
@@ -134,7 +136,24 @@ Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, boo
 	return error;
 }
 
-
+Vector3 QuatToEuler(const Quaternion* q)
+{
+	Vector3 e;
+
+    e.x = atan2f(2*(q->w*q->x + q->y*q->z),
+                 1 - 2*(q->x*q->x + q->y*q->y));
+
+    e.y = asinf(2*(q->w*q->y - q->z*q->x));
+
+    e.z = atan2f(2*(q->w*q->z + q->x*q->y),
+                 1 - 2*(q->y*q->y + q->z*q->z));
+
+    e.x *= 180.0f / PI;
+    e.y *= 180.0f / PI;
+    e.z *= 180.0f / PI;
+
+    return e;
+}