-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathhover.py
105 lines (89 loc) · 3.59 KB
/
hover.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
# When I wrote this, only God and I understood what I was doing
# Now, God only knows
import time, sys
from threading import Thread
#FIXME: Has to be launched from within the example folder
sys.path.append("/home/jonathan/Programs/crazyflie/cfclient-2014.01.0/lib")
import cflib
from cflib.crazyflie import Crazyflie
from controller import SampleListener
import logging
logging.basicConfig(level=logging.ERROR)
class Hover:
def __init__(self, link_uri, control_listener, config=None):
""" Initialize and run the example with the specified link_uri """
self._config = config
self._cf = Crazyflie()
self._control_listener = control_listener
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print "Connecting to %s" % link_uri
def _connected(self, link_uri):
print "Connected to %s" % link_uri
Thread(target=self._hover_this_shit).start()
# self._hover_this_shit()
def _disconnected(self, link_uri):
print "disconnected from %s" % link_uri
def _connection_failed(self, link_uri, msg):
print "Connection to %s failed: %s" % (link_uri, msg)
if not self._config:
if "reconnect" in self._config:
print "Attempting reconnect.."
if self._config["reconnect"]:
self._cf.open_link(link_uri)
else:
sys.exit(2)
def _connection_lost(self, link_uri, msg):
print "Connection to %s lost: %s" % (link_uri, msg)
if "reconnect" in self._config:
print "Attempting reconnect.."
if self._config["reconnect"]:
self._cf.open_link(link_uri)
else:
sys.exit(3)
# def _hover_this_shit(self):
# print "Hovering this shit"
# thrust_mult = 1.5
# thrust_step = 500
# thrust = 20000
# pitch = -6
# roll = -2
# yawrate = 0
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >=47000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
# self._cf.commander.send_setpoint(0, 0, 0, 0)
# # Make sure that the last packet leaves before the link is closed
# # since the message queue is not flushed before closing
# time.sleep(0.1)
# self._cf.close_link()
def _hover_this_shit(self):
print "Hovering this shit"
# try:
while True:
print "asdasd %s %s %s %d" % (
int(self._control_listener.roll()*10),
int(self._control_listener.pitch()*10),
int(self._control_listener.yaw()*100),
int(self._control_listener.y() * 47000))
self._cf.commander.send_setpoint(
int(self._control_listener.roll()*10),
int(self._control_listener.pitch()*10),
int(self._control_listener.yaw()*100),
int(self._control_listener.y() * 47000))
# self._cf.commander.send_setpoint(
# 0,
# 0,
# 0,
# int(self._control_listener.y() * 47000))
time.sleep(0.1)
# except (KeyboardInterrupt):
# self._cf.commander.send_setpoint(0, 0, 0, 0)
# self._cf.close_link()
# exit