From 326f6904fad9dcd7d10d4d0b1c94221e46a2a07f Mon Sep 17 00:00:00 2001
From: IzzyBrand <isaiah_brand@brown.edu>
Date: Sat, 23 Dec 2017 14:09:55 -0500
Subject: [PATCH] added circular buffer to detect if glider is descending
 prematurely or never reaches burst altitude

---
 companion.py | 20 +++++++++++++++++++-
 config.py    |  1 +
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/companion.py b/companion.py
index c61d86b..830eb93 100644
--- a/companion.py
+++ b/companion.py
@@ -39,9 +39,21 @@ print '\n################# READY FOR FLIGHT #################\n'
 # Ascent
 ###############################################################################
 
+# maintain a circular buffer of altitude
+alt_buffer_len 	= int(60/LOOP_DELAY)
+alt_buffer 		= np.ones([alt_buffer_len]) * vehicle.location.global_frame.alt
+alt_buffer_ind 	= 0
+
 prev_time_below_burn_alt = time()
 while True:
 	alt = vehicle.location.global_frame.alt
+	alt_buffer[alt_buffer_ind] = alt
+	alt_buffer_ind += 1
+	alt_buffer_ind = alt_buffer_ind % alt_buffer_len
+	if (alt - alt_buffer[alt_buffer_ind] < 50):
+		print 'WARNING: descended 50m in 60 seconds. Disconnecting.'
+		break
+
 	if alt < BURN_ALTITUDE: 
 		prev_time_below_burn_alt = time()
 	else: 
@@ -50,15 +62,19 @@ while True:
 		if time_above > BURN_TIME_ABOVE:
 			break
 
+	time.sleep(LOOP_DELAY)
+
 print_status()
 print '\n################# REACHED BURN ALTITUDE #################\n'
 
+
 ###############################################################################
 # Burn
 ###############################################################################
 
 GPIO.output(BURN_PIN, GPIO.HIGH)
 vehicle.mode = VehicleMode("GUIDED")
+vehicle.simple_goto
 
 prev_time_above_burn_alt = time()
 while True:
@@ -71,6 +87,8 @@ while True:
 		if time_below > BURN_TIME_ABOVE:
 			break
 
+	time.sleep(LOOP_DELAY)
+
 GPIO.output(BURN_PIN, GPIO.LOW)
 
 print_status()
@@ -82,7 +100,7 @@ print '\n################# VEHICLE DISCONNECTED #################\n'
 
 while True():
 	print_status()
-	time.sleep(1)
+	time.sleep(LOOP_DELAY)
 
 def print_status():
 	print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
diff --git a/config.py b/config.py
index 3f839c8..4d97723 100644
--- a/config.py
+++ b/config.py
@@ -5,3 +5,4 @@ BURN_TIME_ABOVE 		= 20				# time above burn alt before ignite
 TARGET_LAT	 			= 42.345131			# desired landing latitude
 TARGET_LON				= -71.210126		# desired landing longitude
 TARGET_ALT 				= 0 				# meters above sea level
+LOOP_DELAY				= 0.5				# time in seconds to delay within loops