Skip to content

Commit 77b131f

Browse files
ArisMorgensArisMorgens
authored
Merge pull request #515 from bitcraze/Aris/Single_Drone_Reset_Estimator
Replaced reset estimator in single drone examples
2 parents 2a97614 + 119ce20 commit 77b131f

13 files changed

+74
-573
lines changed

‎cflib/utils/reset_estimator.py‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
import time
2+
3+
from cflib.crazyflie.log import LogConfig
4+
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
5+
from cflib.crazyflie.syncLogger import SyncLogger
6+
7+
8+
def reset_estimator(crazyflie):
9+
if isinstance(crazyflie, SyncCrazyflie):
10+
cf = crazyflie.cf
11+
else:
12+
cf = crazyflie
13+
cf.param.set_value('kalman.resetEstimation', '1')
14+
time.sleep(0.1)
15+
cf.param.set_value('kalman.resetEstimation', '0')
16+
17+
_wait_for_position_estimator(cf)
18+
19+
20+
def _wait_for_position_estimator(cf):
21+
print('Waiting for estimator to find position...', end='\r')
22+
23+
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
24+
log_config.add_variable('kalman.varPX', 'float')
25+
log_config.add_variable('kalman.varPY', 'float')
26+
log_config.add_variable('kalman.varPZ', 'float')
27+
28+
var_y_history = [1000] * 10
29+
var_x_history = [1000] * 10
30+
var_z_history = [1000] * 10
31+
32+
threshold = 0.001
33+
34+
with SyncLogger(cf, log_config) as logger:
35+
for log_entry in logger:
36+
data = log_entry[1]
37+
38+
var_x_history.append(data['kalman.varPX'])
39+
var_x_history.pop(0)
40+
var_y_history.append(data['kalman.varPY'])
41+
var_y_history.pop(0)
42+
var_z_history.append(data['kalman.varPZ'])
43+
var_z_history.pop(0)
44+
45+
min_x = min(var_x_history)
46+
max_x = max(var_x_history)
47+
min_y = min(var_y_history)
48+
max_y = max(var_y_history)
49+
min_z = min(var_z_history)
50+
max_z = max(var_z_history)
51+
52+
if (max_x - min_x) < threshold and (
53+
max_y - min_y) < threshold and (
54+
max_z - min_z) < threshold:
55+
print('Waiting for estimator to find position...success!')
56+
break

‎examples/autonomy/autonomousSequence.py‎

Lines changed: 2 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -36,8 +36,8 @@
3636
from cflib.crazyflie import Crazyflie
3737
from cflib.crazyflie.log import LogConfig
3838
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
39-
from cflib.crazyflie.syncLogger import SyncLogger
4039
from cflib.utils import uri_helper
40+
from cflib.utils.reset_estimator import reset_estimator
4141

4242
# URI to the Crazyflie to connect to
4343
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
@@ -56,56 +56,6 @@
5656
]
5757

5858

59-
def wait_for_position_estimator(scf):
60-
print('Waiting for estimator to find position...')
61-
62-
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
63-
log_config.add_variable('kalman.varPX', 'float')
64-
log_config.add_variable('kalman.varPY', 'float')
65-
log_config.add_variable('kalman.varPZ', 'float')
66-
67-
var_y_history = [1000] * 10
68-
var_x_history = [1000] * 10
69-
var_z_history = [1000] * 10
70-
71-
threshold = 0.001
72-
73-
with SyncLogger(scf, log_config) as logger:
74-
for log_entry in logger:
75-
data = log_entry[1]
76-
77-
var_x_history.append(data['kalman.varPX'])
78-
var_x_history.pop(0)
79-
var_y_history.append(data['kalman.varPY'])
80-
var_y_history.pop(0)
81-
var_z_history.append(data['kalman.varPZ'])
82-
var_z_history.pop(0)
83-
84-
min_x = min(var_x_history)
85-
max_x = max(var_x_history)
86-
min_y = min(var_y_history)
87-
max_y = max(var_y_history)
88-
min_z = min(var_z_history)
89-
max_z = max(var_z_history)
90-
91-
# print("{} {} {}".
92-
# format(max_x - min_x, max_y - min_y, max_z - min_z))
93-
94-
if (max_x - min_x) < threshold and (
95-
max_y - min_y) < threshold and (
96-
max_z - min_z) < threshold:
97-
break
98-
99-
100-
def reset_estimator(scf):
101-
cf = scf.cf
102-
cf.param.set_value('kalman.resetEstimation', '1')
103-
time.sleep(0.1)
104-
cf.param.set_value('kalman.resetEstimation', '0')
105-
106-
wait_for_position_estimator(cf)
107-
108-
10959
def position_callback(timestamp, data, logconf):
11060
x = data['kalman.stateX']
11161
y = data['kalman.stateY']
@@ -154,5 +104,5 @@ def run_sequence(scf, sequence):
154104

