X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao_flight.c;h=f4b5279b64cf1edecc57d88986c5ebb2df89807a;hp=37c138fe1586c1e1a6e32b4e2d3e8b826a09f79b;hb=e9584e846b9bd7926d61451d32ba5d7a30416f7b;hpb=5ed3b1cb52b573db1fee9655a29a0e6dd72f53fe

diff --git a/ao_flight.c b/ao_flight.c
index 37c138fe..f4b5279b 100644
--- a/ao_flight.c
+++ b/ao_flight.c
@@ -136,16 +136,12 @@ __xdata int32_t ao_raw_accel_sum, ao_raw_pres_sum;
 void
 ao_flight(void)
 {
-	__pdata static uint8_t	nsamples = 0;
+	__pdata static uint16_t	nsamples = 0;
 
 	ao_flight_adc = ao_adc_head;
 	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,34 +189,15 @@ 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:
 
 			/* startup state:
 			 *
-			 * Collect 100 samples of acceleration and pressure
+			 * Collect 1000 samples of acceleration and pressure
 			 * data and average them to find the resting values
 			 */
-			if (nsamples < 100) {
+			if (nsamples < 1000) {
 				ao_raw_accel_sum += ao_raw_accel;
 				ao_raw_pres_sum += ao_raw_pres;
 				++nsamples;
@@ -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) {
@@ -248,7 +223,7 @@ ao_flight(void)
 				/* Turn on telemetry system
 				 */
 				ao_rdf_set(1);
-				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT);
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
 
 				ao_flight_state = ao_flight_launchpad;
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
@@ -284,6 +259,9 @@ ao_flight(void)
 				/* start logging data */
 				ao_log_start();
 
+				/* Increase telemetry rate */
+				ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT);
+
 				/* disable RDF beacon */
 				ao_rdf_set(0);
 
@@ -372,6 +350,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 +385,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 +395,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;