From caa14fe1774aa8412fd8ad3b36560d5efc7f7ef4 Mon Sep 17 00:00:00 2001
From: IzzyBrand <isaiah_brand@brown.edu>
Date: Sun, 7 Jan 2018 12:10:41 -0500
Subject: [PATCH] Fixed bugs on mac

---
 companion.py | 77 +++++++++++++++++++++++++++++-----------------------
 config.py    |  4 +--
 2 files changed, 45 insertions(+), 36 deletions(-)

diff --git a/companion.py b/companion.py
index 830eb93..7f03fb9 100644
--- a/companion.py
+++ b/companion.py
@@ -1,18 +1,40 @@
 from dronekit import connect, VehicleMode, APIException, LocationGlobal
 from pymavlink import mavutil
-from time
+import time
+import numpy as np
 from config import *
 from helpers import *
-import RPi.GPIO as GPIO
+#import RPi.GPIO as GPIO
 import sys
 import argparse
 
+###############################################################################
+# Functions
+###############################################################################
+
+start_time = time.time()
+def mytime():
+	return time.time() - start_time
+
+def exit(status):
+	if vehicle is not None: vehicle.close()
+	#GPIO.cleanup()
+	sys.exit(status)
+
+def print_status():
+	print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
+		mytime(),
+		vehicle.mode.name,
+		vehicle.location.global_frame.alt,
+		vehicle.location.global_frame.lat,
+		vehicle.location.global_frame.lon)
+
 ###############################################################################
 # Setup
 ###############################################################################
-GPIO.setup(BURN_PIN, GPIO.OUT)
-GPIO.output(BURN_PIN, GPIO.LOW)
-start_time = time.time()
+#GPIO.setup(BURN_PIN, GPIO.OUT)
+#GPIO.output(BURN_PIN, GPIO.LOW)
+
 
 vehicle = None
 try:
@@ -22,15 +44,17 @@ except Exception as e:
 	print 'Failed to connect to pixhawk. exiting.'
 	exit(1)
 
-vehicle.mode = VehicleMode("MANUAL")
-while not vehicle.mode.name == "MANUAL":
+# vehicle.mode = VehicleMode("MANUAL")
+vehicle.mode = VehicleMode("STABILIZE")
+
+while not vehicle.mode.name == "STABILIZE":
 	print 'Waiting for MANUAL mode.'
-	sleep(1)
+	time.sleep(1)
 
 vehicle.armed = True
 while not vehicle.armed:
 	print 'Waiting for arm.'
-	sleep(1)
+	time.sleep(1)
 
 print_status()
 print '\n################# READY FOR FLIGHT #################\n'
@@ -44,20 +68,20 @@ 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()
+prev_time_below_burn_alt = mytime()
 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):
+	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()
+		prev_time_below_burn_alt = mytime()
 	else: 
-		time_above = time() - prev_time_below_burn_alt
+		time_above = mytime() - prev_time_below_burn_alt
 		print 'Above {}m for {} seconds'.format(BURN_ALTITUDE, time_above)
 		if time_above > BURN_TIME_ABOVE:
 			break
@@ -72,24 +96,24 @@ print '\n################# REACHED BURN ALTITUDE #################\n'
 # Burn
 ###############################################################################
 
-GPIO.output(BURN_PIN, GPIO.HIGH)
+#GPIO.output(BURN_PIN, GPIO.HIGH)
 vehicle.mode = VehicleMode("GUIDED")
 vehicle.simple_goto
 
-prev_time_above_burn_alt = time()
+prev_time_above_burn_alt = mytime()
 while True:
 	alt = vehicle.location.global_frame.alt
 	if alt > BURN_ALTITUDE: 
-		prev_time_above_burn_alt = time()
+		prev_time_above_burn_alt = mytime()
 	else: 
-		time_below = time() - prev_time_above_burn_alt
+		time_below = mytime() - prev_time_above_burn_alt
 		print 'Below {}m for {} seconds'.format(BURN_ALTITUDE, time_below)
 		if time_below > BURN_TIME_ABOVE:
 			break
 
 	time.sleep(LOOP_DELAY)
 
-GPIO.output(BURN_PIN, GPIO.LOW)
+#GPIO.output(BURN_PIN, GPIO.LOW)
 
 print_status()
 print '\n################# VEHICLE DISCONNECTED #################\n'
@@ -98,22 +122,7 @@ print '\n################# VEHICLE DISCONNECTED #################\n'
 # Descent
 ###############################################################################
 
-while True():
+while True:
 	print_status()
 	time.sleep(LOOP_DELAY)
 
-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 4d97723..154eac8 100644
--- a/config.py
+++ b/config.py
@@ -1,6 +1,6 @@
-PORT 					= '/dev/ttyUSB0'	# whic port the pixhawk is on
+PORT 					= '/dev/tty.usbmodem35'	# whic port the pixhawk is on
 BURN_PIN 				= 25				# pin with burn relay
-BURN_ALTITUDE 			= 20000 			# altitude at which to burn
+BURN_ALTITUDE 			= 2 			# 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