155105
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
156106
reset_estimator(scf)
157-
# start_position_printing(scf)
107+
start_position_printing(scf)
158108
run_sequence(scf, sequence)

‎examples/autonomy/autonomous_sequence_high_level.py‎

Lines changed: 1 addition & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -33,12 +33,11 @@
3333

3434
import cflib.crtp
3535
from cflib.crazyflie import Crazyflie
36-
from cflib.crazyflie.log import LogConfig
3736
from cflib.crazyflie.mem import MemoryElement
3837
from cflib.crazyflie.mem import Poly4D
3938
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
40-
from cflib.crazyflie.syncLogger import SyncLogger
4139
from cflib.utils import uri_helper
40+
from cflib.utils.reset_estimator import reset_estimator
4241

4342
# URI to the Crazyflie to connect to
4443
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
@@ -62,55 +61,6 @@
6261
]
6362

6463

65-
def wait_for_position_estimator(scf):
66-
print('Waiting for estimator to find position...')
67-
68-
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
69-
log_config.add_variable('kalman.varPX', 'float')
70-
log_config.add_variable('kalman.varPY', 'float')
71-
log_config.add_variable('kalman.varPZ', 'float')
72-
73-
var_y_history = [1000] * 10
74-
var_x_history = [1000] * 10
75-
var_z_history = [1000] * 10
76-
77-
threshold = 0.001
78-
79-
with SyncLogger(scf, log_config) as logger:
80-
for log_entry in logger:
81-
data = log_entry[1]
82-
83-
var_x_history.append(data['kalman.varPX'])
84-
var_x_history.pop(0)
85-
var_y_history.append(data['kalman.varPY'])
86-
var_y_history.pop(0)
87-
var_z_history.append(data['kalman.varPZ'])
88-
var_z_history.pop(0)
89-
90-
min_x = min(var_x_history)
91-
max_x = max(var_x_history)
92-
min_y = min(var_y_history)
93-
max_y = max(var_y_history)
94-
min_z = min(var_z_history)
95-
max_z = max(var_z_history)
96-
97-
# print("{} {} {}".
98-
# format(max_x - min_x, max_y - min_y, max_z - min_z))
99-
100-
if (max_x - min_x) < threshold and (
101-
max_y - min_y) < threshold and (
102-
max_z - min_z) < threshold:
103-
break
104-
105-
106-
def reset_estimator(cf):
107-
cf.param.set_value('kalman.resetEstimation', '1')
108-
time.sleep(0.1)
109-
cf.param.set_value('kalman.resetEstimation', '0')
110-
111-
wait_for_position_estimator(cf)
112-
113-
11464
def activate_mellinger_controller(cf):
11565
cf.param.set_value('stabilizer.controller', '2')
11666

‎examples/autonomy/autonomous_sequence_high_level_compressed.py‎

Lines changed: 1 addition & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -34,13 +34,12 @@
3434
import cflib.crtp
3535
from cflib.crazyflie import Crazyflie
3636
from cflib.crazyflie.high_level_commander import HighLevelCommander
37-
from cflib.crazyflie.log import LogConfig
3837
from cflib.crazyflie.mem import CompressedSegment
3938
from cflib.crazyflie.mem import CompressedStart
4039
from cflib.crazyflie.mem import MemoryElement
4140
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
42-
from cflib.crazyflie.syncLogger import SyncLogger
4341
from cflib.utils import uri_helper
42+
from cflib.utils.reset_estimator import reset_estimator
4443

4544
# URI to the Crazyflie to connect to
4645
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
@@ -58,55 +57,6 @@
5857
]
5958

6059

