kopia lustrzana https://github.com/IzzyBrand/spaceplane
fleshed out companion.py and added more params
rodzic
9d5171492c
commit
bd0e5868f6
76
companion.py
76
companion.py
|
@ -1,22 +1,39 @@
|
||||||
from dronekit import connect, VehicleMode, APIException
|
from dronekit import connect, VehicleMode, APIException, LocationGlobal
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
from time import time
|
from time
|
||||||
from config import *
|
from config import *
|
||||||
from helpers import *
|
from helpers import *
|
||||||
|
import RPi.GPIO as GPIO
|
||||||
import sys
|
import sys
|
||||||
import argparse
|
import argparse
|
||||||
|
|
||||||
|
###############################################################################
|
||||||
|
# Setup
|
||||||
|
###############################################################################
|
||||||
|
GPIO.setup(BURN_PIN, GPIO.OUT)
|
||||||
|
GPIO.output(BURN_PIN, GPIO.LOW)
|
||||||
|
start_time = time.time()
|
||||||
|
|
||||||
###############################################################################
|
|
||||||
# Connect to pixhawk
|
|
||||||
###############################################################################
|
|
||||||
vehicle = None
|
vehicle = None
|
||||||
try:
|
try:
|
||||||
vehicle = connect(PORT, baud=115200, wait_ready=True)
|
vehicle = connect(PORT, baud=115200, wait_ready=True)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print e
|
print e
|
||||||
print 'Failed to connect to pixhawk. exiting.'
|
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
|
# Ascent
|
||||||
|
@ -24,18 +41,61 @@ except Exception as e:
|
||||||
|
|
||||||
prev_time_below_burn_alt = time()
|
prev_time_below_burn_alt = time()
|
||||||
while True:
|
while True:
|
||||||
alt = vehicle.location.global_relate_frame.alt
|
alt = vehicle.location.global_frame.alt
|
||||||
if alt < BURN_ALTITUDE: prev_time_below_burn_alt = time()
|
if alt < BURN_ALTITUDE:
|
||||||
|
prev_time_below_burn_alt = time()
|
||||||
else:
|
else:
|
||||||
time_above = time() - prev_time_below_burn_alt
|
time_above = time() - prev_time_below_burn_alt
|
||||||
print 'Above {}m for {} seconds'.format(BURN_ALTITUDE, time_above)
|
print 'Above {}m for {} seconds'.format(BURN_ALTITUDE, time_above)
|
||||||
if time_above > BURN_TIME_ABOVE:
|
if time_above > BURN_TIME_ABOVE:
|
||||||
break
|
break
|
||||||
|
|
||||||
|
print_status()
|
||||||
|
print '\n################# REACHED BURN ALTITUDE #################\n'
|
||||||
|
|
||||||
###############################################################################
|
###############################################################################
|
||||||
# Burn
|
# 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
|
# 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)
|
11
config.py
11
config.py
|
@ -1,4 +1,7 @@
|
||||||
PORT = '/dev/ttyUSB0'
|
PORT = '/dev/ttyUSB0' # whic port the pixhawk is on
|
||||||
BURN_ALTITUDE = 20000
|
BURN_PIN = 25 # pin with burn relay
|
||||||
BURN_TIME_ABOVE = 20
|
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
|
||||||
|
|
Ładowanie…
Reference in New Issue