#!/usr/bin/env python
from __future__ import print_function
import time
import math
import logging
import subprocess
import multiprocessing as mtp
import RPi.GPIO as GPIO
from dronekit import connect, LocationGlobalRelative, VehicleMode
from pymavlink import mavutil
from physics import getMetersOverTime, getTimeToVelo, GRAVITY
# print debug info
DEBUG = True
# Refresh rate in HZ
REFRESH_RATE = 50
# Max allowable height in meters
MAX_HEIGHT = 40 #121
# Target takeoff altitude
TAKEOFF_ALT = 5
# Amount of buffer to max height to prevent going over max height in meters
BUFFER = 10
# Pin of signal LED
LED_PIN = 24
def setupDebug(time1, time2):
fname = "/home/pi/logs/scriptLogs/" + time1 + "_" + time2 + ".log"
logging.basicConfig(filename=fname, level=logging.DEBUG)
def closeDebug():
logging.shutdown()
def debug(arg):
if DEBUG:
logging.debug(arg)
print(arg)
def ledManager(success, fail, off):
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(LED_PIN, GPIO.OUT)
GPIO.output(LED_PIN, GPIO.LOW)
done = False
while True:
if not done and success.is_set():
ledOn()
success.clear()
done = True
elif fail.is_set() or off.is_set():
ledOff()
return
elif not done:
ledBlink()
time.sleep(1)
def ledBlink():
for i in range(0,3):
ledOn()
time.sleep(.2)
ledOff()
time.sleep(.2)
def ledOn():
GPIO.output(LED_PIN, GPIO.HIGH)
def ledOff():
GPIO.output(LED_PIN, GPIO.LOW)
class Drone:
# Vehicle reference
vehicle = None
# LED thread and signal references
led = None
success = None
fail = None
off = None
def __init__(self, connectionString):
self.success = mtp.Event()
self.fail = mtp.Event()
self.off = mtp.Event()
try:
led = mtp.Process(name='LED', target=ledManager, args=(self.success, self.fail, self.off))
led.start()
self.vehicle = connect(connectionString, rate=REFRESH_RATE, heartbeat_timeout=180)
self.vehicle.wait_ready(True, timeout=180)
debug("Vehicle connection successful")
self.success.set()
except Exception, e:
fail.set()
debug(repr(e))
time.sleep(5)
self.close()
exit()
def goto_position_target_local_ned(self, north, east, down):
"""
Send SET_POSITION_TARGET_LOCAL_NED command to request the vehicle fly to a specified
location in the North, East, Down frame.
It is important to remember that in this frame, positive altitudes are entered as negative
"Down" values. So if down is "10", this will be 10 metres below the home altitude.
Starting from AC3.3 the method respects the frame setting. Prior to that the frame was
ignored. For more information see:
http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_local_ned
See the above link for information on the type_mask (0=enable, 1=ignore).
At time of writing, acceleration and yaw bits are ignored.
"""
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0b0000111111111000, # type_mask (only positions enabled)
north, east, down, # x, y, z positions (or North, East, Down in the MAV_FRAME_BODY_NED frame
0, 0, 0, # x, y, z velocity in m/s (not used)
0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
self.vehicle.send_mavlink(msg)
def send_global_velocity(self, velocity_x, velocity_y, velocity_z):
"""
Move vehicle in direction based on specified velocity vectors.
This uses the SET_POSITION_TARGET_GLOBAL_INT command with type mask enabling only
velocity components
(http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_global_int).
Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
velocity persists until it is canceled. The code below should work on either version
(sending the message multiple times does not cause problems).
See the above link for information on the type_mask (0=enable, 1=ignore).
At time of writing, acceleration and yaw bits are ignored.
"""
msg = self.vehicle.message_factory.set_position_target_global_int_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
0b0000111111000111, # type_mask (only speeds enabled)
0, # lat_int - X Position in WGS84 frame in 1e7 * meters
0, # lon_int - Y Position in WGS84 frame in 1e7 * meters
0, # alt - Altitude in meters in AMSL altitude(not WGS84 if absolute or relative)
# altitude above terrain if GLOBAL_TERRAIN_ALT_INT
-velocity_x, # X velocity in NED frame in m/s
-velocity_y, # Y velocity in NED frame in m/s
-velocity_z, # Z velocity in NED frame in m/s
0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
self.vehicle.send_mavlink(msg)
def sleepOneTick(self):
time.sleep(1/float(REFRESH_RATE))
def heightConstraintExceeded(self, maxHeight):
alt = self.vehicle.location.global_relative_frame.alt
velo = self.vehicle.velocity[2]
debug(velo)
# meters to top of arc if we started 0g right now
metersToTop = getMetersOverTime(velo, GRAVITY, getTimeToVelo(velo, 0, GRAVITY))
debug(metersToTop)
# max height exceeded
if maxHeight < alt + metersToTop:
return True
return False
def accel(self):
while True:
if self.heightConstraintExceeded(MAX_HEIGHT - BUFFER):
return
self.send_global_velocity(0,0,1000)
self.sleepOneTick()
def enter0G(self):
vi = self.vehicle.velocity[2]
ti = time.clock()
stop = getTimeToVelo(vi, 0, GRAVITY) #* 2 # uncomment for up and down
while True:
now = time.clock()
dt = now-ti
if dt > stop:
debug("time elapsed, exiting 0g")
return
targetVelo = vi + (dt * GRAVITY)
error = targetVelo - self.vehicle.velocity[2]
debug("target - actual = " + str(error))
if math.fabs(error) > 2:
debug("Too far above or below target velo, aborting 0g")
return
# target velocity for one tick in the future
adjTargetVelo = vi + ((dt + (1/60.0)) * GRAVITY)
self.send_global_velocity(0,0,adjTargetVelo)
debug("Setting target velo to " + str(adjTargetVelo))
self.sleepOneTick()
def waitForArm(self):
while not self.vehicle.armed:
debug("Waiting for vehicle to be armed...")
time.sleep(.25)
def takeoff(self):
self.vehicle.mode = VehicleMode("GUIDED")
self.vehicle.armed = True
self.waitForArm()
self.vehicle.simple_takeoff(TAKEOFF_ALT)
self.waitForTargetAlt(TAKEOFF_ALT)
def prepForLand(self):
debug("Going towards landing rally point ...")
self.goto_position_target_local_ned(0,0,-15)
self.waitForTargetAlt(15)
def waitForLand(self):
while self.vehicle.armed:
debug("Waiting for vehicle to land and disarm...")
time.sleep(1)
def waitForTargetAlt(self, target):
while True:
debug(" Altitude: " + str(self.vehicle.location.global_relative_frame.alt))
if (self.vehicle.location.global_relative_frame.alt <= target * 1.05 and
self.vehicle.location.global_relative_frame.alt >= target * .95):
debug("Reached target altitude")
break
time.sleep(1)
def close(self):
self.off.set()
self.vehicle.close()
self.vehicle = None
return None