X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao_flight.c;h=06d4ba3d6891f1fc99ecbf0c555cf09df8bea711;hp=37c138fe1586c1e1a6e32b4e2d3e8b826a09f79b;hb=75ca1751b7cac2f8074d0713ee96d6ab45b54f19;hpb=7a1b77c2d7253a681389f32b70e2460aac188807

diff --git a/ao_flight.c b/ao_flight.c
index 37c138fe..06d4ba3d 100644
--- a/ao_flight.c
+++ b/ao_flight.c
@@ -142,10 +142,6 @@ ao_flight(void)
 	ao_raw_accel_prev = 0;
 	ao_raw_accel = 0;
 	ao_raw_pres = 0;
-	ao_interval_cur_min_pres = 0x7fff;
-	ao_interval_cur_max_pres = -0x7fff;
-	ao_interval_cur_min_accel = 0x7fff;
-	ao_interval_cur_max_accel = -0x7fff;
 	ao_flight_tick = 0;
 	for (;;) {
 		ao_sleep(&ao_adc_ring);
@@ -193,25 +189,6 @@ ao_flight(void)
 			    ao_min_vel = -ao_flight_vel;
 		}
 
-		if (ao_flight_pres < ao_interval_cur_min_pres)
-			ao_interval_cur_min_pres = ao_flight_pres;
-		if (ao_flight_pres > ao_interval_cur_max_pres)
-			ao_interval_cur_max_pres = ao_flight_pres;
-		if (ao_flight_accel < ao_interval_cur_min_accel)
-			ao_interval_cur_min_accel = ao_flight_accel;
-		if (ao_flight_accel > ao_interval_cur_max_accel)
-			ao_interval_cur_max_accel = ao_flight_accel;
-
-		if ((int16_t) (ao_flight_tick - ao_interval_end) >= 0) {
-			ao_interval_max_pres = ao_interval_cur_max_pres;
-			ao_interval_min_pres = ao_interval_cur_min_pres;
-			ao_interval_max_accel = ao_interval_cur_max_accel;
-			ao_interval_min_accel = ao_interval_cur_min_accel;
-			ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS;
-			ao_interval_cur_min_pres = ao_interval_cur_max_pres = ao_flight_pres;
-			ao_interval_cur_min_accel = ao_interval_cur_max_accel = ao_flight_accel;
-		}
-
 		switch (ao_flight_state) {
 		case ao_flight_startup:
 
@@ -234,8 +211,6 @@ ao_flight(void)
 			ao_flight_vel = 0;
 			ao_min_vel = 0;
 
-			ao_interval_end = ao_flight_tick;
-
 			/* Go to launchpad state if the nose is pointing up */
 			ao_config_get();
 			if (ao_flight_accel < ao_config.accel_zero_g - ACCEL_NOSE_UP) {
@@ -372,6 +347,24 @@ ao_flight(void)
 				ao_flight_state = ao_flight_drogue;
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
 			}
+			/*
+			 * Start recording min/max accel and pres for a while
+			 * to figure out when the rocket has landed
+			 */
+			/* Set the 'last' limits to max range to prevent
+			 * early resting detection
+			 */
+			ao_interval_min_accel = 0;
+			ao_interval_max_accel = 0x7fff;
+			ao_interval_min_pres = 0;
+			ao_interval_max_pres = 0x7fff;
+
+			/* initialize interval values */
+			ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS;
+
+			ao_interval_cur_min_pres = ao_interval_cur_max_pres = ao_flight_pres;
+			ao_interval_cur_min_accel = ao_interval_cur_max_accel = ao_flight_accel;
+
 			break;
 		case ao_flight_drogue:
 
@@ -389,6 +382,7 @@ ao_flight(void)
 				ao_flight_state = ao_flight_main;
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
 			}
+
 			/* fall through... */
 		case ao_flight_main:
 
@@ -398,10 +392,30 @@ ao_flight(void)
 			 *                           OR
 			 * barometer: altitude stable and within 1000m of the launch altitude
 			 */
+
+			if (ao_flight_pres < ao_interval_cur_min_pres)
+				ao_interval_cur_min_pres = ao_flight_pres;
+			if (ao_flight_pres > ao_interval_cur_max_pres)
+				ao_interval_cur_max_pres = ao_flight_pres;
+			if (ao_flight_accel < ao_interval_cur_min_accel)
+				ao_interval_cur_min_accel = ao_flight_accel;
+			if (ao_flight_accel > ao_interval_cur_max_accel)
+				ao_interval_cur_max_accel = ao_flight_accel;
+
+			if ((int16_t) (ao_flight_tick - ao_interval_end) >= 0) {
+				ao_interval_max_pres = ao_interval_cur_max_pres;
+				ao_interval_min_pres = ao_interval_cur_min_pres;
+				ao_interval_max_accel = ao_interval_cur_max_accel;
+				ao_interval_min_accel = ao_interval_cur_min_accel;
+				ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS;
+				ao_interval_cur_min_pres = ao_interval_cur_max_pres = ao_flight_pres;
+				ao_interval_cur_min_accel = ao_interval_cur_max_accel = ao_flight_accel;
+			}
+
 			if ((abs(ao_flight_vel) < ACCEL_VEL_LAND &&
-			     (ao_interval_max_accel - ao_interval_min_accel) < ACCEL_INT_LAND) ||
+			     (uint16_t) (ao_interval_max_accel - ao_interval_min_accel) < (uint16_t) ACCEL_INT_LAND) ||
 			    (ao_flight_pres > ao_ground_pres - BARO_LAND &&
-			     (ao_interval_max_pres - ao_interval_min_pres) < BARO_INT_LAND))
+			     (uint16_t) (ao_interval_max_pres - ao_interval_min_pres) < (uint16_t) BARO_INT_LAND))
 			{
 				ao_flight_state = ao_flight_landed;