X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fcore%2Fao_sample.c;h=34658951724e7166dbc3008a9079a2f7a1df8e59;hp=adf8399dd702535d960804791669cde9f204a812;hb=afc16e805145c3e9ab4ba948f9ab1d9aa2b27afb;hpb=9877f6b880a0e89fbfbf1a39ded94bdc1891f3d9

diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index adf8399d..34658951 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -92,6 +92,10 @@ __pdata int32_t	ao_sample_roll_sum;
 static struct ao_quaternion ao_rotation;
 #endif
 
+#if HAS_FLIGHT_DEBUG
+extern uint8_t ao_orient_test;
+#endif
+
 static void
 ao_sample_preflight_add(void)
 {
@@ -159,7 +163,11 @@ ao_sample_preflight_set(void)
 	 * that as the current rotation vector
 	 */
 	ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient);
+#if HAS_FLIGHT_DEBUG
+	if (ao_orient_test)
+		printf("\n\treset\n");
 #endif	
+#endif
 	nsamples = 0;
 }
 
@@ -210,6 +218,17 @@ ao_sample_rotate(void)
 	rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r;
 
 	ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI);
+
+#if HAS_FLIGHT_DEBUG
+	if (ao_orient_test) {
+		printf ("rot %d %d %d orient %d     \r",
+			(int) (x * 1000),
+			(int) (y * 1000),
+			(int) (z * 1000),
+			ao_sample_orient);
+	}
+#endif
+
 }
 #endif