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

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 2142546c..4a7055c4 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -102,6 +102,10 @@ uint8_t			ao_flight_force_idle;
 
 #define abs(a)	((a) < 0 ? -(a) : (a))
 
+#if !HAS_BARO
+// #define DEBUG_ACCEL_ONLY	1
+#endif
+
 void
 ao_flight(void)
 {
@@ -127,8 +131,8 @@ 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
@@ -200,6 +204,15 @@ ao_flight(void)
 			ao_wakeup(&ao_flight_state);
 
 			break;
+
+#if DEBUG_ACCEL_ONLY
+		case ao_flight_invalid:
+		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 +229,8 @@ 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)
+				&& ao_speed > AO_MS_TO_SPEED(5))
 #endif
 				)
 			{
@@ -448,7 +461,7 @@ ao_flight(void)
 #if HAS_FLIGHT_DEBUG
 		case ao_flight_test:
 #if HAS_GYRO
-			printf ("angle %4d pitch %7d yaw %7d roll %7d\n",
+			printf ("angle %4d pitch %7ld yaw %7ld roll %7ld\n",
 				ao_sample_orient,
 				((ao_sample_pitch << 9) - ao_ground_pitch) >> 9,
 				((ao_sample_yaw << 9) - ao_ground_yaw) >> 9,
@@ -479,14 +492,14 @@ ao_flight_dump(void)
 	printf ("sample:\n");
 	printf ("  tick        %d\n", ao_sample_tick);
 #if HAS_BARO
-	printf ("  raw pres    %d\n", ao_sample_pres);
+	printf ("  raw pres    %ld\n", ao_sample_pres);
 #endif
 #if HAS_ACCEL
 	printf ("  raw accel   %d\n", ao_sample_accel);
 #endif
 #if HAS_BARO
-	printf ("  ground pres %d\n", ao_ground_pres);
-	printf ("  ground alt  %d\n", ao_ground_height);
+	printf ("  ground pres %ld\n", ao_ground_pres);
+	printf ("  ground alt  %ld\n", ao_ground_height);
 #endif
 #if HAS_ACCEL
 	printf ("  raw accel   %d\n", ao_sample_accel);
@@ -495,8 +508,8 @@ ao_flight_dump(void)
 #endif
 
 #if HAS_BARO
-	printf ("  alt         %d\n", ao_sample_alt);
-	printf ("  height      %d\n", ao_sample_height);
+	printf ("  alt         %ld\n", ao_sample_alt);
+	printf ("  height      %ld\n", ao_sample_height);
 #endif
 
 #if HAS_ACCEL
@@ -505,12 +518,12 @@ ao_flight_dump(void)
 
 
 	printf ("kalman:\n");
-	printf ("  height      %d\n", ao_height);
+	printf ("  height      %ld\n", ao_height);
 	printf ("  speed       %d.%02d\n", int_part(ao_speed), frac_part(ao_speed));
 	printf ("  accel       %d.%02d\n", int_part(ao_accel), frac_part(ao_accel));
-	printf ("  max_height  %d\n", ao_max_height);
-	printf ("  avg_height  %d\n", ao_avg_height);
-	printf ("  error_h     %d\n", ao_error_h);
+	printf ("  max_height  %ld\n", ao_max_height);
+	printf ("  avg_height  %ld\n", ao_avg_height);
+	printf ("  error_h     %ld\n", ao_error_h);
 #if !HAS_ACCEL
 	printf ("  error_avg   %d\n", ao_error_h_sq_avg);
 #endif
@@ -530,6 +543,7 @@ static void
 ao_orient_test_select(void)
 {
 	ao_orient_test = !ao_orient_test;
+	printf("orient test %d\n", ao_orient_test);
 }
 
 const struct ao_cmds ao_flight_cmds[] = {