From 0136cef90ef9ac2a7325df588d12db3883f509d0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 10 Oct 2020 22:48:31 -0700 Subject: [PATCH] altos: More easy motor invalid mode debug Signed-off-by: Keith Packard --- src/kernel/ao_flight.c | 25 ++++++++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index f778b2fb..4b6bfd8f 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -102,6 +102,13 @@ uint8_t ao_flight_force_idle; #define abs(a) ((a) < 0 ? -(a) : (a)) +static bool accel_plus_g_failed; +static bool accel_minus_g_failed; +static bool accel_plus_failed; +static bool accel_minus_failed; + +static char *btos(bool x) { return x? "true" : "false"; } + void ao_flight(void) { @@ -127,14 +134,22 @@ ao_flight(void) #if HAS_ACCEL if (ao_config.accel_plus_g == 0 || ao_config.accel_minus_g == 0 || - ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || - ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP + ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP || + ao_ground_accel > (accel_t) ao_config.accel_minus_g + ACCEL_NOSE_UP #if HAS_BARO || ao_ground_height < -1000 || ao_ground_height > 7000 #endif ) { + if (ao_config.accel_plus_g == 0) + accel_plus_g_failed = true; + if (ao_config.accel_minus_g == 0) + accel_minus_g_failed = true; + if (ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP) + accel_plus_failed = true; + if (ao_ground_accel > (accel_t) ao_config.accel_minus_g + ACCEL_NOSE_UP) + accel_minus_failed = true; /* Detected an accel value outside -1.5g to 1.5g * (or uncalibrated values), so we go into invalid mode */ @@ -202,7 +217,11 @@ ao_flight(void) break; case ao_flight_invalid: - printf("+g %d -g %d ga %d +g-NU %d -g+NU %d\n", + printf("+g? %s -g? %s +? %s -? %s +g %d -g %d ga %d +g-NU %d -g+NU %d\n", + btos(accel_plus_g_failed), + btos(accel_minus_g_failed), + btos(accel_plus_failed), + btos(accel_minus_failed), ao_config.accel_plus_g, ao_config.accel_minus_g, ao_ground_accel, -- 2.30.2