From bd0e5868f687b3ff391c9f00fd566e7a33ed432a Mon Sep 17 00:00:00 2001
From: IzzyBrand <isaiah_brand@brown.edu>
Date: Fri, 22 Dec 2017 11:10:21 -0500
Subject: [PATCH] fleshed out companion.py and added more params

---
 companion.py | 76 ++++++++++++++++++++++++++++++++++++++++++++++------
 config.py    | 11 +++++---
 2 files changed, 75 insertions(+), 12 deletions(-)

diff --git a/companion.py b/companion.py
index ca52a8c..c61d86b 100644
--- a/companion.py
+++ b/companion.py
@@ -1,22 +1,39 @@
-from dronekit import connect, VehicleMode, APIException
+from dronekit import connect, VehicleMode, APIException, LocationGlobal
 from pymavlink import mavutil
-from time import time
+from time
 from config import *
 from helpers import *
+import RPi.GPIO as GPIO
 import sys
 import argparse
 
+###############################################################################
+# Setup
+###############################################################################
+GPIO.setup(BURN_PIN, GPIO.OUT)
+GPIO.output(BURN_PIN, GPIO.LOW)
+start_time = time.time()
 
-###############################################################################
-# Connect to pixhawk
-###############################################################################
 vehicle = None
 try:
 	vehicle = connect(PORT, baud=115200, wait_ready=True)
 except Exception as e:
 	print e
 	print 'Failed to connect to pixhawk. exiting.'
-	sys.exit(1)
+	exit(1)
+
+vehicle.mode = VehicleMode("MANUAL")
+while not vehicle.mode.name == "MANUAL":
+	print 'Waiting for MANUAL mode.'
+	sleep(1)
+
+vehicle.armed = True
+while not vehicle.armed:
+	print 'Waiting for arm.'
+	sleep(1)
+
+print_status()
+print '\n################# READY FOR FLIGHT #################\n'
 
 ###############################################################################
 # Ascent
@@ -24,18 +41,61 @@ except Exception as e:
 
 prev_time_below_burn_alt = time()
 while True:
-	alt = vehicle.location.global_relate_frame.alt
-	if alt < BURN_ALTITUDE: prev_time_below_burn_alt = time()
+	alt = vehicle.location.global_frame.alt
+	if alt < BURN_ALTITUDE: 
+		prev_time_below_burn_alt = time()
 	else: 
 		time_above = time() - prev_time_below_burn_alt
 		print 'Above {}m for {} seconds'.format(BURN_ALTITUDE, time_above)
 		if time_above > BURN_TIME_ABOVE:
 			break
 
+print_status()
+print '\n################# REACHED BURN ALTITUDE #################\n'
+
 ###############################################################################
 # Burn
 ###############################################################################
 
+GPIO.output(BURN_PIN, GPIO.HIGH)
+vehicle.mode = VehicleMode("GUIDED")
+
+prev_time_above_burn_alt = time()
+while True:
+	alt = vehicle.location.global_frame.alt
+	if alt > BURN_ALTITUDE: 
+		prev_time_above_burn_alt = time()
+	else: 
+		time_below = time() - prev_time_above_burn_alt
+		print 'Below {}m for {} seconds'.format(BURN_ALTITUDE, time_below)
+		if time_below > BURN_TIME_ABOVE:
+			break
+
+GPIO.output(BURN_PIN, GPIO.LOW)
+
+print_status()
+print '\n################# VEHICLE DISCONNECTED #################\n'
+
 ###############################################################################
 # Descent
 ###############################################################################
+
+while True():
+	print_status()
+	time.sleep(1)
+
+def print_status():
+	print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
+		time(),
+		vehicle.mode.name,
+		vehicle.location.global_frame.alt,
+		vehicle.location.global_frame.lat,
+		vehicle.location.global_frame.lon)
+
+def time():
+	return time.time() - start_time
+
+def exit(status):
+	if vehicle is not None: vehicle.close()
+	GPIO.cleanup()
+	sys.exit(status)
\ No newline at end of file
diff --git a/config.py b/config.py
index ec3011e..3f839c8 100644
--- a/config.py
+++ b/config.py
@@ -1,4 +1,7 @@
-PORT = '/dev/ttyUSB0'
-BURN_ALTITUDE = 20000 
-BURN_TIME_ABOVE = 20
-
+PORT 					= '/dev/ttyUSB0'	# whic port the pixhawk is on
+BURN_PIN 				= 25				# pin with burn relay
+BURN_ALTITUDE 			= 20000 			# altitude at which to burn
+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