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 | |
| 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')
| -rw-r--r-- | src/core/ao_flight.c | 9 | ||||
| -rw-r--r-- | src/core/ao_sample.c | 19 |
2 files changed, 28 insertions, 0 deletions
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 |