X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fcore%2Fao_flight.c;h=5e19463867bb85718dabbd7ec93c9cce256290f8;hb=059f09dbca4703c25b42389b54c6510331c39485;hp=85c1825bddfebf68c06f2d2b055c511eddae4164;hpb=9513be7f9d3d0b0ec29f6487fa9dc8f1ac24d0de;p=fw%2Faltos

diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c
index 85c1825b..5e194638 100644
--- a/src/core/ao_flight.c
+++ b/src/core/ao_flight.c
@@ -34,15 +34,17 @@
 /* Main flight thread. */
 
 __pdata enum ao_flight_state	ao_flight_state;	/* current flight state */
-__pdata uint16_t		ao_launch_tick;		/* time of launch detect */
+__pdata uint16_t		ao_boost_tick;		/* time of launch detect */
 
 /*
  * track min/max data over a long interval to detect
  * resting
  */
-__pdata uint16_t		ao_interval_end;
-__pdata int16_t			ao_interval_min_height;
-__pdata int16_t			ao_interval_max_height;
+static __data uint16_t		ao_interval_end;
+static __data int16_t		ao_interval_min_height;
+static __data int16_t		ao_interval_max_height;
+static __data int16_t		ao_coast_avg_accel;
+
 __pdata uint8_t			ao_flight_force_idle;
 
 /* We also have a clock, which can be used to sanity check things in
@@ -91,6 +93,8 @@ ao_flight(void)
 				 */
 				ao_flight_state = ao_flight_invalid;
 
+				/* Turn on packet system in invalid mode on TeleMetrum */
+				ao_packet_slave_start();
 			} else
 #endif
 				if (!ao_flight_force_idle
@@ -108,8 +112,10 @@ ao_flight(void)
 				ao_usb_disable();
 #endif
 
-				/* Disable packet mode in pad state */
+#if !HAS_ACCEL
+				/* Disable packet mode in pad state on TeleMini */
 				ao_packet_slave_stop();
+#endif
 
 				/* Turn on telemetry system */
 				ao_rdf_set(1);
@@ -121,6 +127,11 @@ ao_flight(void)
 				/* Set idle mode */
  				ao_flight_state = ao_flight_idle;
  
+#if HAS_ACCEL
+				/* Turn on packet system in idle mode on TeleMetrum */
+				ao_packet_slave_start();
+#endif
+
 				/* signal successful initialization by turning off the LED */
 				ao_led_off(AO_LED_RED);
 			}
@@ -151,7 +162,7 @@ ao_flight(void)
 				)
 			{
 				ao_flight_state = ao_flight_boost;
-				ao_launch_tick = ao_sample_tick;
+				ao_boost_tick = ao_sample_tick;
 
 				/* start logging data */
 				ao_log_start();
@@ -184,10 +195,11 @@ ao_flight(void)
 			 * (15 seconds) has past.
 			 */
 			if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) ||
-			    (int16_t) (ao_sample_tick - ao_launch_tick) > BOOST_TICKS_MAX)
+			    (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX)
 			{
 #if HAS_ACCEL
 				ao_flight_state = ao_flight_fast;
+				ao_coast_avg_accel = ao_accel;
 #else
 				ao_flight_state = ao_flight_coast;
 #endif
@@ -205,7 +217,8 @@ ao_flight(void)
 			{
 				ao_flight_state = ao_flight_coast;
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
-			}
+			} else
+				goto check_re_boost;
 			break;
 #endif
 		case ao_flight_coast:
@@ -237,6 +250,17 @@ ao_flight(void)
 				ao_flight_state = ao_flight_drogue;
 				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
 			}
+#if HAS_ACCEL
+			else {
+			check_re_boost:
+				ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6);
+				if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) {
+					ao_boost_tick = ao_sample_tick;
+					ao_flight_state = ao_flight_boost;
+					ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+				}
+			}
+#endif
 
 			break;
 		case ao_flight_drogue: