X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao_flight.c;h=bf20f3570e4691f57d08a786fa5262c0c35ffe39;hp=f31b4cd4e617af11e0bbe9237954fba8cf0c85f6;hb=3d5a5fc4db5f681e848202c4ee4099d2879677d6;hpb=acc4fc635edb70ec1ba2dff9f7ac0c8542c72c47

diff --git a/ao_flight.c b/ao_flight.c
index f31b4cd4..bf20f357 100644
--- a/ao_flight.c
+++ b/ao_flight.c
@@ -3,8 +3,7 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
+ * the Free Software Foundation; version 2 of the License.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -20,15 +19,32 @@
 
 /* Main flight thread. */
 
-__xdata struct ao_adc	ao_flight_data;
-__data enum flight_state	ao_flight_state;
-__data uint16_t			ao_flight_state_tick;
-__data int16_t			ao_flight_accel;
-__data int16_t			ao_flight_pres;
-__data int16_t			ao_ground_pres;
-__data int16_t			ao_ground_accel;
-__data int16_t			ao_min_pres;
-__data uint16_t			ao_launch_time;
+__xdata struct ao_adc		ao_flight_data;		/* last acquired data */
+__data enum flight_state	ao_flight_state;	/* current flight state */
+__data uint16_t			ao_flight_tick;		/* time of last data */
+__data int16_t			ao_flight_accel;	/* filtered acceleration */
+__data int16_t			ao_flight_pres;		/* filtered pressure */
+__data int16_t			ao_ground_pres;		/* startup pressure */
+__data int16_t			ao_ground_accel;	/* startup acceleration */
+__data int16_t			ao_min_pres;		/* minimum recorded pressure */
+__data uint16_t			ao_launch_time;		/* time of launch detect */
+__data int16_t			ao_main_pres;		/* pressure to eject main */
+
+/*
+ * track min/max data over a long interval to detect
+ * resting
+ */
+__data uint16_t			ao_interval_end;
+__data int16_t			ao_interval_cur_min_accel;
+__data int16_t			ao_interval_cur_max_accel;
+__data int16_t			ao_interval_cur_min_pres;
+__data int16_t			ao_interval_cur_max_pres;
+__data int16_t			ao_interval_min_accel;
+__data int16_t			ao_interval_max_accel;
+__data int16_t			ao_interval_min_pres;
+__data int16_t			ao_interval_max_pres;
+
+#define AO_INTERVAL_TICKS	AO_SEC_TO_TICKS(5)
 
 /* Accelerometer calibration
  *
@@ -73,6 +89,8 @@ __data uint16_t			ao_launch_time;
 #define BARO_kPa	268
 #define BARO_LAUNCH	(BARO_kPa / 5)	/* .2kPa */
 #define BARO_APOGEE	(BARO_kPa / 10)	/* .1kPa */
+#define BARO_MAIN	(BARO_kPa)	/* 1kPa */
+#define BARO_LAND	(BARO_kPa / 20)	/* .05kPa */
 
 /* We also have a clock, which can be used to sanity check things in
  * case of other failures
@@ -92,7 +110,17 @@ ao_flight(void)
 		ao_flight_accel += ao_flight_data.accel >> 4;
 		ao_flight_pres -= ao_flight_pres >> 4;
 		ao_flight_pres += ao_flight_data.pres >> 4;
+		ao_flight_tick = ao_time();
 		
+		ao_flight_tick = ao_time();
+		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;
+		}
+			       
 		switch (ao_flight_state) {
 		case ao_flight_startup:
 			if (nsamples < 100) {
@@ -102,22 +130,25 @@ ao_flight(void)
 			ao_ground_accel = ao_flight_accel;
 			ao_ground_pres = ao_flight_pres;
 			ao_min_pres = ao_flight_pres;
+			ao_main_pres = ao_ground_pres - BARO_MAIN;
+			
+			ao_interval_end = ao_flight_tick;
+			
 			if (ao_flight_accel < ACCEL_NOSE_UP) {
 				ao_flight_state = ao_flight_launchpad;
-				ao_flight_state_tick = ao_time();
 				ao_report_notify();
 			} else {
 				ao_flight_state = ao_flight_idle;
-				ao_flight_state_tick = ao_time();
 				ao_report_notify();
 			}
+			/* signal successful initialization by turning off the LED */
+			ao_led_off(AO_LED_RED);
 			break;
 		case ao_flight_launchpad:
 			if (ao_flight_accel < ACCEL_BOOST || 
 			    ao_flight_pres + BARO_LAUNCH < ao_ground_pres)
 			{
 				ao_flight_state = ao_flight_boost;
-				ao_flight_state_tick = ao_time();
 				ao_log_start();
 				ao_report_notify();
 				break;
@@ -128,7 +159,6 @@ ao_flight(void)
 			    (int16_t) (ao_flight_data.tick - ao_launch_time) > BOOST_TICKS_MAX)
 			{
 				ao_flight_state = ao_flight_coast;
-				ao_flight_state_tick = ao_time();
 				ao_report_notify();
 				break;
 			}
@@ -138,11 +168,33 @@ ao_flight(void)
 				ao_min_pres = ao_flight_pres;
 			if (ao_flight_pres - BARO_APOGEE > ao_min_pres) {
 				ao_flight_state = ao_flight_apogee;
-				ao_flight_state_tick = ao_time();
 				ao_report_notify();
 			}
 			break;
 		case ao_flight_apogee:
+//			ao_ignite(AO_IGNITE_DROGUE);
+			ao_flight_state = ao_flight_drogue;
+			ao_report_notify();
+			break; 
+		case ao_flight_drogue:
+			if (ao_flight_pres >= ao_main_pres) {
+//				ao_ignite(AO_IGNITE_MAIN);
+				ao_flight_state = ao_flight_main;
+				ao_report_notify();
+			}
+			if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) {
+				ao_flight_state = ao_flight_landed;
+				ao_report_notify();
+			}
+			break;
+		case ao_flight_main:
+			if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) {
+				ao_flight_state = ao_flight_landed;
+				ao_report_notify();
+			}
+			break;
+		case ao_flight_landed:
+			ao_log_stop();
 			break;
 		}
 	}
@@ -154,6 +206,11 @@ void
 ao_flight_init(void)
 {
 	ao_flight_state = ao_flight_startup;
+	ao_interval_min_accel = 0;
+	ao_interval_max_accel = 0x7fff;
+	ao_interval_min_pres = 0;
+	ao_interval_max_pres = 0x7fff;
+	ao_interval_end = AO_INTERVAL_TICKS;
 
 	ao_add_task(&flight_task, ao_flight);
 }