altos: More easy motor invalid mode debug

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index f778b2fbb62896dca526eeafbd83755f3def1dad..4b6bfd8f7109be78233e25f13f2aa9317410bbbb 100644 (file)
--- 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,