diff options
| author | Keith Packard <keithp@keithp.com> | 2013-12-28 10:18:53 -0800 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2013-12-28 10:18:53 -0800 |
| commit | afc16e805145c3e9ab4ba948f9ab1d9aa2b27afb (patch) | |
| tree | 3fbd4113f8ae8759037df2e75e5c91a30d9cbae3 /src/core/ao_sample.c | |
| parent | 9877f6b880a0e89fbfbf1a39ded94bdc1891f3d9 (diff) | |
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>
Diffstat (limited to 'src/core/ao_sample.c')
| -rw-r--r-- | src/core/ao_sample.c | 19 |
1 files changed, 19 insertions, 0 deletions
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 |