forked from bitcraze/crazyflie-lib-python
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathbasicparam.py
More file actions
169 lines (140 loc) · 6.57 KB
/
Copy pathbasicparam.py
File metadata and controls
169 lines (140 loc) · 6.57 KB
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
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2014 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Simple example that connects to the first Crazyflie found, triggers
reading of all the parameters and displays their values. It then modifies
one parameter and reads back it's value. Finally it disconnects.
"""
import logging
import random
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
class ParamExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
# Connect some callbacks from the Crazyflie API
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)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
self._param_check_list = []
self._param_groups = []
random.seed()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
# Print the param TOC
p_toc = self._cf.param.toc.toc
for group in sorted(p_toc.keys()):
print('{}'.format(group))
for param in sorted(p_toc[group].keys()):
print('\t{}'.format(param))
self._param_check_list.append('{0}.{1}'.format(group, param))
self._param_groups.append('{}'.format(group))
# For every group, register the callback
self._cf.param.add_update_callback(group=group, name=None,
cb=self._param_callback)
# You can also register a callback for a specific group.name combo
self._cf.param.add_update_callback(group='cpu', name='flash',
cb=self._cpu_flash_callback)
print('')
def _cpu_flash_callback(self, name, value):
"""Specific callback for the cpu.flash parameter"""
print('The connected Crazyflie has {}kb of flash'.format(value))
def _param_callback(self, name, value):
"""Generic callback registered for all the groups"""
print('{0}: {1}'.format(name, value))
# Remove each parameter from the list and close the link when
# all are fetched
self._param_check_list.remove(name)
if len(self._param_check_list) == 0:
print('Have fetched all parameter values.')
# First remove all the group callbacks
for g in self._param_groups:
self._cf.param.remove_update_callback(group=g,
cb=self._param_callback)
# Create a new random value [0.00,1.00] for pid_attitude.pitch_kd
# and set it
pkd = random.random()
print('')
print('Write: pid_attitude.pitch_kd={:.2f}'.format(pkd))
self._cf.param.add_update_callback(group='pid_attitude',
name='pitch_kd',
cb=self._a_pitch_kd_callback)
# When setting a value the parameter is automatically read back
# and the registered callbacks will get the updated value
self._cf.param.set_value('pid_attitude.pitch_kd',
'{:.2f}'.format(pkd))
def _a_pitch_kd_callback(self, name, value):
"""Callback for pid_attitude.pitch_kd"""
print('Readback: {0}={1}'.format(name, value))
# End the example by closing the link (will cause the app to quit)
self._cf.close_link()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) > 0:
pe = ParamExample(available[0][0])
# The Crazyflie lib doesn't contain anything to keep the application
# alive, so this is where your application should do something. In our
# case we are just waiting until we are disconnected.
while pe.is_connected:
time.sleep(1)
else:
print('No Crazyflies found, cannot run example')