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 aac6880db1260d6dea90a4ce47cd5319e6e1d0f4..08302140650b4c57456a5265e2ad2c1468a388aa 100644 (file)
--- 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 adf8399dd702535d960804791669cde9f204a812..34658951724e7166dbc3008a9079a2f7a1df8e59 100644 (file)
--- 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