-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathhelpers.py
186 lines (162 loc) · 5.44 KB
/
helpers.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
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
# -*- coding: utf-8 -*-
"""
Helper functions for close-range interactive Crazyflie applications.
"""
from math import sqrt
from time import sleep
import sys
from bitalino import BITalino
def bt_connect(bt_address, bt_samplingRate=100, bt_acqChannels=[0]):
"""
Connect to bitalino at 'bt_address'.
"""
print(f'Connecting to BITalino...')
for i in range(0,10):
print(f'Connection attempt {str(i+1)}/10...')
try:
bt = BITalino(bt_address, timeout=2)
except:
print("Failed! Retrying...")
if i == 9:
print("Failed to connect to BITalino. Exiting.")
sys.exit()
else:
print("Connected to Bitalino " + bt_address)
bt.start(bt_samplingRate, bt_acqChannels)
return bt
def cf_hover_safely(cf, mr, z, cf_maxSpeed_xy = 0.5, cf_buffer_xy = 0.8):
"""
Set hover setpoint while avoiding collisions.
Recommended: check cf_is_safe() before calling this.
"""
# Reset
vx = 0.0
vy = 0.0
# Avoid obstacles
if mr.front is not None:
dvx = remap(mr.front, 0.0, cf_buffer_xy, cf_maxSpeed_xy, 0.0)
vx -= dvx
# print("FRONT: " + str(mr.front) + " --> " + str(dvx))
if mr.back is not None:
dvx = remap(mr.back, 0.0, cf_buffer_xy, cf_maxSpeed_xy, 0.0)
vx += dvx
# print("BACK: " + str(mr.back) + " --> " + str(dvx))
if mr.left is not None:
dvy = remap(mr.left, 0.0, cf_buffer_xy, cf_maxSpeed_xy, 0.0)
vy -= dvy
# print("LEFT: " + str(mr.left) + " --> " + str(dvy))
if mr.right is not None:
dvy = remap(mr.right, 0.0, cf_buffer_xy, cf_maxSpeed_xy, 0.0)
vy += dvy
# print("RIGHT: " + str(mr.right) + " --> " + str(dvy))
led_r = int(remap(sqrt((vx ** 2) + (vy ** 2)), 0, cf_maxSpeed_xy, 10, 255))
cf.param.set_value('ring.solidRed', str(led_r))
cf.commander.send_hover_setpoint(vx, vy, 0, z)
def cf_is_safe(mr, d=0.06):
"""
Performs critical safety checks and returns False if unsafe.
"""
# Is the drone too close to anything from any direction?
if mr.up is not None:
if mr.up < d:
print('PROXIMITY ALERT!')
return False
if mr.front is not None:
if mr.front < d:
print('PROXIMITY ALERT!')
return False
if mr.back is not None:
if mr.back < d:
print('PROXIMITY ALERT!')
return False
if mr.left is not None:
if mr.left < d:
print('PROXIMITY ALERT!')
return False
if mr.right is not None:
if mr.right < d:
print('PROXIMITY ALERT!')
return False
return True
def cf_land_safely(cf, mr, z0):
"""
Land slowly using hover setpoints, while avoiding collisions.
"""
print('Landing sequence initiated.')
cf_reset_ledring(cf, brightness=100)
z = z0
while (z > 0):
z -= 0.05
print(f'z: {z}')
cf_hover_safely(cf, mr, z)
try:
cf.param.set_value('ring.solidRed', str(int(z*10)))
cf.param.set_value('ring.solidGreen', str(int(z*10)))
cf.param.set_value('ring.solidBlue', str(int(z*10)))
except Exception as e:
pass
sleep(0.1)
print('Landed.')
sys.exit()
def cf_preflight_reset(cf):
"""
Pre-flight reset boilerplate.
"""
sleep(0.5)
cf.param.set_value('ring.effect', '0')
sleep(0.5)
cf.param.set_value('kalman.resetEstimation', '1')
sleep(0.5)
cf.param.set_value('kalman.resetEstimation', '0')
sleep(0.5)
def cf_pulsate_led(cf, dt, per, pattern='sawtooth', color_key='ring.solidBlue'):
"""
Pulsates LED in a specified period (in seconds) and pattern.
Assumes 'ring.effect' parameter is set to 7 (solid color).
Patterns: 'sawtooth' (default), 'triangle'
"""
if pattern == 'triangle':
if dt % per < per / 2:
led_val = int(remap(dt % per, 0, per / 2, 10, 255))
else:
led_val = int(remap(dt % per, per / 2, per, 255, 10))
else:
led_val = int(remap(dt % per, per / 2, per, 255, 10))
cf.param.set_value(color_key, str(led_val))
def cf_reset_ledring(cf, brightness=10):
"""
Resets LED ring to solid while at specified brightness (range 0-255, default 10).
"""
cf.param.set_value('ring.effect', '7')
cf.param.set_value('ring.solidRed', str(10))
cf.param.set_value('ring.solidGreen', str(10))
cf.param.set_value('ring.solidBlue', str(10))
def cf_takeoff_safely(cf, mr, z0):
"""
Take off slowly using hover setpoints, while avoiding collisions.
"""
print('Take-off sequence initiated.')
cf_reset_ledring(cf, brightness=0)
z = z0
while (z < z0):
z += 0.1
print(f'z: {z}')
cf_hover_safely(cf, mr, z)
try:
cf.param.set_value('ring.solidRed', str(int(z*10)))
cf.param.set_value('ring.solidGreen', str(int(z*10)))
cf.param.set_value('ring.solidBlue', str(int(z*10)))
except Exception as e:
pass
sleep(0.1)
print('Hovering.')
def remap(val, inMin, inMax, outMin, outMax):
"""
Clamps value 'val' to input range [inMin, inMax] and remaps the clamped
value to output range [outMin, outMax]. Linear.
"""
if val < inMin:
val = inMin
if val > inMax:
val = inMax
return outMin + (val - inMin) * (outMax - outMin) / (inMax - inMin)