61-
def wait_for_position_estimator(scf):
62-
print('Waiting for estimator to find position...')
63-
64-
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
65-
log_config.add_variable('kalman.varPX', 'float')
66-
log_config.add_variable('kalman.varPY', 'float')
67-
log_config.add_variable('kalman.varPZ', 'float')
68-
69-
var_y_history = [1000] * 10
70-
var_x_history = [1000] * 10
71-
var_z_history = [1000] * 10
72-
73-
threshold = 0.001
74-
75-
with SyncLogger(scf, log_config) as logger:
76-
for log_entry in logger:
77-
data = log_entry[1]
78-
79-
var_x_history.append(data['kalman.varPX'])
80-
var_x_history.pop(0)
81-
var_y_history.append(data['kalman.varPY'])
82-
var_y_history.pop(0)
83-
var_z_history.append(data['kalman.varPZ'])
84-
var_z_history.pop(0)
85-
86-
min_x = min(var_x_history)
87-
max_x = max(var_x_history)
88-
min_y = min(var_y_history)
89-
max_y = max(var_y_history)
90-
min_z = min(var_z_history)
91-
max_z = max(var_z_history)
92-
93-
# print("{} {} {}".
94-
# format(max_x - min_x, max_y - min_y, max_z - min_z))
95-
96-
if (max_x - min_x) < threshold and (
97-
max_y - min_y) < threshold and (
98-
max_z - min_z) < threshold:
99-
break
100-
101-
102-
def reset_estimator(cf):
103-
cf.param.set_value('kalman.resetEstimation', '1')
104-
time.sleep(0.1)
105-
cf.param.set_value('kalman.resetEstimation', '0')
106-
107-
wait_for_position_estimator(cf)
108-
109-
11060
def activate_mellinger_controller(cf):
11161
cf.param.set_value('stabilizer.controller', '2')
11262

‎examples/autonomy/circling_square_demo.py‎

Lines changed: 1 addition & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -36,13 +36,11 @@
3636

3737
import cflib.crtp
3838
from cflib.crazyflie.high_level_commander import HighLevelCommander
39-
from cflib.crazyflie.log import LogConfig
4039
from cflib.crazyflie.mem import CompressedSegment
4140
from cflib.crazyflie.mem import CompressedStart
4241
from cflib.crazyflie.mem import MemoryElement
4342
from cflib.crazyflie.swarm import CachedCfFactory
4443
from cflib.crazyflie.swarm import Swarm
45-
from cflib.crazyflie.syncLogger import SyncLogger
4644

4745
URI1 = 'radio://0/60/2M/E7E7E7E710'
4846
URI2 = 'radio://0/60/2M/E7E7E7E711'
@@ -72,55 +70,6 @@ def rotate_beizer_node(xl, yl, alpha):
7270
return x_rot, y_rot
7371

7472

75-
def wait_for_position_estimator(scf):
76-
print('Waiting for estimator to find position...')
77-
78-
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
79-
log_config.add_variable('kalman.varPX', 'float')
80-
log_config.add_variable('kalman.varPY', 'float')
81-
log_config.add_variable('kalman.varPZ', 'float')
82-
83-
var_y_history = [1000] * 10
84-
var_x_history = [1000] * 10
85-
var_z_history = [1000] * 10
86-
87-
threshold = 0.001
88-
89-
with SyncLogger(scf, log_config) as logger:
90-
for log_entry in logger:
91-
data = log_entry[1]
92-
93-
var_x_history.append(data['kalman.varPX'])
94-
var_x_history.pop(0)
95-
var_y_history.append(data['kalman.varPY'])
96-
var_y_history.pop(0)
97-
var_z_history.append(data['kalman.varPZ'])
98-
var_z_history.pop(0)
99-
100-
min_x = min(var_x_history)
101-
max_x = max(var_x_history)
102-
min_y = min(var_y_history)
103-
max_y = max(var_y_history)
104-
min_z = min(var_z_history)
105-
max_z = max(var_z_history)
106-
107-
# print("{} {} {}".
108-
# format(max_x - min_x, max_y - min_y, max_z - min_z))
109-
110-
if (max_x - min_x) < threshold and (
111-
max_y - min_y) < threshold and (
112-
max_z - min_z) < threshold:
113-
break
114-
115-
116-
def reset_estimator(cf):
117-
cf.param.set_value('kalman.resetEstimation', '1')
118-
time.sleep(0.1)
119-
cf.param.set_value('kalman.resetEstimation', '0')
120-
121-
wait_for_position_estimator(cf)
122-
123-
12473
def activate_high_level_commander(cf):
12574
cf.param.set_value('commander.enHighLevel', '1')
12675

@@ -234,6 +183,7 @@ def upload_trajectories(scf, alpha, r):
234183

235184
factory = CachedCfFactory(rw_cache='./cache')
236185
with Swarm(uris, factory=factory) as swarm:
186+
swarm.reset_estimators()
237187
swarm.parallel_safe(turn_off_leds)
238188
swarm.parallel_safe(upload_trajectories, args_dict=position_params)
239189
time.sleep(5)

0 commit comments

Comments
 (0)