2017-12-22 16:10:21 +00:00
|
|
|
from dronekit import connect, VehicleMode, APIException, LocationGlobal
|
2017-12-21 18:19:48 +00:00
|
|
|
from pymavlink import mavutil
|
2018-01-07 17:10:41 +00:00
|
|
|
import time
|
|
|
|
import numpy as np
|
2017-12-21 18:55:27 +00:00
|
|
|
from config import *
|
2018-01-10 19:08:16 +00:00
|
|
|
import RPi.GPIO as GPIO
|
2017-12-21 18:19:48 +00:00
|
|
|
import sys
|
2017-12-21 18:55:27 +00:00
|
|
|
import argparse
|
2017-12-21 18:19:48 +00:00
|
|
|
|
2017-12-21 18:55:27 +00:00
|
|
|
###############################################################################
|
2018-01-07 17:10:41 +00:00
|
|
|
# Functions
|
2017-12-21 18:55:27 +00:00
|
|
|
###############################################################################
|
2018-01-07 17:10:41 +00:00
|
|
|
|
2017-12-22 16:10:21 +00:00
|
|
|
start_time = time.time()
|
2018-01-07 17:10:41 +00:00
|
|
|
def mytime():
|
|
|
|
return time.time() - start_time
|
|
|
|
|
|
|
|
def exit(status):
|
|
|
|
if vehicle is not None: vehicle.close()
|
2018-01-10 19:08:16 +00:00
|
|
|
GPIO.cleanup()
|
2018-01-07 17:10:41 +00:00
|
|
|
sys.exit(status)
|
|
|
|
|
|
|
|
def print_status():
|
|
|
|
print 'Time: {}\tMode: {}\t Alt: {}\tLoc: ({}, {})'.format(
|
|
|
|
mytime(),
|
|
|
|
vehicle.mode.name,
|
2018-01-10 19:08:16 +00:00
|
|
|
vehicle.location.global_relative_frame.alt,
|
2018-01-07 17:10:41 +00:00
|
|
|
vehicle.location.global_frame.lat,
|
|
|
|
vehicle.location.global_frame.lon)
|
|
|
|
|
|
|
|
###############################################################################
|
|
|
|
# Setup
|
|
|
|
###############################################################################
|
2018-01-10 19:08:16 +00:00
|
|
|
GPIO.setmode(GPIO.BCM)
|
|
|
|
GPIO.setup(BURN_PIN, GPIO.OUT)
|
|
|
|
GPIO.output(BURN_PIN, GPIO.LOW)
|
2018-01-10 19:49:01 +00:00
|
|
|
target_location = LocationGlobal(TARGET_LAT, TARGET_LON, 1000)
|
2017-12-22 16:10:21 +00:00
|
|
|
|
2017-12-21 18:55:27 +00:00
|
|
|
vehicle = None
|
|
|
|
try:
|
|
|
|
vehicle = connect(PORT, baud=115200, wait_ready=True)
|
|
|
|
except Exception as e:
|
|
|
|
print e
|
|
|
|
print 'Failed to connect to pixhawk. exiting.'
|
2017-12-22 16:10:21 +00:00
|
|
|
exit(1)
|
|
|
|
|
2018-01-10 15:22:18 +00:00
|
|
|
vehicle.mode = VehicleMode("MANUAL")
|
2018-01-07 17:10:41 +00:00
|
|
|
|
2018-01-10 15:22:18 +00:00
|
|
|
while not vehicle.mode.name == "MANUAL":
|
2017-12-22 16:10:21 +00:00
|
|
|
print 'Waiting for MANUAL mode.'
|
2018-01-07 17:10:41 +00:00
|
|
|
time.sleep(1)
|
2017-12-22 16:10:21 +00:00
|
|
|
|
|
|
|
vehicle.armed = True
|
|
|
|
while not vehicle.armed:
|
|
|
|
print 'Waiting for arm.'
|
2018-01-07 17:10:41 +00:00
|
|
|
time.sleep(1)
|
2017-12-22 16:10:21 +00:00
|
|
|
|
|
|
|
print_status()
|
|
|
|
print '\n################# READY FOR FLIGHT #################\n'
|
2017-12-21 18:55:27 +00:00
|
|
|
|
|
|
|
###############################################################################
|
|
|
|
# Ascent
|
|
|
|
###############################################################################
|
|
|
|
|
2017-12-23 19:09:55 +00:00
|
|
|
# maintain a circular buffer of altitude
|
|
|
|
alt_buffer_len = int(60/LOOP_DELAY)
|
2018-01-10 19:08:16 +00:00
|
|
|
alt_buffer = np.ones([alt_buffer_len]) * vehicle.location.global_relative_frame.alt
|
2017-12-23 19:09:55 +00:00
|
|
|
alt_buffer_ind = 0
|
|
|
|
|
2018-01-07 17:10:41 +00:00
|
|
|
prev_time_below_burn_alt = mytime()
|
2017-12-21 18:55:27 +00:00
|
|
|
while True:
|
2018-01-10 19:08:16 +00:00
|
|
|
alt = vehicle.location.global_relative_frame.alt
|
2017-12-23 19:09:55 +00:00
|
|
|
alt_buffer[alt_buffer_ind] = alt
|
|
|
|
alt_buffer_ind += 1
|
|
|
|
alt_buffer_ind = alt_buffer_ind % alt_buffer_len
|
2018-01-10 19:23:12 +00:00
|
|
|
alt_diff = alt - alt_buffer[alt_buffer_ind]
|
|
|
|
if (alt_diff < -50):
|
|
|
|
print 'WARNING: descended {}m in 60 seconds. Disconnecting.'.format(alt_diff)
|
2017-12-23 19:09:55 +00:00
|
|
|
break
|
|
|
|
|
2017-12-22 16:10:21 +00:00
|
|
|
if alt < BURN_ALTITUDE:
|
2018-01-07 17:10:41 +00:00
|
|
|
prev_time_below_burn_alt = mytime()
|
2017-12-21 18:55:27 +00:00
|
|
|
else:
|
2018-01-07 17:10:41 +00:00
|
|
|
time_above = mytime() - prev_time_below_burn_alt
|
2017-12-21 18:55:27 +00:00
|
|
|
print 'Above {}m for {} seconds'.format(BURN_ALTITUDE, time_above)
|
|
|
|
if time_above > BURN_TIME_ABOVE:
|
|
|
|
break
|
|
|
|
|
2018-01-10 19:49:01 +00:00
|
|
|
if alt_buffer_ind == 0: print_status() # print status once every 60 seconds
|
2017-12-23 19:09:55 +00:00
|
|
|
time.sleep(LOOP_DELAY)
|
|
|
|
|
2017-12-22 16:10:21 +00:00
|
|
|
print_status()
|
2018-01-10 19:08:16 +00:00
|
|
|
print '\n################# REACHED ALTITUDE: BURN STARTED #################\n'
|
2017-12-22 16:10:21 +00:00
|
|
|
|
2017-12-23 19:09:55 +00:00
|
|
|
|
2017-12-21 18:55:27 +00:00
|
|
|
###############################################################################
|
|
|
|
# Burn
|
|
|
|
###############################################################################
|
|
|
|
|
2018-01-10 19:08:16 +00:00
|
|
|
GPIO.output(BURN_PIN, GPIO.HIGH)
|
2017-12-22 16:10:21 +00:00
|
|
|
vehicle.mode = VehicleMode("GUIDED")
|
2018-01-10 19:49:01 +00:00
|
|
|
vehicle.simple_goto(target_location)
|
2017-12-22 16:10:21 +00:00
|
|
|
|
2018-01-07 17:10:41 +00:00
|
|
|
prev_time_above_burn_alt = mytime()
|
2017-12-22 16:10:21 +00:00
|
|
|
while True:
|
2018-01-10 19:08:16 +00:00
|
|
|
alt = vehicle.location.global_relative_frame.alt
|
2017-12-22 16:10:21 +00:00
|
|
|
if alt > BURN_ALTITUDE:
|
2018-01-07 17:10:41 +00:00
|
|
|
prev_time_above_burn_alt = mytime()
|
2017-12-22 16:10:21 +00:00
|
|
|
else:
|
2018-01-07 17:10:41 +00:00
|
|
|
time_below = mytime() - prev_time_above_burn_alt
|
2017-12-22 16:10:21 +00:00
|
|
|
print 'Below {}m for {} seconds'.format(BURN_ALTITUDE, time_below)
|
2018-01-10 19:08:16 +00:00
|
|
|
if time_below > BURN_TIME_BELOW:
|
2017-12-22 16:10:21 +00:00
|
|
|
break
|
|
|
|
|
2017-12-23 19:09:55 +00:00
|
|
|
time.sleep(LOOP_DELAY)
|
|
|
|
|
2018-01-10 19:08:16 +00:00
|
|
|
GPIO.output(BURN_PIN, GPIO.LOW)
|
2017-12-22 16:10:21 +00:00
|
|
|
|
|
|
|
print_status()
|
2018-01-10 19:08:16 +00:00
|
|
|
print '\n################# VEHICLE DISCONNECTED: BURN STOPPED #################\n'
|
2017-12-22 16:10:21 +00:00
|
|
|
|
2017-12-21 18:55:27 +00:00
|
|
|
###############################################################################
|
|
|
|
# Descent
|
|
|
|
###############################################################################
|
2017-12-22 16:10:21 +00:00
|
|
|
|
2018-01-07 17:10:41 +00:00
|
|
|
while True:
|
2017-12-22 16:10:21 +00:00
|
|
|
print_status()
|
2018-01-10 19:49:01 +00:00
|
|
|
time.sleep(60)
|
2017-12-22 16:10:21 +00:00
|
|
|
|