from pyliner.app.communication import Communication
from pyliner.util import read_json
from pyliner.util.scripting_wrapper import ScriptingWrapper
from pyliner.vehicle import Vehicle
vehicle = Vehicle(vehicle_id='rocky',
communications=Communication(
airliner_map=read_json('airliner.json'),
address="192.168.1.2",
ci_port=5009,
to_port=5012))
with ScriptingWrapper(vehicle) as rocky:
geo = rocky.geographic
# Takeoff
rocky.ctrl.atp('Begin Mission')
rocky.ctrl.arm()
rocky.ctrl.takeoff()
# Generate waypoints
last = rocky.nav.position
last.altitude += 10
wpts = []
motion = [(0, 30), (90, 10), (180, 30), (90, 10), (0, 30), (90, 10),
(180, 30), (90, 10), (0, 30), (90, 10), (180, 30)]
for brng, dist in motion:
last = geo.pbd(last, brng, dist)
wpts.append(last)
# Goto
from pyliner.util import read_json
from pyliner.util.scripting_wrapper import ScriptingWrapper
def range_limit(current, target):
return limiter(-0.2, 0.2)(proportional(0.1 / 50.0)(current, target))
rky = Vehicle(vehicle_id='rocky',
communications=Communication(
airliner_map=read_json("airliner.json"),
address="192.168.1.2",
ci_port=5009,
to_port=5012))
with ScriptingWrapper(rky) as rocky:
while rocky.nav.altitude == "NULL":
sleep(1)
print "Waiting for telemetry downlink..."
rocky.ctrl.atp('Arm')
rocky.ctrl.arm()
rocky.ctrl.atp('Takeoff')
rocky.ctrl.takeoff()
rocky.ctrl.flight_mode(FlightMode.PosCtl)
rocky.ctrl.atp('Move Up')
rocky.nav.vnav(method=proportional(0.25), tolerance=0.5).up(10)
lnav = rocky.nav.lnav(method=proportional(0.20), tolerance=0.5)
yaw = rocky.nav.rotate(method=range_limit, tolerance=2)
HALF_X = 400 / 2
HALF_Y = 400 / 2
TARGET_DISTANCE = 10
# Need to scale [-200, 200] to [-1, 1]
rotator = proportional(1.0 / 200.0)
# Scale distance by some factor
lnav = proportional(1.0 / 10.0)
# TODO OpenCV initialization here
rocky = ScriptingWrapper(Vehicle(
vehicle_id='rocky',
communications=Communication(
airliner_map=read_json("airliner.json"),
ci_port=5009,
to_port=5012)
))
def opencv_follow():
# TODO Get video from rocky
# x and y in pixels, z in meters
x, y, z = # TODO method name
# Normalize x, y, and z
x -= HALF_X
y -= HALF_Y
z -= TARGET_DISTANCE