X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=4b6bfd8f7109be78233e25f13f2aa9317410bbbb;hb=0136cef90ef9ac2a7325df588d12db3883f509d0;hp=2142546c9b0737b186cca6f6dce65fa51c8e30ec;hpb=d0c0dec140b8fa847704c9a22a39d254445188b9;p=fw%2Faltos

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 2142546c..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
 				 */
@@ -200,6 +215,25 @@ ao_flight(void)
 			ao_wakeup(&ao_flight_state);
 
 			break;
+
+		case ao_flight_invalid:
+			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,
+			       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:
 			 *
@@ -216,8 +250,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
 				)
 			{