From 293666dd2f6f2dd657afa68b48bf0a53ebe28248 Mon Sep 17 00:00:00 2001
From: Izzy Brand <isaiah_brand@brown.edu>
Date: Fri, 12 Jan 2018 00:15:44 -0500
Subject: [PATCH] Added straightline dist and changed status format

---
 companion.py | 14 +++++++++++---
 config.py    |  2 +-
 util.py      | 22 ++++++++++++++++++++++
 3 files changed, 34 insertions(+), 4 deletions(-)
 create mode 100644 util.py

diff --git a/companion.py b/companion.py
index 23f95d1..1431b60 100644
--- a/companion.py
+++ b/companion.py
@@ -3,6 +3,7 @@ from pymavlink import mavutil
 import time
 import numpy as np
 from config import *
+from util import *
 import RPi.GPIO as GPIO
 import sys
 import argparse
@@ -12,6 +13,8 @@ import argparse
 ###############################################################################
 
 start_time = time.time()
+target_location = LocationGlobalRelative(TARGET_LAT, TARGET_LON, 0)
+
 def mytime():
 	return time.time() - start_time
 
@@ -21,12 +24,13 @@ def exit(status):
 	sys.exit(status)
 
 def print_status():
-	print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
+	print 'Time: {:.2f}s\tMode: {}\t Alt: {}m\tLoc: ({}, {})\tdist: {:.3f}km.'.format(
 		mytime(),
 		vehicle.mode.name,
 		vehicle.location.global_relative_frame.alt,
 		vehicle.location.global_frame.lat,
-		vehicle.location.global_frame.lon)
+		vehicle.location.global_frame.lon,
+		straight_line_dist(target_location, vehicle.location.global_frame))
 
 ###############################################################################
 # Setup
@@ -34,7 +38,10 @@ def print_status():
 GPIO.setmode(GPIO.BCM)
 GPIO.setup(BURN_PIN, GPIO.OUT)
 GPIO.output(BURN_PIN, GPIO.LOW)
-target_location = LocationGlobalRelative(TARGET_LAT, TARGET_LON, 0)
+print 'GPIO enabled on pin {}'.format(BURN_PIN)
+print 'Burst altitude set to {} meters\n'.format(BURN_ALTITUDE)
+
+
 
 vehicle = None
 try:
@@ -59,6 +66,7 @@ while not vehicle.armed:
 	print 'Waiting for arm.'
 	time.sleep(1)
 
+
 print_status()
 print '\n################# READY FOR FLIGHT #################\n'
 
diff --git a/config.py b/config.py
index acad29e..3f6a50c 100644
--- a/config.py
+++ b/config.py
@@ -1,4 +1,4 @@
-PORT 					= '/dev/tty.usbmodem1'	# whic port the pixhawk is on
+PORT 					= '/dev/ttyACM0'	# whic port the pixhawk is on
 BURN_PIN 				= 27				# pin with burn relay
 BURN_ALTITUDE 			= 85 			    # altitude at which to burn
 BURN_TIME_ABOVE 		= 5					# time above burn alt before ignite
diff --git a/util.py b/util.py
new file mode 100644
index 0000000..3e80958
--- /dev/null
+++ b/util.py
@@ -0,0 +1,22 @@
+from math import sin, cos, sqrt, atan2, radians
+
+
+def straight_line_dist(loc1, loc2):
+	# approximate radius of earth in km
+	R = 6371.000
+
+	lat1 = radians(loc1.lat)
+	lon1 = radians(loc1.lon)
+	lat2 = radians(loc2.lat)
+	lon2 = radians(loc2.lon)
+
+	dlon = lon2 - lon1
+	dlat = lat2 - lat1
+
+	a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
+	c = 2 * atan2(sqrt(a), sqrt(1 - a))
+
+	distance = R * c
+	return distance
+	print("Result:", distance)
+	print("Should be:", 278.546, "km")
\ No newline at end of file