Added straightline dist and changed status format

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'
 

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

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")