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>

1 files changed, 9 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