From: Keith Packard <keithp@keithp.com>
Date: Sat, 28 Dec 2013 18:18:53 +0000 (-0800)
Subject: altos: Add 'O' command for TeleMega orient testing
X-Git-Tag: 1.3.1~26
X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=afc16e805145c3e9ab4ba948f9ab1d9aa2b27afb

altos: Add 'O' command for TeleMega orient testing

Only present when HAS_FLIGHT_DEBUG is enabled, this command lets the
user check the orientation tracking code by showing the current
orientation and when the calibration values are reset.

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

diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index aac6880d..08302140 100644
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -443,9 +443,18 @@ ao_gyro_test(void)
 	ao_flight_state = ao_flight_idle;
 }
 
+uint8_t ao_orient_test;
+
+static void
+ao_orient_test_select(void)
+{
+	ao_orient_test = !ao_orient_test;
+}
+
 __code struct ao_cmds ao_flight_cmds[] = {
 	{ ao_flight_dump, 	"F\0Dump flight status" },
 	{ ao_gyro_test,		"G\0Test gyro code" },
+	{ ao_orient_test_select,"O\0Test orientation code" },
 	{ 0, NULL },
 };
 #endif
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