altos: debug invalid state for easymotor

[fw/altos] / src / kernel / ao_flight.c

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 98a4b75faae1318ab6698847d7668c2bd8e5500a..f778b2fbb62896dca526eeafbd83755f3def1dad 100644 (file)
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -201,10 +201,19 @@ ao_flight(void)
 
                        break;
 
-               case ao_flight_idle:
                case ao_flight_invalid:
-                       printf("accel %ld speed %ld\n", ao_accel, ao_speed);
+                       printf("+g %d -g %d ga %d +g-NU %d -g+NU %d\n",
+                              ao_config.accel_plus_g,
+                              ao_config.accel_minus_g,
+                              ao_ground_accel,
+                              ao_config.accel_plus_g - ACCEL_NOSE_UP,
+                              ao_config.accel_minus_g + ACCEL_NOSE_UP);
+                       break;
+#if 0
+               case ao_flight_idle:
+                       printf("+g %d ga %d sa %d accel %ld speed %ld\n", ao_config.accel_plus_g, ao_ground_accel, ao_sample_accel, ao_accel, ao_speed);
                        break;
+#endif
 
                case ao_flight_pad:
                        /* pad to boost:
@@ -222,8 +231,11 @@ ao_flight(void)
                         */
                        if (ao_height > AO_M_TO_HEIGHT(20)
 #if HAS_ACCEL
-                           || (ao_accel > AO_MSS_TO_ACCEL(20) &&
-                               ao_speed > AO_MS_TO_SPEED(5))
+                           || (ao_accel > AO_MSS_TO_ACCEL(20)
+#if HAS_BARO
+                               && ao_speed > AO_MS_TO_SPEED(5)
+#endif
+)
 #endif
                                )
                        {