def land_on_elevated_surface():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, default_landing_height=0.35) as pc:
# fly onto a landing platform at non-zero height (ex: from floor to desk, etc)
pc.forward(1.0)
pc.left(1.0)
def __init__(self, URI):
cflib.crtp.init_drivers(enable_debug_driver=False)
self.URI = URI
self.scf = SyncCrazyflie(self.URI, cf=Crazyflie(rw_cache='./cache'))
def _flash_deck(self, artifacts: List[FlashArtifact],
targets: List[Target]):
flash_all_targets = len(targets) == 0
if self.progress_cb:
self.progress_cb('Detecting deck to be updated', int(25))
with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf:
deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY)
deck_mems_count = len(deck_mems)
if deck_mems_count == 0:
return
mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0])
decks = mgr.query_decks()
for (deck_index, deck) in decks.items():
if self.terminate_flashing_cb and self.terminate_flashing_cb():
raise Exception('Flashing terminated')
# Check that we want to flash this deck
deck_target = [
t for t in targets if t == Target('deck', deck.name, 'fw')
]
if (not flash_all_targets) and len(deck_target) == 0:
print(f'Skipping {deck.name}')
continue
# Check that we have an artifact for this deck
deck_artifacts = [
a for a in artifacts
if a.target == Target('deck', deck.name, 'fw')
]
if len(deck_artifacts) == 0:
print(
f'Skipping {deck.name}, no artifact for it in the .zip'
)
continue
deck_artifact = deck_artifacts[0]
if self.progress_cb:
self.progress_cb(f'Updating deck {deck.name}', int(50))
print(f'Handling {deck.name}')
# Test and wait for the deck to be started
while not deck.is_started:
print('Deck not yet started ...')
time.sleep(500)
deck = mgr.query_decks()[deck_index]
# Run a brunch of sanity checks ...
if not deck.supports_fw_upgrade:
print(
f'Deck {deck.name} does not support firmware update, skipping!'
)
continue
if not deck.is_fw_upgrade_required:
print(f'Deck {deck.name} firmware up to date, skipping')
continue
if not deck.is_bootloader_active:
print(
f'Error: Deck {deck.name} bootloader not active, skipping!'
)
continue
# ToDo, white the correct file there ...
result = deck.write_sync(0, deck_artifact.content)
if result:
if self.progress_cb:
self.progress_cb(
f'Deck {deck.name} updated succesfully!', int(75))
else:
if self.progress_cb:
self.progress_cb(f'Failed to update deck {deck.name}',
int(0))
raise Exception(f'Failed to update deck {deck.name}')
def _flash_deck_incrementally(self, artifacts: List[FlashArtifact],
targets: List[Target], start_index: int):
flash_all_targets = len(targets) == 0
if self.progress_cb:
self.progress_cb('Identifying deck to be updated', 0)
with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf:
# Uncomment to enable console logs from the CF.
# scf.cf.console.receivedChar.add_callback(self.console_callback)
deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY)
deck_mems_count = len(deck_mems)
if deck_mems_count == 0:
return -1
mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0])
try:
decks = mgr.query_decks()
except RuntimeError as e:
if self.progress_cb:
message = f'Failed to read decks: {str(e)}'
self.progress_cb(message, 0)
logger.error(message)
time.sleep(2)
raise RuntimeError(message)
for (deck_index, deck) in decks.items():
if self.terminate_flashing_cb and self.terminate_flashing_cb():
raise Exception('Flashing terminated')
# Skip decks up to the start_index
if deck_index < start_index:
continue
# Check that we want to flash this deck
deck_target = [
t for t in targets if t == Target('deck', deck.name, 'fw')
]
if (not flash_all_targets) and len(deck_target) == 0:
print(f'Skipping {deck.name}, not in the target list')
continue
# Check that we have an artifact for this deck
deck_artifacts = [
a for a in artifacts
if a.target == Target('deck', deck.name, 'fw')
]
if len(deck_artifacts) == 0:
print(
f'Skipping {deck.name}, no artifact for it in the .zip'
)
continue
deck_artifact = deck_artifacts[0]
if self.progress_cb:
self.progress_cb(f'Updating deck {deck.name}', 0)
# Test and wait for the deck to be started
timeout_time = time.time() + 5
while not deck.is_started:
if time.time() > timeout_time:
raise RuntimeError(f'Deck {deck.name} did not start')
print('Deck not yet started ...')
time.sleep(0.5)
deck = mgr.query_decks()[deck_index]
# Supports upgrades?
if not deck.supports_fw_upgrade:
print(
f'Deck {deck.name} does not support firmware update, skipping!'
)
continue
# Reset to bootloader mode, if supported
if deck.supports_reset_to_bootloader:
print(f'Deck {deck.name}, reset to bootloader')
deck.reset_to_bootloader()
time.sleep(0.1)
deck = mgr.query_decks()[deck_index]
else:
# Is an upgrade required?
if not deck.is_fw_upgrade_required:
print(
f'Deck {deck.name} firmware up to date, skipping')
continue
# Wait for bootloader to be ready
timeout_time = time.time() + 5
while not deck.is_bootloader_active:
if time.time() > timeout_time:
raise RuntimeError(
f'Deck {deck.name} did not enter bootloader mode')
print(
f'Error: Deck {deck.name} bootloader not active yet...'
)
time.sleep(0.5)
deck = mgr.query_decks()[deck_index]
progress_cb = self.progress_cb
if not progress_cb:
def progress_cb(msg: str, percent: int):
frames = ['|', '/', '-', '\\']
frame = frames[int(percent) % 4]
print(f'{frame} {percent}% {msg}')
# Flash the new firmware
deck.set_fw_new_flash_size(len(deck_artifact.content))
result = deck.write_sync(0, deck_artifact.content, progress_cb)
if result:
if self.progress_cb:
self.progress_cb(
f'Deck {deck.name} updated successful!', 100)
else:
if self.progress_cb:
self.progress_cb(f'Failed to update deck {deck.name}',
int(0))
raise RuntimeError(f'Failed to update deck {deck.name}')
# We flashed a deck, return for re-boot
next_index = deck_index + 1
if next_index >= len(decks):
next_index = -1
return next_index
# We have flashed the last deck
return -1
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
self.cfg = Config()
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers()
# Create the connection dialogue
self.connectDialogue = ConnectDialogue()
# Create and start the Input Reader
self._statusbar_label = QLabel("Loading device and configuration.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
self._update_input
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.connectionFailed.add_callback(self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self.connectionFailed)
self.joystickReader.inputDeviceErrorSignal.connect(
self.inputDeviceError)
self.joystickReader.discovery_signal.connect(self.device_discovery)
# Connect UI signals
self.menuItemConnect.triggered.connect(self.connectButtonClicked)
self.logConfigAction.triggered.connect(self.doLogConfigDialogue)
self.connectButton.clicked.connect(self.connectButtonClicked)
self.quickConnectButton.clicked.connect(self.quickConnect)
self.menuItemQuickConnect.triggered.connect(self.quickConnect)
self.menuItemConfInputDevice.triggered.connect(self.configInputDevice)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self.updateBatteryVoltage)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
# Do not queue data from the controller output to the Crazyflie wrapper
# to avoid latency
self.joystickReader.sendControlSetpointSignal.connect(
self.cf.commander.send_setpoint, Qt.DirectConnection)
# Connection callbacks and signal wrappers for UI protection
self.cf.connectSetupFinished.add_callback(
self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI))
self.cf.connectionLost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self.connectionLost)
self.cf.connectionInitiated.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI))
# Connect link quality feedback
self.cf.linkQuality.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
# Set UI state in disconnected buy default
self.setUIState(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader()
self.logConfigReader.readConfigFiles()
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxes = []
self.toolboxesMenuItem.setMenu(QtGui.QMenu())
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [
dockToolbox,
]
# Add menu item for the toolbox
item = QtGui.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.menu().addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# Load and connect tabs
self.tabsMenuItem.setMenu(QtGui.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtGui.QAction(tab.getMenuName(), self)
item.setCheckable(True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.menu().addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
# First instantiate all tabs and then open them in the correct order
try:
for tName in Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t != None):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [%s]", e)
def __init__(self, options):
self.options = options
self.crazyflie = Crazyflie()
self.battery_monitor = BatteryMonitor()
cflib.crtp.init_drivers()
self.zspeed = zSpeed()
# Keep track of this to know if the user changed imu mode to/from imu6/9
self.preMagcalib = None
#self.csvwriter = csv.writer(open('baro.csv', 'wb'), delimiter=',',quotechar='"', quoting=csv.QUOTE_MINIMAL)
#self.baro_start_time = None
# Some shortcuts
self.HZ100 = 10
self.HZ10 = 100
self.HZ1 = 1000
self.HZ500 = 2
self.HZ250 = 4
self.HZ125 = 8
self.HZ50 = 20
# Callbacks the CF driver calls
self.crazyflie.connectSetupFinished.add_callback(
self.connectSetupFinishedCB)
self.crazyflie.connectionFailed.add_callback(self.connectionFailedCB)
self.crazyflie.connectionInitiated.add_callback(
self.connectionInitiatedCB)
self.crazyflie.disconnected.add_callback(self.disconnectedCB)
self.crazyflie.connectionLost.add_callback(self.connectionLostCB)
self.crazyflie.linkQuality.add_callback(self.linkQualityCB)
# Advertise publishers
self.pub_acc = rospy.Publisher("/cf/acc", accMSG)
self.pub_mag = rospy.Publisher("/cf/mag", magMSG)
self.pub_gyro = rospy.Publisher("/cf/gyro", gyroMSG)
self.pub_baro = rospy.Publisher("/cf/baro", baroMSG)
self.pub_motor = rospy.Publisher("/cf/motor", motorMSG)
self.pub_hover = rospy.Publisher("/cf/hover", hoverMSG)
self.pub_attitude = rospy.Publisher("/cf/attitude", attitudeMSG)
self.pub_bat = rospy.Publisher("/cf/bat", batMSG)
self.pub_zpid = rospy.Publisher("/cf/zpid", zpidMSG)
self.pub_tf = tf.TransformBroadcaster()
# Subscribers
self.sub_tf = tf.TransformListener()
self.sub_joy = rospy.Subscriber("/cfjoy", joyMSG, self.new_joydata)
self.sub_magCalib = rospy.Subscriber("/magCalib", magCalibMSG,
self.new_magCalib)
# Keep track of link quality to send out with battery status
self.link = 0
# Loggers
self.logMotor = None
self.logBaro = None
self.logMag = None
self.logGyro = None
self.logAttitude = None
self.logAcc = None
self.logBat = None
self.logHover = None
self.logZPid = None
self.logGravOffset = None #only here till it works
# keep tracks if loggers are running
self.acc_monitor = False
self.gyro_monitor = False
self.baro_monitor = False
self.mag_monitor = False
self.bat_monitor = False
self.motor_monitor = False
self.attitude_monitor = False
self.hover_monitor = False
self.zpid_monitor = False
# CF params we wanna be able to change
self.cf_param_groups = ["hover", "sensorfusion6", "magCalib"]
self.cf_params_cache = {}
# Dynserver
self.dynserver = DynamicReconfigureServer(driverCFG, self.reconfigure)
# Finished set up. Connect to flie
self.connect(self.options)
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
URI = 'radio://0/80/250K'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
cf = scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
#DEFAULTS
cf.param.set_value('posCtlPid.xKp', '{:.2f}'.format(2))
cf.param.set_value('posCtlPid.xKi', '{:.2f}'.format(0))
cf.param.set_value('posCtlPid.xKd', '{:.2f}'.format(0))
cf.param.set_value('posCtlPid.yKp', '{:.2f}'.format(2))
cf.param.set_value('posCtlPid.yKi', '{:.2f}'.format(0))
cf.param.set_value('posCtlPid.yKd', '{:.2f}'.format(0))
while True:
if keyboard.is_pressed('w'):
def connect_and_estimate(uri: str):
"""Connect to a Crazyflie, collect data and estimate the geometry of the system"""
print(f'Step 1. Connecting to the Crazyflie on uri {uri}...')
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
print(' Connected')
print('')
print('In the 3 following steps we will define the coordinate system.')
print(
'Step 2. Put the Crazyflie where you want the origin of your coordinate system.'
)
origin = None
do_repeat = True
while do_repeat:
input('Press return when ready. ')
print(' Recording...')
measurement = record_angles_average(scf)
do_repeat = False
if measurement is not None:
origin = measurement
else:
do_repeat = True
print(
f'Step 3. Put the Crazyflie on the positive X-axis, exactly {REFERENCE_DIST} meters from the origin. '
+
'This position defines the direction of the X-axis, but it is also used for scaling of the system.'
)
x_axis = []
do_repeat = True
while do_repeat:
input('Press return when ready. ')
print(' Recording...')
measurement = record_angles_average(scf)
do_repeat = False
if measurement is not None:
x_axis = [measurement]
else:
do_repeat = True
print(
'Step 4. Put the Crazyflie somehere in the XY-plane, but not on the X-axis.'
)
print(
'Multiple samples can be recorded if you want to, type "r" before you hit enter to repeat the step.'
)
xy_plane = []
do_repeat = True
while do_repeat:
do_repeat = 'r' == input('Press return when ready. ').lower()
print(' Recording...')
measurement = record_angles_average(scf)
if measurement is not None:
xy_plane.append(measurement)
else:
do_repeat = True
print()
print(
'Step 5. We will now record data from the space you plan to fly in and optimize the base station '
+
'geometry based on this data. Move the Crazyflie around, try to cover all of the space, make sure '
+ 'all the base stations are received and do not move too fast.')
default_time = 20
recording_time = input(
f'Enter the number of seconds you want to record ({default_time} by default), '
+ 'recording starts when you hit enter. ')
recording_time_s = parse_recording_time(recording_time, default_time)
print(' Recording started...')
samples = record_angles_sequence(scf, recording_time_s)
print(' Recording ended')
print('Step 6. Estimating geometry...')
bs_poses = estimate_geometry(origin, x_axis, xy_plane, samples)
print(' Geometry estimated')
print('Step 7. Upload geometry to the Crazyflie')
input('Press enter to upload geometry. ')
upload_geometry(scf, bs_poses)
print('Geometry uploaded')
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
######################################################
### By lxrocks
### 'Skinny Progress Bar' tweak for Yosemite
### Tweak progress bar - artistic I am not - so pick your own colors !!!
### Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info(
"This is a MAC - checking if we can apply Progress Bar Stylesheet for Yosemite Skinny Bars "
)
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite:")
tcss = """
QProgressBar {
border: 2px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: #05B8CC;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite")
######################################################
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(
enable_debug_driver=GuiConfig().get("enable_debug_driver"))
# Create the connection dialogue
self.connectDialogue = ConnectDialogue()
# Create and start the Input Reader
self._statusbar_label = QLabel("Loading device and configuration.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
self._update_input
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self.connectionFailed)
self._input_device_error_signal.connect(self.inputDeviceError)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Connect UI signals
self.menuItemConnect.triggered.connect(self.connectButtonClicked)
self.logConfigAction.triggered.connect(self.doLogConfigDialogue)
self.connectButton.clicked.connect(self.connectButtonClicked)
self.quickConnectButton.clicked.connect(self.quickConnect)
self.menuItemQuickConnect.triggered.connect(self.quickConnect)
self.menuItemConfInputDevice.triggered.connect(self.configInputDevice)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self.updateBatteryVoltage)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(
GuiConfig().get("auto_reconnect"))
# Do not queue data from the controller output to the Crazyflie wrapper
# to avoid latency
#self.joystickReader.sendControlSetpointSignal.connect(
# self.cf.commander.send_setpoint,
# Qt.DirectConnection)
self.joystickReader.input_updated.add_callback(
self.cf.commander.send_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self.connectionLost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI))
self._log_error_signal.connect(self._logging_error)
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
# Set UI state in disconnected buy default
self.setUIState(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxes = []
self.toolboxesMenuItem.setMenu(QtGui.QMenu())
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [
dockToolbox,
]
# Add menu item for the toolbox
item = QtGui.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.menu().addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# Load and connect tabs
self.tabsMenuItem.setMenu(QtGui.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtGui.QAction(tab.getMenuName(), self)
item.setCheckable(True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.menu().addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in GuiConfig().get("open_tabs").split(","):
t = tabItems[tName]
if (t != None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [%s]", e)
print("Controller: " + controller_body_names[controller_select])
print("Offset: X: {:5.2f} Y: {:5.2f} Z: {:5.2f}".format(
controller_offset_x, controller_offset_y, controller_offset_z))
#
# ACTION
#
# Init Crazyflie drivers
cflib.crtp.init_drivers(enable_debug_driver=False)
# Connect to QTM
qtm_wrapper = QtmWrapper()
with SyncCrazyflie(cf_uri, cf=Crazyflie(rw_cache='./cache')) as scf:
cf = scf.cf
listener = keyboard.Listener(on_press=on_press)
listener.start()
# Slow down
cf.param.set_value('posCtlPid.xyVelMax', cf_max_vel)
cf.param.set_value('posCtlPid.zVelMax', cf_max_vel)
# Set active marker IDs
# Set up callbacks to handle data from QTM
qtm_wrapper.on_cf_pose = lambda pose: send_extpose_rot_matrix(
cf, pose[0], pose[1], pose[2], pose[3])
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.connect_input = QShortcut("Ctrl+I", self.connectButton,
self._connect)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
cfclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [dockToolbox, ]
# Add menu item for the toolbox
item = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self.enable_monitor()
self._mapping_support = True
if trigger:
curr = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]]
setpoints[closest] = vector_add(
grab_setpoint_start,
vector_substract(curr, grab_controller_start))
cf0.commander.send_position_setpoint(setpoints[0][0], setpoints[0][1],
setpoints[0][2], 0)
cf1.commander.send_position_setpoint(setpoints[1][0], setpoints[1][1],
setpoints[1][2], 0)
time.sleep(0.02)
cf0.commander.send_setpoint(0, 0, 0, 0)
cf1.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)
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(uri0, cf=Crazyflie(rw_cache='./cache')) as scf0:
reset_estimator(scf0)
with SyncCrazyflie(uri1, cf=Crazyflie(rw_cache='./cache')) as scf1:
reset_estimator(scf1)
run_sequence(scf0, scf1)
openvr.shutdown()
def construct(self, uri):
cf = Crazyflie(ro_cache=self.ro_cache, rw_cache=self.rw_cache)
return SyncCrazyflie(uri, cf=cf)
def main():
parser = argparse.ArgumentParser(
description='Set transmit power of LPS Anchors. A new power '
'configuration is sent to anchors using a Crazyflie with '
'an LPS deck as a bridge. IMPORTANT: There are '
'regulations on how much energy a '
'radio source is allowed to transmit, and the regulations '
'are different from country to country. It might be '
'possible that some configurations that this script can '
'set are not within limits in some countries. If you '
'choose to run this script and modify the transmit power, '
'make sure it is within limits where you will use the '
'anchor.')
parser.add_argument('-u',
'--uri',
dest='uri',
default='radio://0/80/2M/E7E7E7E7E7',
help='The URI of the Crazyflie (default '
'radio://0/80/2M/E7E7E7E7E7)')
parser.add_argument('-b',
'--begin',
dest='anchor_id_lower',
default=0,
type=int,
help='The lower id in the anchor range to send to '
'(default 0)')
parser.add_argument('-e',
'--end',
dest='anchor_id_upper',
default=7,
type=int,
help='The upper id in the anchor range to send to '
'(default 7)')
valid_powers = list(
map(lambda x: ('%.1f' % (x / 2)).replace(".0", ""), range(1, 68)))
valid_powers.append('default')
parser.add_argument('power',
type=str,
choices=valid_powers,
help='The transmit power to use. Valid values are '
'between 0.5 and 33.5 (dB) in steps of 0.5 or '
'"default". "default" will set the power to the '
'default power as well as configure the anchor '
'to use smart power.')
args = parser.parse_args()
unit = 'dB'
if args.power == 'default':
unit = ' (smart)'
validate_anchor_id(args.anchor_id_lower)
validate_anchor_id(args.anchor_id_upper)
if args.anchor_id_lower > args.anchor_id_upper:
raise Exception("(%i to %i) is not a valid anchor id range" %
(args.anchor_id_lower, args.anchor_id_upper))
(power_conf, force, smart) = generate_config(args.power)
power_pack = generate_lpp_packet(power_conf, force, smart)
print('Sending anchor transmit power configuration to anchors (%i to %i) '
'using %s. Setting transmit power to %s%s. Anchors will reset'
'when configured.' % (args.anchor_id_lower, args.anchor_id_upper,
args.uri, args.power, unit))
logging.basicConfig(level=logging.ERROR)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(args.uri, cf=cf) as scf:
print('Starting transmission. Terminate with CTRL+C.')
while True:
for id in range(args.anchor_id_lower, args.anchor_id_upper + 1):
print('Anchor %i' % id)
for _ in range(7):
scf.cf.loc.send_short_lpp_packet(id, power_pack)
time.sleep(0.2)
time.sleep(0.5)
position_internal[i] = position[i]
time.sleep(0.1)
if __name__ == '__main__':
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()
#time.sleep(0.5)
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) > 0:
with SyncCrazyflie(available[0][0] + '/E7E7E7E7E8', cf=Crazyflie(rw_cache='./cache')) as scf:
reset_estimator(scf)
if sys.argv[1] == 'l':
hourglass_path(scf)
elif sys.argv[1] == 'p':
corner_path(scf)
elif sys.argv[1] == 'v':
square_path(scf)
else: #sys.argv[1] == 'm':
right_path(scf)
go_circular(scf, 360, 1, 0.4, 0, 5, DT)
left_path(scf)
go_circular(scf, 360, 1, 0.4, 0, 5, DT)
def crazyFlieLoop(self, radio_id, drone_id):
# All uneven drones are assigned the channel below
# if drone_id % 2 == 0:
channel = drone_id * 10
#else:
# channel = (drone_id-1)*10
# The complete uri of the crazyfly
URI = 'radio://' + str(radio_id) + '/' + str(
channel) + '/2M/E7E7E7E70' + str(drone_id)
print("Connect to ", URI)
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Open up the logging file, seperated by files per date
datestr = "../../experiments/" + time.strftime("%y%m%d")
if os.path.exists(datestr) == False:
os.mkdir(datestr)
#in
first_run = True
pos_x_int = 0
pos_y_int = 0
rssi_array = []
rssi_heading_array = []
stop_looping = False
start_time = time.time()
outbound = 1
reconnect = True
radio = crazyradio.Crazyradio()
number_count = 0
while reconnect:
self.time_keep_flying = 0
try:
cf = Crazyflie(rw_cache='./cache')
cf.param.add_update_callback(group="gbug",
name="keep_flying",
cb=self.param_keepflying_callback)
cf.param.add_update_callback(group="gbug",
name="outbound",
cb=self.param_outbound_callback)
lg_states = LogConfig(name='kalman_states', period_in_ms=500)
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
#lg_states.add_variable('radio.rssi')
#lg_states.add_variable('radio.rssi_inter')
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('gradientbug.rssi_angle')
lg_states.add_variable('gradientbug.state')
lg_states.add_variable('gradientbug.state_wf')
lg_states.add_variable('gradientbug.rssi_beacon')
lg_states.add_variable('gradientbug.rssi_angle_i')
lg_states.add_variable('gradientbug.rssi_i')
# lg_states.add_variable('stateEstimate.z')
lg_states.data_received_cb.add_callback(self.data_received)
cf.param.add_update_callback(group="gbug",
name="sendnum",
cb=self.param_sendnum_callback)
time.sleep(drone_id)
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_states) as logger_states:
reconnect = False
time.sleep(2)
while 1:
#pk = radio.send_packet((3,4,88,48 ))
#if pk.ack:
#print "got back!"
time.sleep(0.5)
#cf.param.request_param_update("gbug.keep_flying")
#cf.param.request_param_update("gbug.outbound")
time_delta = time.time() - start_time
param_name = "gbug.sendnum"
param_value = str(int(time_delta * 2) % 10)
print("param", str(int(time_delta * 2) % 10))
cf.param.set_value(param_name, param_value)
cf.param.request_param_update(param_name)
number_count = number_count + 1
print(time.time() - self.time_keep_flying)
# if (time.time()-self.time_keep_flying)>5:
# raise Exception('parameter not received')
if (time.time() - start_time) % 5:
if scf.is_link_open() == False:
raise Exception('link is not open')
if (time.time() - self.time_send_num
) > 5 and self.time_send_num is not 0:
raise Exception('lost parameter link')
except Exception as ex:
print ex
reconnect = True
import sys
sys.path.append("../lib")
import cflib.crtp
import time
from cflib.crazyflie import Crazyflie
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
print("Scanning interfaces for Crazyflies...")
if True:
# Create a Crazyflie object without specifying any cache dirs
cf = Crazyflie()
def handle_connected(link_uri):
print("Connected to %s" % link_uri)
print("Sending thrust 45000")
cf.commander.send_setpoint(0, 0, 0, 45000)
time.sleep(0.75)
print("Stopping thrust; hovering")
cf.commander.send_setpoint(0, 0, 0, 32767)
cf.param.set_value("flightmode.althold", "True")
def close_link():
print('Closing')
cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
# 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:
print('No Crazyflies found, cannot run example')
else:
lg_acc = LogConfig(name='Acelerometer', period_in_ms=100)
lg_acc.add_variable('acc.z', 'float')
lg_acc.add_variable('acc.x', 'float')
lg_acc.add_variable('acc.y', 'float')
with SyncCrazyflie(available[0][0],
cf=Crazyflie(rw_cache='./cache')) as scf:
with SyncLogger(scf, lg_acc) as logger:
endTime = time.time() + 1
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
movement(0, 0, 0, 0.3, 10)
movement(0.5, 0, 0, 0.3, 50)
logfile.close()
"""if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
def run(self):
cap, resolution = init_cv()
t = threading.current_thread()
cf, URI = init_cf()
if cf is None:
print('Not running cf code.')
return
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
parameters = aruco.DetectorParameters_create()
font = cv2.FONT_HERSHEY_PLAIN
id2find = [0, 1]
marker_size = [0.112, 0.0215] #0.1323
ids_seen = [0, 0]
id2follow = 0
zvel = 0.0
camera_matrix, camera_distortion, _ = loadCameraParams('runcam_nano3')
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
cf = scf.cf
activate_high_level_commander(cf)
# We take off when the commander is created
with PositionHlCommander(scf) as pc:
#pc.go_to(0.0, 0.0, 0.5)
while not self._stopevent.isSet():
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(
image=gray,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
if ids is not None:
#-- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners)
#-- Calculate which marker has been seen most at late
if 0 in ids:
ids_seen[0] += 1
else:
ids_seen[0] = 0
if 1 in ids:
ids_seen[1] += 2
else:
ids_seen[1] = 0
id2follow = 0 #np.argmax(ids_seen)
idx_r, idx_c = np.where(ids == id2follow)
#-- Extract the id to follow
corners = np.asarray(corners)
corners = corners[idx_r, :]
#-- Estimate poses of the marker to follow
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, marker_size[id2follow], camera_matrix,
camera_distortion)
#-- Unpack the output, get only the first
rvec, tvec = rvecs[0, 0, :], tvecs[0, 0, :]
# Draw the detected markers axis
for i in range(len(rvecs)):
aruco.drawAxis(frame, camera_matrix,
camera_distortion, rvecs[i, 0, :],
tvecs[i, 0, :],
marker_size[id2follow] / 2)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
#-- Now get Position and attitude of the camera respect to the marker
if id2follow == 0:
pos_camera = -R_tc * (np.matrix(tvec).T - T_slide)
else:
pos_camera = -R_tc * (np.matrix(tvec).T)
str_position = "Position error: x=%4.4f y=%4.4f z=%4.4f" % (
pos_camera[0], pos_camera[1], pos_camera[2])
cv2.putText(frame, str_position, (0, 20), font, 1,
ugly_const.BLACK, 2, cv2.LINE_AA)
#-- Get the attitude of the camera respect to the frame
roll_camera, pitch_camera, yaw_camera = rotationMatrixToEulerAngles(
R_flip * R_tc)
att_camera = [
math.degrees(roll_camera),
math.degrees(pitch_camera),
math.degrees(yaw_camera)
]
str_attitude = "Anglular error: roll=%4.4f pitch=%4.4f yaw (z)=%4.4f" % (
att_camera[0], att_camera[1], att_camera[2])
cv2.putText(frame, str_attitude, (0, 40), font, 1,
ugly_const.BLACK, 2, cv2.LINE_AA)
drawHUD(frame, resolution, yaw_camera)
pos_flip = np.array([[-pos_camera.item(1)],
[pos_camera.item(0)]])
cmd_flip = np.array(
[[np.cos(yaw_camera), -np.sin(yaw_camera)],
[np.sin(yaw_camera),
np.cos(yaw_camera)]])
pos_cmd = cmd_flip.dot(pos_flip)
cf.extpos.send_extpos(pos_cmd[0], pos_cmd[1],
pos_cmd[2])
print('Following tag ', id2follow, ' with position ',
pos_cmd[0], pos_cmd[1])
#if np.sqrt(pos_cmd[0]*pos_cmd[0]+pos_cmd[1]*pos_cmd[1]) > 0.05:
#mc._set_vel_setpoint(pos_cmd[0]*ugly_const.Kx, pos_cmd[1]*ugly_const.Ky, zvel, -att_camera[2]*ugly_const.Kyaw)
# elif pos_camera.item(2) > 0.1:
# mc._set_vel_setpoint(pos_cmd[0]*ugly_const.Kx, pos_cmd[1]*ugly_const.Ky, -0.05, -att_camera[2]*ugly_const.Kyaw)
# else:
# mc._set_vel_setpoint(pos_cmd[0]*ugly_const.Kx, pos_cmd[1]*ugly_const.Ky, 0.05, -att_camera[2]*ugly_const.Kyaw)
pc.go_to(0.0, 0.0)
cv2.imshow('frame', frame)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cap.release()
cv2.destroyAllWindows()
elif key == ord('w'):
zvel += 0.1
elif key == ord('s'):
zvel -= 0.1
# We land when the commander goes out of scope
print('Landing...')
print('Stopping cf_thread')
def run(self):
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
scan_rate = rospy.Rate(2)
while not rospy.is_shutdown():
# Use first Crazyflie found
rospy.loginfo('Crazyflie FC Comms scanning for Crazyflies')
crazyflies = cflib.crtp.scan_interfaces()
if len(crazyflies) != 0:
break
rospy.logerr(
'Crazyflie FC Comms could not find Crazyflie. Trying again.')
scan_rate.sleep()
uri = crazyflies[0][0]
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connection_made)
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)
rospy.loginfo('Crazyflie FC Comms connecting to: {}'.format(uri))
self._cf.open_link(uri)
connected_check_rate = rospy.Rate(2)
while not self._connected and not rospy.is_shutdown():
connected_check_rate.sleep()
pass
rospy.loginfo('Crazyflie FC Comms reseting kalmann filter')
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
fast_log_stab = LogConfig(name='high_update_rate', period_in_ms=20)
medium_log_stab = LogConfig(name='medium_update_rate', period_in_ms=30)
slow_log_stab = LogConfig(name='slow_update_rate', period_in_ms=100)
fast_log_stab.add_variable('acc.x', 'float')
fast_log_stab.add_variable('acc.y', 'float')
fast_log_stab.add_variable('acc.z', 'float')
fast_log_stab.add_variable('stabilizer.roll', 'float')
fast_log_stab.add_variable('stabilizer.pitch', 'float')
fast_log_stab.add_variable('stabilizer.yaw', 'float')
# kalman_states.ox and kalman_states.oy are also available
# and provide position estimates
medium_log_stab.add_variable('kalman_states.vx', 'float')
medium_log_stab.add_variable('kalman_states.vy', 'float')
medium_log_stab.add_variable('range.zrange', 'uint16_t')
slow_log_stab.add_variable('pm.vbat', 'float')
slow_log_stab.add_variable('mag.x', 'float')
slow_log_stab.add_variable('mag.y', 'float')
slow_log_stab.add_variable('mag.z', 'float')
try:
self._cf.log.add_config(fast_log_stab)
self._cf.log.add_config(medium_log_stab)
self._cf.log.add_config(slow_log_stab)
fast_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
medium_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
slow_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
fast_log_stab.error_cb.add_callback(self._receive_crazyflie_error)
medium_log_stab.error_cb.add_callback(
self._receive_crazyflie_error)
slow_log_stab.error_cb.add_callback(self._receive_crazyflie_error)
fast_log_stab.start()
medium_log_stab.start()
slow_log_stab.start()
except KeyError as e:
rospy.logerr('Crazyflie FC Comms could not start logging,'
'{} not found'.format(str(e)))
except AttributeError as e:
rospy.logerr(
'Crazyflie FC Comms could not finishing configuring logging: {}'
.format(str(e)))
rospy.loginfo("Crazyflie FC Comms finished configured logs")
# Publish a landing gear contact message so that the landing detector can start up
switch_msg = LandingGearContactsStamped()
switch_msg.front = True
switch_msg.back = True
switch_msg.left = True
switch_msg.right = True
self._switch_pub.publish(switch_msg)
rospy.loginfo('Crazyflie FC Comms trying to form bond')
# forming bond with safety client
if not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'Crazyflie FC Comms could not form bond with safety client')
rospy.loginfo('Crazyflie FC Comms done forming bond')
# Update at the same rate as low level motion
rate = rospy.Rate(60)
while not rospy.is_shutdown() and self._connected:
status = FlightControllerStatus()
status.header.stamp = rospy.Time.now()
status.armed = self._armed
status.auto_pilot = True
status.failsafe = False
self._status_publisher.publish(status)
if self._safety_client.is_fatal_active():
raise IARCFatalSafetyException('Safety Client is fatal active')
if self._safety_client.is_safety_active():
rospy.logwarn_throttle(
1.0, 'Crazyflie FC Comms activated safety response')
self._commands_allowed = False
self._cf.commander.send_setpoint(0, 0, 0, 0)
if self._crash_detected:
rospy.logwarn_throttle(1.0,
'Crazyflie FC Comms detected crash')
self._commands_allowed = False
if self._current_height > 0.15 and not self._crash_landed:
self._cf.commander.send_setpoint(
0, 0, 0, 0.47 * 65535) # Max throttle is 65535
else:
self._crash_landed = True
rospy.logwarn_throttle(
1.0, 'Crazyflie FC Comms shut down motors')
self._cf.commander.send_setpoint(0, 0, 0, 0)
if self._armed and self._commands_allowed:
if self._uav_command is None:
# Keep the connection alive
self._cf.commander.send_setpoint(0, 0, 0, 0)
else:
throttle = max(10000.0, self._uav_command.throttle *
65535.0) # Max throttle is 65535
self._cf.commander.send_setpoint(
self._uav_command.data.roll * 180.0 / math.pi,
self._uav_command.data.pitch * 180.0 / math.pi,
-1.0 * self._uav_command.data.yaw * 180.0 / math.pi,
throttle)
rate.sleep()
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Give time for buffer to flush
time.sleep(0.1)
self._cf.close_link()
def is_close(range):
MIN_DISTANCE = 0.2 # m
if range is None:
return False
else:
return range < MIN_DISTANCE
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(URI, cf=cf) as scf:
with MotionCommander(scf) as motion_commander:
with MultiRanger(scf) as multi_ranger:
keep_flying = True
while keep_flying:
VELOCITY = 0.5
velocity_x = 0.0
velocity_y = 0.0
if is_close(multi_ranger.front):
velocity_x -= VELOCITY
if is_close(multi_ranger.back):
velocity_x += VELOCITY
def crazyFlieLoop(self, radio_id, drone_id):
# All uneven drones are assigned the channel below
#if drone_id % 2 == 0:
channel = drone_id * 10
#else:
# channel = (drone_id-1)*10
# The complete uri of the crazyfly
if drone_id < 10:
URI = 'radio://' + str(radio_id) + '/' + str(
channel) + '/2M/E7E7E7E70' + str(drone_id)
else:
URI = 'radio://' + str(radio_id) + '/' + str(
channel) + '/2M/E7E7E7E7' + str(drone_id)
print("Connect to ", URI)
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Open up the logging file, seperated by files per date
datestr = "../../experiments/" + time.strftime("%y%m%d")
if os.path.exists(datestr) == False:
os.mkdir(datestr)
timestr = time.strftime("%Y%m%d_%H%M%S")
fh = open(datestr + "/log_" + timestr + "_" + str(drone_id) + ".txt",
"w")
print("change crazyradio power")
cr = crazyradio.Crazyradio(None, radio_id)
cr.set_power(3)
#in
first_run = True
pos_x_int = 0
pos_y_int = 0
rssi_array = []
rssi_heading_array = []
stop_looping = False
start_time = 0
outbound = 1
reconnect = True
while reconnect:
try:
cf = Crazyflie(rw_cache='./cache')
cf.param.add_update_callback(group="gbug",
name="keep_flying",
cb=self.param_keepflying_callback)
lg_states = LogConfig(name='kalman_states', period_in_ms=100)
lg_states.add_variable('kalman_states.ox')
lg_states.add_variable('kalman_states.oy')
lg_states.add_variable('radio.rssi')
lg_states.add_variable('radio.rssi_inter')
lg_states.add_variable('stabilizer.yaw')
lg_states.add_variable('gradientbug.rssi_angle')
lg_states.add_variable('gradientbug.state')
lg_states.add_variable('gradientbug.state_wf')
lg_states.add_variable('gradientbug.rssi_beacon')
lg_states.add_variable('gradientbug.rssi_angle_i')
lg_states.add_variable('gradientbug.rssi_i')
# lg_states.add_variable('stateEstimate.z')
lg_states.data_received_cb.add_callback(self.data_received)
time.sleep(drone_id)
with SyncCrazyflie(URI, cf=cf) as scf:
with SyncLogger(scf, lg_states) as logger_states:
reconnect = False
time.sleep(2)
start_time = time.time()
while 1:
#print self.gradientbug_state
#print self.gradientbug_rssi_angle
#print("RSSI BEACON", self.gradientbug_rssi_beacon)
#print("rssi angle",self.gradientbug_rssi_angle)
time.sleep(0.03)
print("time", time.time())
print("RSSI", self.radio_rssi)
print("RSSI _inter", self.radio_rssi_inter)
#param_name = "gbug.keep_flying"
#param_value = "0"
#cf.param.set_value(param_name, param_value)
if first_run:
pos_x_int = self.kalman_states_ox
pos_y_int = self.kalman_states_oy
first_run = False
self.kalman_states_ox = self.kalman_states_ox - pos_x_int
self.kalman_states_oy = self.kalman_states_oy - pos_y_int
if time.time() - start_time > (
0) * 10 and self.keep_flying == 0:
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
cf.param.request_param_update(
"gbug.keep_flying")
print "don't fly!"
for event in pygame.event.get():
if event.type == KEYDOWN:
stop_looping = True
break
pygame.event.pump()
if stop_looping:
break
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
param_name = "gbug.keep_flying"
param_value = "0"
cf.param.set_value(param_name, param_value)
print "demo over"
except Exception as ex:
print ex
reconnect = True
time.sleep(2)
