X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_flight.c;h=3c1067cb44e9e079675d863a2eb38368e096e733;hb=f10009b07b651f69014ac5608f3ca29bce874c24;hp=c5069158a61cc603914566a4917608a54a300be3;hpb=d779d8e5b1106aaec6170761e6f5bd4e8d5ac6e7;p=fw%2Faltos

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index c5069158..3c1067cb 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,12 +229,14 @@ 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
 				)
 			{
 				ao_flight_state = ao_flight_boost;
+				ao_wakeup(&ao_flight_state);
+
 				ao_launch_tick = ao_boost_tick = ao_sample_tick;
 
 				/* start logging data */
@@ -242,8 +257,6 @@ ao_flight(void)
 				ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING;
 				ao_wakeup(&ao_gps_new);
 #endif
-
-				ao_wakeup(&ao_flight_state);
 			}
 			break;
 		case ao_flight_boost:
@@ -278,8 +291,8 @@ ao_flight(void)
 #else
 				ao_flight_state = ao_flight_coast;
 #endif
-				++ao_motor_number;
 				ao_wakeup(&ao_flight_state);
+				++ao_motor_number;
 			}
 			break;
 #if HAS_ACCEL && HAS_BARO
@@ -327,11 +340,9 @@ ao_flight(void)
 #endif
 				)
 			{
-#if HAS_IGNITE
-				/* ignite the drogue charge */
-				ao_ignite(ao_igniter_drogue);
-#endif
-
+				/* enter drogue state */
+				ao_flight_state = ao_flight_drogue;
+				ao_wakeup(&ao_flight_state);
 #if HAS_TELEMETRY
 				/* slow down the telemetry system */
 				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER);
@@ -339,10 +350,6 @@ ao_flight(void)
 				/* Turn the RDF beacon back on */
 				ao_rdf_set(1);
 #endif
-
-				/* and enter drogue state */
-				ao_flight_state = ao_flight_drogue;
-				ao_wakeup(&ao_flight_state);
 			}
 			else
 #else /* not HAS_BARO */
@@ -362,12 +369,11 @@ ao_flight(void)
 				    ao_interval_max_accel_through - ao_interval_min_accel_through <= ao_data_accel_to_sample(MAX_QUIET_ACCEL))
 				{
 					ao_flight_state = ao_flight_landed;
+					ao_wakeup(&ao_flight_state);
 #if HAS_ADC
 					/* turn off the ADC capture */
 					ao_timer_set_adc_interval(0);
 #endif
-
-					ao_wakeup(&ao_flight_state);
 				}
 
 				/* Reset interval values */
@@ -410,9 +416,8 @@ ao_flight(void)
 			 */
 			if (ao_height <= ao_config.main_deploy)
 			{
-#if HAS_IGNITE
-				ao_ignite(ao_igniter_main);
-#endif
+				ao_flight_state = ao_flight_main;
+				ao_wakeup(&ao_flight_state);
 
 				/*
 				 * Start recording min/max height
@@ -423,9 +428,6 @@ ao_flight(void)
 				ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
 
 				ao_interval_min_height = ao_interval_max_height = ao_avg_height;
-
-				ao_flight_state = ao_flight_main;
-				ao_wakeup(&ao_flight_state);
 			}
 			break;
 
@@ -445,13 +447,11 @@ ao_flight(void)
 				if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4))
 				{
 					ao_flight_state = ao_flight_landed;
-
+					ao_wakeup(&ao_flight_state);
 #if HAS_ADC
 					/* turn off the ADC capture */
 					ao_timer_set_adc_interval(0);
 #endif
-
-					ao_wakeup(&ao_flight_state);
 				}
 				ao_interval_min_height = ao_interval_max_height = ao_avg_height;
 				ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;