From afc16e805145c3e9ab4ba948f9ab1d9aa2b27afb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 28 Dec 2013 10:18:53 -0800 Subject: 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 --- src/core/ao_sample.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) (limited to 'src/core/ao_sample.c') 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 -- cgit v1.2.3