From 6e366ee1ba1df36db5ba37f4bf518a07b2560a2a Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Tue, 7 Apr 2026 12:49:29 +0200 Subject: [PATCH 1/2] Removed most of the examples and added a README directing to the crazyflie-demos repository --- examples/README.md | 16 + examples/aideck/fpv.py | 298 ------------- ...tonomous_sequence_high_level_compressed.py | 115 ----- examples/autonomy/circling_square_demo.py | 190 -------- examples/autonomy/full_state_setpoint_demo.py | 150 ------- examples/autonomy/position_commander_demo.py | 109 ----- examples/cfbridge.py | 135 ------ examples/color_led_deck/color_led_cycle.py | 154 ------- examples/color_led_deck/color_led_set.py | 132 ------ examples/console/console.py | 61 --- examples/generic_led_deck/led_cycle.py | 105 ----- examples/led-ring/led_mem_set.py | 65 --- examples/led-ring/led_param_set.py | 74 --- examples/led-ring/led_timing.py | 74 --- .../lighthouse_config_visualization.py | 155 ------- examples/lighthouse/lighthouse_mass_upload.py | 88 ---- examples/lighthouse/lighthouse_openvr_grab.py | 131 ------ .../lighthouse_openvr_grab_color.py | 163 ------- .../lighthouse/lighthouse_openvr_multigrab.py | 160 ------- .../multi_bs_geometry_estimation.py | 422 ------------------ examples/lighthouse/read_lighthouse_mem.py | 76 ---- examples/lighthouse/write_lighthouse_mem.py | 130 ------ examples/link_quality/latency.py | 55 --- .../loco_nodes/lps_reboot_to_bootloader.py | 100 ----- examples/memory/flash_memory.py | 221 --------- examples/memory/read_deck_mem.py | 102 ----- examples/memory/read_eeprom.py | 134 ------ examples/memory/read_l5.py | 75 ---- examples/memory/read_ow.py | 84 ---- examples/memory/read_paa3905.py | 77 ---- examples/memory/write_eeprom.py | 146 ------ examples/memory/write_ow.py | 149 ------- examples/mocap/mocap_hl_commander.py | 192 -------- examples/mocap/mocap_swarm.py | 144 ------ examples/mocap/mocap_swarm_multi_commander.py | 149 ------- examples/mocap/qualisys_hl_commander.py | 258 ----------- examples/motors/multiramp.py | 122 ----- examples/motors/ramp.py | 118 ----- .../multiranger/multiranger_pointcloud.py | 369 --------------- examples/multiranger/multiranger_push.py | 108 ----- .../multiranger/multiranger_wall_following.py | 132 ------ .../wall_following/wall_following.py | 383 ---------------- examples/positioning/flowsequence_sync.py | 86 ---- examples/positioning/initial_position.py | 112 ----- examples/positioning/matrix_light_printer.py | 118 ----- examples/positioning/monalisa.png | Bin 4886 -> 0 bytes examples/radio/radio_test.py | 160 ------- examples/radio/scan.py | 37 -- examples/swarm/asynchronized_swarm.py | 159 ------- examples/swarm/christmas_tree.py | 185 -------- examples/swarm/hl_commander_swarm.py | 94 ---- examples/swarm/leader_follower.py | 207 --------- examples/swarm/swarm_sequence.py | 253 ----------- examples/swarm/swarm_sequence_circle.py | 147 ------ examples/swarm/synchronized_sequence.py | 205 --------- 55 files changed, 16 insertions(+), 7868 deletions(-) create mode 100644 examples/README.md delete mode 100644 examples/aideck/fpv.py delete mode 100644 examples/autonomy/autonomous_sequence_high_level_compressed.py delete mode 100644 examples/autonomy/circling_square_demo.py delete mode 100644 examples/autonomy/full_state_setpoint_demo.py delete mode 100644 examples/autonomy/position_commander_demo.py delete mode 100755 examples/cfbridge.py delete mode 100644 examples/color_led_deck/color_led_cycle.py delete mode 100644 examples/color_led_deck/color_led_set.py delete mode 100644 examples/console/console.py delete mode 100644 examples/generic_led_deck/led_cycle.py delete mode 100644 examples/led-ring/led_mem_set.py delete mode 100644 examples/led-ring/led_param_set.py delete mode 100644 examples/led-ring/led_timing.py delete mode 100644 examples/lighthouse/lighthouse_config_visualization.py delete mode 100644 examples/lighthouse/lighthouse_mass_upload.py delete mode 100644 examples/lighthouse/lighthouse_openvr_grab.py delete mode 100644 examples/lighthouse/lighthouse_openvr_grab_color.py delete mode 100644 examples/lighthouse/lighthouse_openvr_multigrab.py delete mode 100644 examples/lighthouse/multi_bs_geometry_estimation.py delete mode 100644 examples/lighthouse/read_lighthouse_mem.py delete mode 100644 examples/lighthouse/write_lighthouse_mem.py delete mode 100644 examples/link_quality/latency.py delete mode 100644 examples/loco_nodes/lps_reboot_to_bootloader.py delete mode 100644 examples/memory/flash_memory.py delete mode 100644 examples/memory/read_deck_mem.py delete mode 100644 examples/memory/read_eeprom.py delete mode 100644 examples/memory/read_l5.py delete mode 100644 examples/memory/read_ow.py delete mode 100644 examples/memory/read_paa3905.py delete mode 100644 examples/memory/write_eeprom.py delete mode 100644 examples/memory/write_ow.py delete mode 100644 examples/mocap/mocap_hl_commander.py delete mode 100644 examples/mocap/mocap_swarm.py delete mode 100644 examples/mocap/mocap_swarm_multi_commander.py delete mode 100644 examples/mocap/qualisys_hl_commander.py delete mode 100644 examples/motors/multiramp.py delete mode 100644 examples/motors/ramp.py delete mode 100644 examples/multiranger/multiranger_pointcloud.py delete mode 100755 examples/multiranger/multiranger_push.py delete mode 100644 examples/multiranger/multiranger_wall_following.py delete mode 100644 examples/multiranger/wall_following/wall_following.py delete mode 100644 examples/positioning/flowsequence_sync.py delete mode 100644 examples/positioning/initial_position.py delete mode 100644 examples/positioning/matrix_light_printer.py delete mode 100644 examples/positioning/monalisa.png delete mode 100644 examples/radio/radio_test.py delete mode 100644 examples/radio/scan.py delete mode 100644 examples/swarm/asynchronized_swarm.py delete mode 100644 examples/swarm/christmas_tree.py delete mode 100644 examples/swarm/hl_commander_swarm.py delete mode 100644 examples/swarm/leader_follower.py delete mode 100644 examples/swarm/swarm_sequence.py delete mode 100644 examples/swarm/swarm_sequence_circle.py delete mode 100755 examples/swarm/synchronized_sequence.py diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 000000000..985db5a25 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,16 @@ +# Examples + +This folder contains basic examples for getting started with the Crazyflie Python library. They cover the core building blocks: connecting, logging, parameters, and simple autonomous flight. + +## What's here + +| Folder | Description | +|--------|-------------| +| `logging/` | Read sensor data and log variables from the Crazyflie | +| `parameters/` | Read and write parameters, including persistent parameter storage | +| `autonomy/` | Basic autonomous flight using different commanders | +| `step-by-step/` | Step-by-step tutorial scripts that accompany the [user guides](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/user-guides/) | + +## More examples + +For more complete and advanced demos see the **[crazyflie-demos](https://github.com/bitcraze/crazyflie-demos)** repository. diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py deleted file mode 100644 index 908c836ef..000000000 --- a/examples/aideck/fpv.py +++ /dev/null @@ -1,298 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018-2022 Bitcraze AB -# -# Crazyflie Python Library -# -# 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, see . -""" -Example script which can be used to fly the Crazyflie in "FPV" mode -using the Flow deck and the AI deck. - -The example shows how to connect to a Crazyflie over the WiFi and -use both CPX and CRTP for communication over WiFI. - -When the application is started the Crazyflie will hover at 0.3 m. The -Crazyflie can then be controlled by using keyboard input: - * Move by using the arrow keys (left/right/forward/backwards) - * Adjust the right with w/s (0.1 m for each keypress) - * Yaw slowly using a/d (CCW/CW) - * Yaw fast using z/x (CCW/CW) - -The demo is ended by closing the application. - -For the example to run the following hardware is needed: - * Crazyflie 2.1 - * Crazyradio PA - * Flow v2 deck - * AI deck 1.1 -""" -import logging -import struct -import sys -import threading -from time import time - -import numpy as np - -import cflib.crtp -from cflib.cpx import CPXFunction -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.utils import uri_helper - -try: - from sip import setapi - setapi('QVariant', 2) - setapi('QString', 2) -except ImportError: - pass - -from PyQt6 import QtCore, QtWidgets, QtGui - -logging.basicConfig(level=logging.INFO) - -# If you have set a CFLIB_URI environment variable, that address will be used. -URI = uri_helper.uri_from_env(default='tcp://192.168.4.1:5000') -# 192.168.4.1 is the default IP address if the aideck is Access point. -# If you are using the aideck as a station, you should use the assigned IP address -# 5000 is the default port for the aideck - -CAM_HEIGHT = 244 -CAM_WIDTH = 324 -# Set the speed factor for moving and rotating -SPEED_FACTOR = 0.3 - -if len(sys.argv) > 1: - URI = sys.argv[1] - - -class ImageDownloader(threading.Thread): - def __init__(self, cpx, cb): - threading.Thread.__init__(self) - self.daemon = True - self._cpx = cpx - self._cb = cb - - def run(self): - while True: - p = self._cpx.receivePacket(CPXFunction.APP) - [magic, width, height, depth, format, size] = struct.unpack('. -""" -Simple example that connects to one crazyflie (check the address at the top -and update it to your crazyflie address) and uses the high level commander -to send setpoints and trajectory to fly a figure 8. - -This example is intended to work with any positioning system (including LPS). -It aims at documenting how to set the Crazyflie in position control mode -and how to send setpoints using the high level commander. -""" -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.high_level_commander import HighLevelCommander -from cflib.crazyflie.mem import CompressedSegment -from cflib.crazyflie.mem import CompressedStart -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# The trajectory to fly -a = 0.9 -b = 0.5 -c = 0.5 -trajectory = [ - CompressedStart(0.0, 0.0, 0.0, 0.0), - CompressedSegment(2.0, [0.0, 1.0, 1.0], [0.0, a, 0.0], [], []), - CompressedSegment(2.0, [1.0, b, 0.0], [-a, -c, 0.0], [], []), - CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []), - CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []), -] - - -def activate_mellinger_controller(cf): - cf.param.set_value('stabilizer.controller', '2') - - -def upload_trajectory(cf, trajectory_id, trajectory): - trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] - - trajectory_mem.trajectory = trajectory - - upload_result = trajectory_mem.write_data_sync() - if not upload_result: - print('Upload failed, aborting!') - sys.exit(1) - cf.high_level_commander.define_trajectory( - trajectory_id, - 0, - len(trajectory), - type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) - - total_duration = 0 - # Skip the start element - for segment in trajectory[1:]: - total_duration += segment.duration - - return total_duration - - -def run_sequence(cf, trajectory_id, duration): - commander = cf.high_level_commander - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - commander.takeoff(1.0, 2.0) - time.sleep(3.0) - relative = True - commander.start_trajectory(trajectory_id, 1.0, relative) - time.sleep(duration) - commander.land(0.0, 2.0) - time.sleep(2) - commander.stop() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - trajectory_id = 1 - - # activate_mellinger_controller(cf) - duration = upload_trajectory(cf, trajectory_id, trajectory) - print('The sequence is {:.1f} seconds long'.format(duration)) - reset_estimator(cf) - run_sequence(cf, trajectory_id, duration) diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py deleted file mode 100644 index e1871bca5..000000000 --- a/examples/autonomy/circling_square_demo.py +++ /dev/null @@ -1,190 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2023 Bitcraze AB -# -# 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, see . -""" -Simple "show" example that connects to 8 crazyflies (check the addresses at the top -and update it to your crazyflies addresses) and uses the high level commander with bezier curves -to send trajectories to fly a circle (while the 8 drones are positioned in a square). -To spice it up, the LEDs are changing color - the color move factor defines how fast and in which direction. - -This example is intended to work with any positioning system (including LPS). -It aims at documenting how to set the Crazyflie in position control mode -and how to send setpoints using the high level commander. -""" -import sys -import time - -import numpy as np - -import cflib.crtp -from cflib.crazyflie.high_level_commander import HighLevelCommander -from cflib.crazyflie.mem import CompressedSegment -from cflib.crazyflie.mem import CompressedStart -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm - -URI1 = 'radio://0/60/2M/E7E7E7E710' -URI2 = 'radio://0/60/2M/E7E7E7E711' -URI3 = 'radio://0/60/2M/E7E7E7E712' -URI4 = 'radio://0/60/2M/E7E7E7E713' -URI5 = 'radio://0/60/2M/E7E7E7E714' -URI6 = 'radio://0/60/2M/E7E7E7E715' -URI7 = 'radio://0/60/2M/E7E7E7E716' -URI8 = 'radio://0/60/2M/E7E7E7E717' - -# The trajectory to fly -a = 0.55 # where the Beizer curve control point should be https://spencermortensen.com/articles/bezier-circle/ -h = 1.0 # [m] how high we should fly -t = 2.0 # seconds per step, one circle has 4 steps -r1 = 1.0 # [m] the radius at which the first half of the drones are -r2 = np.sqrt(2.0) # [m] the radius at which every second other drone is -loops = 2 # how many loops we should fly -color_move_factor = 3 # magic factor which defines how fast the colors move - - -def rotate_beizer_node(xl, yl, alpha): - x_rot = [] - y_rot = [] - for x, y in zip(xl, yl): - x_rot.append(x*np.cos(alpha) - y*np.sin(alpha)) - y_rot.append(x*np.sin(alpha) + y*np.cos(alpha)) - return x_rot, y_rot - - -def activate_high_level_commander(cf): - cf.param.set_value('commander.enHighLevel', '1') - - -def activate_mellinger_controller(cf): - cf.param.set_value('stabilizer.controller', '2') - - -def upload_trajectory(cf, trajectory_id, trajectory): - trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] - - trajectory_mem.trajectory = trajectory - - upload_result = trajectory_mem.write_data_sync() - if not upload_result: - print('Upload failed, aborting!') - sys.exit(1) - cf.high_level_commander.define_trajectory( - trajectory_id, - 0, - len(trajectory), - type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) - - total_duration = 0 - # Skip the start element - for segment in trajectory[1:]: - total_duration += segment.duration - - return total_duration - - -def turn_off_leds(scf): - # Set solid color effect - scf.cf.param.set_value('ring.effect', '7') - # Set the RGB values - scf.cf.param.set_value('ring.solidRed', '0') - scf.cf.param.set_value('ring.solidGreen', '0') - scf.cf.param.set_value('ring.solidBlue', '0') - - -def run_sequence(scf, alpha, r): - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - commander = scf.cf.high_level_commander - trajectory_id = 1 - duration = 4*t - commander.takeoff(h, 2.0) - time.sleep(3.0) - x_start, y_start = rotate_beizer_node([r], [0.0], alpha) - commander.go_to(x_start[0], y_start[0], h, 0.0, 2.0) - time.sleep(3.0) - relative = False - start_time_leds = time.time() - for i in range(loops): - commander.start_trajectory(trajectory_id, 1.0, relative) - start_time = time.time() - end_time = start_time + duration - while True: - color_angle = alpha + ((time.time() - start_time_leds)/duration)*2.0*np.pi*color_move_factor - scf.cf.param.set_value('ring.solidRed', np.cos(color_angle)*127 + 127) - scf.cf.param.set_value('ring.solidGreen', 128 - np.cos(color_angle)*127) - scf.cf.param.set_value('ring.solidBlue', '0') - if time.time() > end_time: - break - time.sleep(0.2) - commander.land(0.0, 2.0) - scf.cf.param.set_value('ring.solidRed', '0') - scf.cf.param.set_value('ring.solidGreen', '0') - scf.cf.param.set_value('ring.solidBlue', '0') - time.sleep(20) # sleep long enough to be sure to have turned off leds - commander.stop() - - -def create_trajectory(alpha, r): - x_start, y_start = rotate_beizer_node([r], [0.0], alpha) - beizer_point_1_x, beizer_point_1_y = rotate_beizer_node([r, r*a, 0.0], [r*a, r, r], alpha) - beizer_point_2_x, beizer_point_2_y = rotate_beizer_node([-r*a, -r, -r], [r, r*a, 0.0], alpha) - beizer_point_3_x, beizer_point_3_y = rotate_beizer_node([-r, -r*a, 0.0], [-r*a, -r, -r], alpha) - beizer_point_4_x, beizer_point_4_y = rotate_beizer_node([r*a, r, r], [-r, -r*a, 0.0], alpha) - trajectory = [ - CompressedStart(x_start[0], y_start[0], h, 0.0), - CompressedSegment(t, beizer_point_1_x, beizer_point_1_y, [h], []), - CompressedSegment(t, beizer_point_2_x, beizer_point_2_y, [h], []), - CompressedSegment(t, beizer_point_3_x, beizer_point_3_y, [h], []), - CompressedSegment(t, beizer_point_4_x, beizer_point_4_y, [h], []), - ] - return trajectory - - -def upload_trajectories(scf, alpha, r): - trajectory_id = 1 - trajectory = create_trajectory(alpha, r) - upload_trajectory(scf.cf, trajectory_id, trajectory) - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - uris = [URI1, URI2, URI3, URI4, URI5, URI6, URI7, URI8] - # uris = [URI1, URI5] - position_params = { - URI1: [0.0, r1], - URI2: [np.pi/4, r2], - URI3: [np.pi/2, r1], - URI4: [np.pi/4*3, r2], - URI5: [np.pi, r1], - URI6: [np.pi/4*5, r2], - URI7: [np.pi/4*6, r1], - URI8: [np.pi/4*7, r2]} - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - swarm.reset_estimators() - swarm.parallel_safe(turn_off_leds) - swarm.parallel_safe(upload_trajectories, args_dict=position_params) - time.sleep(5) - swarm.parallel_safe(run_sequence, args_dict=position_params) diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py deleted file mode 100644 index b5954fc85..000000000 --- a/examples/autonomy/full_state_setpoint_demo.py +++ /dev/null @@ -1,150 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2023 Bitcraze AB -# -# 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, see . -""" -Shows how to send full state control setpoints to the Crazyflie -""" -import time - -from scipy.spatial.transform import Rotation - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -def quaternion_from_euler(roll: float, pitch: float, yaw: float): - """Convert Euler angles to quaternion - - Args: - roll (float): roll, in radians - pitch (float): pitch, in radians - yaw (float): yaw, in radians - - Returns: - array: the quaternion [x, y, z, w] - """ - return Rotation.from_euler(seq='xyz', angles=(roll, pitch, yaw), degrees=False).as_quat() - - -def position_callback(timestamp, data, logconf): - x = data['kalman.stateX'] - y = data['kalman.stateY'] - z = data['kalman.stateZ'] - print('pos: ({}, {}, {})'.format(x, y, z)) - - -def start_position_printing(scf): - log_conf = LogConfig(name='Position', period_in_ms=500) - log_conf.add_variable('kalman.stateX', 'float') - log_conf.add_variable('kalman.stateY', 'float') - log_conf.add_variable('kalman.stateZ', 'float') - - scf.cf.log.add_config(log_conf) - log_conf.data_received_cb.add_callback(position_callback) - log_conf.start() - - -def send_setpoint(cf, duration, pos, vel, acc, orientation, rollrate, pitchrate, yawrate): - # Set points must be sent continuously to the Crazyflie, if not it will think that connection is lost - end_time = time.time() + duration - while time.time() < end_time: - cf.commander.send_full_state_setpoint(pos, vel, acc, orientation, rollrate, pitchrate, yawrate) - time.sleep(0.2) - - -def run_sequence(scf): - cf = scf.cf - - # Set to mellinger controller - # cf.param.set_value('stabilizer.controller', '2') - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - print('takeoff') - send_setpoint(cf, 4.0, - [0.0, 0.0, 1.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - quaternion_from_euler(0.0, 0.0, 0.0), - 0.0, 0.0, 0.0) - - send_setpoint(cf, 2.0, - [0.0, 0.0, 1.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - quaternion_from_euler(0.0, 0.0, 0.7), - 0.0, 0.0, 0.0) - print('land') - send_setpoint(cf, 2.0, - [0.0, 0.0, 0.1], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - quaternion_from_euler(0.0, 0.0, 0.0), - 0.0, 0.0, 0.0) - - cf.commander.send_stop_setpoint() - # Hand control over to the high level commander to avoid timeout and locking of the Crazyflie - cf.commander.send_notify_setpoint_stop() - - # 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) - - -def _stab_log_data(timestamp, data, logconf): - print('roll: {}, pitch: {}, yaw: {}'.format(data['controller.roll'], - data['controller.pitch'], - data['controller.yaw'])) - print('ctrltarget.x: {}, ctrltarget.y: {}, ctrltarget.z: {}'.format(data['ctrltarget.x'], - data['ctrltarget.y'], - data['ctrltarget.z'])) - - -def set_up_logging(scf): - _lg_stab = LogConfig(name='Stabilizer', period_in_ms=500) - _lg_stab.add_variable('controller.roll', 'float') - _lg_stab.add_variable('controller.pitch', 'float') - _lg_stab.add_variable('controller.yaw', 'float') - _lg_stab.add_variable('ctrltarget.x', 'float') - _lg_stab.add_variable('ctrltarget.y', 'float') - _lg_stab.add_variable('ctrltarget.z', 'float') - - scf.cf.log.add_config(_lg_stab) - _lg_stab.data_received_cb.add_callback(_stab_log_data) - _lg_stab.start() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - # set_up_logging(scf) - reset_estimator(scf) - run_sequence(scf) diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py deleted file mode 100644 index 1b0f93796..000000000 --- a/examples/autonomy/position_commander_demo.py +++ /dev/null @@ -1,109 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018 Bitcraze AB -# -# 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, see . -""" -This script shows the basic use of the PositionHlCommander class. - -Simple example that connects to the crazyflie at `URI` and runs a -sequence. This script requires some kind of location system. - -The PositionHlCommander uses position setpoints. - -Change the URI variable to your Crazyflie configuration. -""" -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.position_hl_commander import PositionHlCommander -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -def slightly_more_complex_usage(): - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - with PositionHlCommander( - scf, - x=0.0, y=0.0, z=0.0, - default_velocity=0.3, - default_height=0.5, - controller=PositionHlCommander.CONTROLLER_PID) as pc: - # Go to a coordinate - pc.go_to(1.0, 1.0, 1.0) - - # Move relative to the current position - pc.right(1.0) - - # Go to a coordinate and use default height - pc.go_to(0.0, 0.0) - - # Go slowly to a coordinate - pc.go_to(1.0, 1.0, velocity=0.2) - - # Set new default velocity and height - pc.set_default_velocity(0.3) - pc.set_default_height(1.0) - pc.go_to(0.0, 0.0) - - -def land_on_elevated_surface(): - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - with PositionHlCommander(scf, - default_height=0.5, - default_velocity=0.2, - default_landing_height=0.35, - controller=PositionHlCommander.CONTROLLER_PID) 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) - # land() will be called on context exit, gradually lowering to default_landing_height, then stopping motors - - -def simple_sequence(): - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: - pc.forward(1.0) - pc.left(1.0) - pc.back(1.0) - pc.go_to(0.0, 0.0, 1.0) - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - simple_sequence() - # slightly_more_complex_usage() - # land_on_elevated_surface() diff --git a/examples/cfbridge.py b/examples/cfbridge.py deleted file mode 100755 index 466e80f9c..000000000 --- a/examples/cfbridge.py +++ /dev/null @@ -1,135 +0,0 @@ -#!/usr/bin/env python -""" -Bridge a Crazyflie connected to a Crazyradio to a local MAVLink port -Requires 'pip install cflib' - -As the ESB protocol works using PTX and PRX (Primary Transmitter/Receiver) -modes. Thus, data is only received as a response to a sent packet. -So, we need to constantly poll the receivers for bidirectional communication. - -@author: Dennis Shtatnov (densht@gmail.com) - -Based off example code from crazyflie-lib-python/examples/read_eeprom.py -""" -# import struct -import logging -import socket -import sys -import threading -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crtp.crtpstack import CRTPPacket -# from cflib.crtp.crtpstack import CRTPPort - -CRTP_PORT_MAVLINK = 8 - - -# Only output errors from the logging framework -logging.basicConfig(level=logging.DEBUG) - - -class RadioBridge: - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # UDP socket for interfacing with GCS - self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - self._sock.bind(('127.0.0.1', 14551)) - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # Connect some callbacks from the Crazyflie API - self._cf.link_established.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 - - threading.Thread(target=self._server).start() - - 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) - - self._cf.packet_received.add_callback(self._got_packet) - - def _got_packet(self, pk): - if pk.port == CRTP_PORT_MAVLINK: - self._sock.sendto(pk.data, ('127.0.0.1', 14550)) - - def _forward(self, data): - pk = CRTPPacket() - pk.port = CRTP_PORT_MAVLINK # CRTPPort.COMMANDER - pk.data = data # struct.pack(' 1: - channel = str(sys.argv[1]) - else: - channel = 80 - - link_uri = 'radio://0/' + str(channel) + '/2M' - le = RadioBridge(link_uri) - - # 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. - try: - while le.is_connected: - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/color_led_deck/color_led_cycle.py b/examples/color_led_deck/color_led_cycle.py deleted file mode 100644 index 97d7a1c97..000000000 --- a/examples/color_led_deck/color_led_cycle.py +++ /dev/null @@ -1,154 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -class wrgb: - def __init__(self, w: int, r: int, g: int, b: int): - self.w = w - self.r = r - self.g = g - self.b = b - - -class hsv: - def __init__(self, h: int, s: int, v: int): - self.h = h - self.s = s - self.v = v - - -def hsv_to_wrgb(input: hsv) -> wrgb: - h, s, v = input.h, input.s / 255.0, input.v / 255.0 - - if s == 0: - r = g = b = v - else: - h = h / 60.0 - i = int(h) - f = h - i - p = v * (1.0 - s) - q = v * (1.0 - s * f) - t = v * (1.0 - s * (1.0 - f)) - - if i == 0: - r, g, b = v, t, p - elif i == 1: - r, g, b = q, v, p - elif i == 2: - r, g, b = p, v, t - elif i == 3: - r, g, b = p, q, v - elif i == 4: - r, g, b = t, p, v - else: - r, g, b = v, p, q - - return wrgb(0, int(r * 255), int(g * 255), int(b * 255)) - - -def cycle_colors(step: int) -> wrgb: - h = step % 360 - s = 255 - v = 255 - return hsv_to_wrgb(hsv(h, s, v)) - - -def pack_wrgb(input: wrgb) -> int: - """Pack WRGB values into uint32 format: 0xWWRRGGBB""" - return (input.w << 24) | (input.r << 16) | (input.g << 8) | input.b - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - # Detect which color LED deck is present (bottom or top) - # We check for whichever deck is attached and use the first one found - # Check for bottom-facing deck first - if str(cf.param.get_value('deck.bcColorLedBot')) == '1': - deck_params = { - 'color': 'colorLedBot.wrgb8888', - 'brightness': 'colorLedBot.brightCorr', - 'throttle': 'colorLedBot.throttlePct', - 'temp': 'colorLedBot.deckTemp' - } - print('Detected bottom-facing color LED deck') - # Check for top-facing deck - elif str(cf.param.get_value('deck.bcColorLedTop')) == '1': - deck_params = { - 'color': 'colorLedTop.wrgb8888', - 'brightness': 'colorLedTop.brightCorr', - 'throttle': 'colorLedTop.throttlePct', - 'temp': 'colorLedTop.deckTemp' - } - print('Detected top-facing color LED deck') - else: - raise RuntimeError('No color LED deck detected!') - - # Thermal status callback - def thermal_status_callback(timestamp, data, logconf): - throttle_pct = data[deck_params['throttle']] - if throttle_pct > 0: - temp = data[deck_params['temp']] - print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') - - # Setup log configuration for thermal monitoring - log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) - log_conf.add_variable(deck_params['temp'], 'uint8_t') - log_conf.add_variable(deck_params['throttle'], 'uint8_t') - - cf.log.add_config(log_conf) - log_conf.data_received_cb.add_callback(thermal_status_callback) - log_conf.start() - - # Brightness correction: balances luminance across W/R/G/B channels - # Set to 1 (enabled, default) for perceptually uniform colors - # Set to 0 (disabled) for maximum brightness per channel - cf.param.set_value(deck_params['brightness'], 1) - time.sleep(0.1) - - try: - print('Cycling through colors. Press Ctrl-C to stop.') - while True: - for i in range(0, 360, 1): - color: wrgb = cycle_colors(i) - # print(color.r, color.g, color.b) - color_uint32 = pack_wrgb(color) - cf.param.set_value(deck_params['color'], str(color_uint32)) - time.sleep(0.01) - except KeyboardInterrupt: - print('\nStopping and turning off LED...') - cf.param.set_value(deck_params['color'], '0') - time.sleep(0.1) diff --git a/examples/color_led_deck/color_led_set.py b/examples/color_led_deck/color_led_set.py deleted file mode 100644 index d91757ebb..000000000 --- a/examples/color_led_deck/color_led_set.py +++ /dev/null @@ -1,132 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -class wrgb: - def __init__(self, w: int, r: int, g: int, b: int): - self.w = w - self.r = r - self.g = g - self.b = b - - -class rgb: - def __init__(self, r: int, g: int, b: int): - self.w = min(r, g, b) - self.r = r - self.w - self.g = g - self.w - self.b = b - self.w - - -def pack_wrgb(input: wrgb | rgb) -> int: - """Pack WRGB values into uint32 format: 0xWWRRGGBB""" - return (input.w << 24) | (input.r << 16) | (input.g << 8) | input.b - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - # Detect which color LED deck is present (bottom or top) - # We check for whichever deck is attached and use the first one found - # Check for bottom-facing deck first - if str(cf.param.get_value('deck.bcColorLedBot')) == '1': - deck_params = { - 'color': 'colorLedBot.wrgb8888', - 'brightness': 'colorLedBot.brightCorr', - 'throttle': 'colorLedBot.throttlePct', - 'temp': 'colorLedBot.deckTemp' - } - print('Detected bottom-facing color LED deck') - # Check for top-facing deck - elif str(cf.param.get_value('deck.bcColorLedTop')) == '1': - deck_params = { - 'color': 'colorLedTop.wrgb8888', - 'brightness': 'colorLedTop.brightCorr', - 'throttle': 'colorLedTop.throttlePct', - 'temp': 'colorLedTop.deckTemp' - } - print('Detected top-facing color LED deck') - else: - raise RuntimeError('No color LED deck detected!') - - # Thermal status callback - def thermal_status_callback(timestamp, data, logconf): - throttle_pct = data[deck_params['throttle']] - if throttle_pct > 0: - temp = data[deck_params['temp']] - print(f'WARNING: Thermal throttling active! Temp: {temp}°C, Throttle: {throttle_pct}%') - - # Setup log configuration for thermal monitoring - log_conf = LogConfig(name='ThermalStatus', period_in_ms=100) - log_conf.add_variable(deck_params['temp'], 'uint8_t') - log_conf.add_variable(deck_params['throttle'], 'uint8_t') - - cf.log.add_config(log_conf) - log_conf.data_received_cb.add_callback(thermal_status_callback) - log_conf.start() - - # Brightness correction: balances luminance across R/G/B/W channels - # Set to 1 (enabled, default) for perceptually uniform colors - # Set to 0 (disabled) for maximum brightness per channel - cf.param.set_value(deck_params['brightness'], 1) - time.sleep(0.1) - - try: - # ======================================== - # Set your desired color here: - # ======================================== - # Examples: - # color = wrgb(w=0, r=255, g=0, b=0) # Pure red - # color = wrgb(w=0, r=0, g=255, b=0) # Pure green - # color = wrgb(w=0, r=0, g=0, b=255) # Pure blue - # color = wrgb(w=255, r=0, g=0, b=0) # Pure white LED - # color = wrgb(w=50, r=255, g=128, b=0) # Orange + white - # color = rgb(r=255, g=255, b=255) # White (auto W extraction) - - color = wrgb(w=0, r=255, g=0, b=100) - # ======================================== - - color_uint32 = pack_wrgb(color) - cf.param.set_value(deck_params['color'], color_uint32) - time.sleep(0.01) - print(f'Setting LED to R={color.r}, G={color.g}, B={color.b}, W={color.w}') - print('Press Ctrl-C to turn off LED and exit.') - while True: - time.sleep(0.1) - except KeyboardInterrupt: - print('\nStopping and turning off LED...') - cf.param.set_value(deck_params['color'], 0) - time.sleep(0.1) diff --git a/examples/console/console.py b/examples/console/console.py deleted file mode 100644 index 0b9ef16de..000000000 --- a/examples/console/console.py +++ /dev/null @@ -1,61 +0,0 @@ -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2021 Bitcraze AB -# -# 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 3 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, see . -import time - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Reads the CFLIB_URI environment variable for URI or uses default -uri = uri_helper.uri_from_env(default='radio://0/30/2M/E7E7E7E7E7') - - -def console_callback(text: str): - '''A callback to run when we get console text from Crazyflie''' - # We do not add newlines to the text received, we get them from the - # Crazyflie at appropriate places. - print(text, end='') - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - # Create Crazyflie object, with cache to avoid re-reading param and log TOC - cf = Crazyflie(rw_cache='./cache') - - # Add a callback to whenever we receive 'console' text from the Crazyflie - # This is the output from DEBUG_PRINT calls in the firmware. - # - # This could also be a Python lambda, something like: - # cf.console.receivedChar.add_callback(lambda text: logger.info(text)) - cf.console.receivedChar.add_callback(console_callback) - - # This will connect the Crazyflie with the URI specified above. - # You might have to restart your Crazyflie in order to get output - # from console, since not much is written during regular uptime. - with SyncCrazyflie(uri, cf=cf) as scf: - print('[host] Connected, use ctrl-c to quit.') - - while True: - time.sleep(1) diff --git a/examples/generic_led_deck/led_cycle.py b/examples/generic_led_deck/led_cycle.py deleted file mode 100644 index b704f9477..000000000 --- a/examples/generic_led_deck/led_cycle.py +++ /dev/null @@ -1,105 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -class rgb: - def __init__(self, r: int, g: int, b: int): - self.r = r - self.g = g - self.b = b - - -class hsv: - def __init__(self, h: int, s: int, v: int): - self.h = h - self.s = s - self.v = v - - -def hsv_to_rgb(input: hsv) -> rgb: - h, s, v = input.h, input.s / 255.0, input.v / 255.0 - - if s == 0: - r = g = b = v - else: - h = h / 60.0 - i = int(h) - f = h - i - p = v * (1.0 - s) - q = v * (1.0 - s * f) - t = v * (1.0 - s * (1.0 - f)) - - if i == 0: - r, g, b = v, t, p - elif i == 1: - r, g, b = q, v, p - elif i == 2: - r, g, b = p, v, t - elif i == 3: - r, g, b = p, q, v - elif i == 4: - r, g, b = t, p, v - else: - r, g, b = v, p, q - - return rgb(int(r * 255), int(g * 255), int(b * 255)) - - -def cycle_colors(step: int) -> rgb: - h = step % 360 - s = 255 - v = 255 - return hsv_to_rgb(hsv(h, s, v)) - - -def construct_uint32_color(input: rgb) -> int: - return (input.r << 16) | (input.g << 8) | input.b - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - try: - print('Cycling through colors. Press Ctrl-C to stop.') - while True: - for i in range(0, 360, 1): - color = cycle_colors(i) - color_uint32 = construct_uint32_color(color) - cf.param.set_value('led_deck_ctrl.rgb888', str(color_uint32)) - time.sleep(0.01) - except KeyboardInterrupt: - print('\nStopping and turning off LED...') - cf.param.set_value('led_deck_ctrl.rgb888', '0') - time.sleep(0.1) diff --git a/examples/led-ring/led_mem_set.py b/examples/led-ring/led_mem_set.py deleted file mode 100644 index 57defadd8..000000000 --- a/examples/led-ring/led_mem_set.py +++ /dev/null @@ -1,65 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 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, see . -""" -Simple example that connects to the crazyflie at `URI` and writes to -the LED memory so that individual leds in the LED-ring can be set, -it has been tested with (and designed for) the LED-ring deck. - -Change the URI variable to your Crazyflie configuration. -""" -import logging -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - # Set virtual mem effect effect - cf.param.set_value('ring.effect', '13') - - # Get LED memory and write to it - mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED) - if len(mem) > 0: - mem[0].leds[0].set(r=0, g=100, b=0) - mem[0].leds[3].set(r=0, g=0, b=100) - mem[0].leds[6].set(r=100, g=0, b=0) - mem[0].leds[9].set(r=100, g=100, b=100) - mem[0].write_data(None) - - time.sleep(2) diff --git a/examples/led-ring/led_param_set.py b/examples/led-ring/led_param_set.py deleted file mode 100644 index bb4326372..000000000 --- a/examples/led-ring/led_param_set.py +++ /dev/null @@ -1,74 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 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, see . -""" -Simple example that connects to the crazyflie at `URI` and writes to -parameters that control the LED-ring, -it has been tested with (and designed for) the LED-ring deck. - -Change the URI variable to your Crazyflie configuration. -""" -import logging -import time - -import cflib.crtp -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI) as scf: - cf = scf.cf - - # Set solid color effect - cf.param.set_value('ring.effect', '7') - # Set the RGB values - cf.param.set_value('ring.solidRed', '100') - cf.param.set_value('ring.solidGreen', '0') - cf.param.set_value('ring.solidBlue', '0') - time.sleep(2) - - # Set black color effect - cf.param.set_value('ring.effect', '0') - time.sleep(1) - - # Set fade to color effect - cf.param.set_value('ring.effect', '14') - # Set fade time i seconds - cf.param.set_value('ring.fadeTime', '1.0') - # Set the RGB values in one uint32 0xRRGGBB - cf.param.set_value('ring.fadeColor', int('0000A0', 16)) - time.sleep(1) - cf.param.set_value('ring.fadeColor', int('00A000', 16)) - time.sleep(1) - cf.param.set_value('ring.fadeColor', int('A00000', 16)) - time.sleep(1) diff --git a/examples/led-ring/led_timing.py b/examples/led-ring/led_timing.py deleted file mode 100644 index f4fcddcb6..000000000 --- a/examples/led-ring/led_timing.py +++ /dev/null @@ -1,74 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 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, see . -""" -This example demonstrate the LEDTIMING led ring sequence memory. This memory and -led-ring effect allows to pre-program a LED sequence to be played autonomously -by the Crazyflie. - -Change the URI variable to your Crazyflie configuration. -""" -import logging -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - # Get LED memory and write to it - mem = cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LEDTIMING) - if len(mem) > 0: - mem[0].add(25, {'r': 100, 'g': 0, 'b': 0}) - mem[0].add(0, {'r': 0, 'g': 100, 'b': 0}, leds=1) - mem[0].add(0, {'r': 0, 'g': 100, 'b': 0}, leds=2) - mem[0].add(3000, {'r': 0, 'g': 100, 'b': 0}, leds=3, rotate=1) - mem[0].add(50, {'r': 0, 'g': 0, 'b': 100}, leds=1) - mem[0].add(25, {'r': 0, 'g': 0, 'b': 100}, leds=0, fade=True) - mem[0].add(25, {'r': 100, 'g': 0, 'b': 100}, leds=1) - mem[0].add(25, {'r': 100, 'g': 0, 'b': 0}) - mem[0].add(50, {'r': 100, 'g': 0, 'b': 100}) - mem[0].write_data(None) - else: - print('No LED ring present') - - # Set virtual mem effect effect - cf.param.set_value('ring.effect', '0') - time.sleep(2) - cf.param.set_value('ring.effect', '17') - time.sleep(2) diff --git a/examples/lighthouse/lighthouse_config_visualization.py b/examples/lighthouse/lighthouse_config_visualization.py deleted file mode 100644 index 5174e6b51..000000000 --- a/examples/lighthouse/lighthouse_config_visualization.py +++ /dev/null @@ -1,155 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, see . -""" -Simple script for visualizing the Ligithouse positioning system's configuration -using matplotlib. Each base station is represented by a local coordinate frame, while -each one's coverage is represented by 2 circular sectors; a horizontal and a vertical one. -Notice that the base station coordinate frame is defined as: - - X-axis pointing forward through the glass - - Y-axis pointing right, when the base station is seen from the front. - - Z-axis pointing up - -To run the script, just change the path to your .yaml file. -""" -import matplotlib.pyplot as plt -import numpy as np -import yaml - -config_file = 'lighthouse.yaml' # Add the path to your .yaml file - -Range = 5 # Range of each base station in meters -FoV_h = 150 # Horizontal Field of View in degrees -FoV_v = 110 # Vertical Field of View in degrees - - -def draw_coordinate_frame(ax, P, R, label='', length=0.5, is_bs=False): - """Draw a coordinate frame at position t with orientation R.""" - x_axis = R @ np.array([length, 0, 0]) - y_axis = R @ np.array([0, length, 0]) - z_axis = R @ np.array([0, 0, length]) - - ax.quiver(P[0], P[1], P[2], x_axis[0], x_axis[1], x_axis[2], color='r', linewidth=2) - ax.quiver(P[0], P[1], P[2], y_axis[0], y_axis[1], y_axis[2], color='g', linewidth=2) - ax.quiver(P[0], P[1], P[2], z_axis[0], z_axis[1], z_axis[2], color='b', linewidth=2) - if is_bs: - ax.scatter(P[0], P[1], P[2], s=50, color='black') - ax.text(P[0], P[1], P[2], label, fontsize=10, color='black') - - -def draw_horizontal_sector(ax, P, R, radius=Range, angle_deg=FoV_h, color='r', alpha=0.3, n_points=50): - """ - Draw a circular sector centered at the origin of the local coordinate frame,lying in - the local XY-plane, so that its central axis is aligned with the positive X-axis. - """ - # Angle range (centered on X-axis) - half_angle = np.deg2rad(angle_deg / 2) - thetas = np.linspace(-half_angle, half_angle, n_points) - - # Circle points in local XY-plane - x_local = radius * np.cos(thetas) - y_local = radius * np.sin(thetas) - z_local = np.zeros_like(thetas) - - # Stack the coordinates into a 3xN matix - pts_local = np.vstack([x_local, y_local, z_local]) - - # Transfer the points to the global frame, creating a 3xN matrix - pts_global = R @ pts_local + P.reshape(3, 1) - - # Close the sector by adding the center point at the start and end - X = np.concatenate(([P[0]], pts_global[0, :], [P[0]])) - Y = np.concatenate(([P[1]], pts_global[1, :], [P[1]])) - Z = np.concatenate(([P[2]], pts_global[2, :], [P[2]])) - - # Plot filled sector - ax.plot_trisurf(X, Y, Z, color=color, alpha=alpha, linewidth=0) - - -def draw_vertical_sector(ax, P, R, radius=Range, angle_deg=FoV_v, color='r', alpha=0.3, n_points=50): - """ - Draw a circular sector centered at the origin of the local coordinate frame,lying in - the local XZ-plane, so that its central axis is aligned with the positive X-axis. - """ - # Angle range (centered on X-axis) - half_angle = np.deg2rad(angle_deg / 2) - thetas = np.linspace(-half_angle, half_angle, n_points) - - # Circle points in local XZ-plane - x_local = radius * np.cos(thetas) - y_local = np.zeros_like(thetas) - z_local = radius * np.sin(thetas) - - # Stack the coordinates into a 3xN matix - pts_local = np.vstack([x_local, y_local, z_local]) - - # Transfer the points to the global frame, creating a 3xN matrix - pts_global = R @ pts_local + P.reshape(3, 1) - - # Close the sector by adding the center point at the start and end - X = np.concatenate(([P[0]], pts_global[0, :], [P[0]])) - Y = np.concatenate(([P[1]], pts_global[1, :], [P[1]])) - Z = np.concatenate(([P[2]], pts_global[2, :], [P[2]])) - - # Plot filled sector - ax.plot_trisurf(X, Y, Z, color=color, alpha=alpha, linewidth=0) - - -if __name__ == '__main__': - # Load the .yamnl file - with open(config_file, 'r') as f: - data = yaml.safe_load(f) - geos = data['geos'] - - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - - # Draw global frame - draw_coordinate_frame(ax, np.zeros(3), np.eye(3), label='Global', length=1.0) - - # Draw local frames + sectors - for key, geo in geos.items(): - origin = np.array(geo['origin']) - rotation = np.array(geo['rotation']) - draw_coordinate_frame(ax, origin, rotation, label=f'BS {key+1}', length=0.5, is_bs=True) - - # Local XY-plane sector - draw_horizontal_sector(ax, origin, rotation, radius=Range, angle_deg=FoV_h, color='red', alpha=0.15) - - # Local YZ-plane sector - draw_vertical_sector(ax, origin, rotation, radius=Range, angle_deg=FoV_v, color='red', alpha=0.15) - - ax.set_xlabel('X') - ax.set_ylabel('Y') - ax.set_zlabel('Z') - ax.set_title('Lighthouse Visualization') - - # Set equal aspect ratio - all_points = [np.array(geo['origin']) for geo in geos.values()] - all_points.append(np.zeros(3)) - all_points = np.array(all_points) - max_range = np.ptp(all_points, axis=0).max() - mid = all_points.mean(axis=0) - ax.set_xlim(mid[0] - max_range/2, mid[0] + max_range/2) - ax.set_ylim(mid[1] - max_range/2, mid[1] + max_range/2) - ax.set_zlim(mid[2] - max_range/2, mid[2] + max_range/2) - - plt.show() diff --git a/examples/lighthouse/lighthouse_mass_upload.py b/examples/lighthouse/lighthouse_mass_upload.py deleted file mode 100644 index ee4747761..000000000 --- a/examples/lighthouse/lighthouse_mass_upload.py +++ /dev/null @@ -1,88 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, see . -""" -Simple script that connects to multiple Crazyflies in sequence and uploads -the lighthouse configuration file. The Crazyflies that successfully received -the file are powered off, while the ones that didn't get it remain powered on. -This could be really helpful if you're dealing with a big swarm of Crazyflies. - -Make sure that each Crazyflie has a lighthouse deck attached. -""" -import os -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.localization import LighthouseConfigWriter -from cflib.utils import uri_helper -from cflib.utils.power_switch import PowerSwitch - -file_path = 'lighthouse.yaml' # Add the path to your .yaml file - -# Modify the list of Crazyflies according to your setup -uris = [ - 'radio://0/80/2M/E7E7E7E7E7', - 'radio://0/80/2M/E7E7E7E7E8', - 'radio://0/80/2M/E7E7E7E7E9', - 'radio://0/80/2M/E7E7E7E7EA', - 'radio://0/80/2M/E7E7E7E7EB', - 'radio://0/80/2M/E7E7E7E7EC', -] - - -def write_one(file_name, scf: SyncCrazyflie): - print(f'Writing to \033[92m{uri}\033[97m...', end='', flush=True) - writer = LighthouseConfigWriter(scf.cf) - writer.write_and_store_config_from_file(None, file_name) - print('Success!') - time.sleep(1) - - -if __name__ == '__main__': - if not os.path.exists(file_path): - print('Error: file not found!') - sys.exit(1) - - print(f'Using file {file_path}') - - cflib.crtp.init_drivers() - - for uri in uris: - try: - Drone = uri_helper.uri_from_env(default=uri) - with SyncCrazyflie(Drone, cf=Crazyflie(rw_cache='./cache')) as scf: - print(f'\033[92m{uri} \033[97mFully connected ', end='', flush=True) - while scf.is_params_updated() is False: - print('.', end='', flush=True) - time.sleep(0.1) - print(f'{scf.is_params_updated()}') - time.sleep(0.5) - write_one(file_path, scf) - ps = PowerSwitch(Drone) - ps.platform_power_down() - time.sleep(2) - - except (Exception): - print(f'Couldnt connect to \033[91m{uri}\033[97m') - continue diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py deleted file mode 100644 index 632ebc9d7..000000000 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python3 -# Demo that makes one Crazyflie take off 30cm above the first controller found -# Using the controller trigger it is then possible to 'grab' the Crazyflie -# and to make it move. -import sys -import time - -import openvr - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = 'radio://0/80/2M/E7E7E7E7E7' - -print('Opening') -vr = openvr.init(openvr.VRApplication_Other) -print('Opened') - -# Find first controller or tracker -controllerId = None -poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) -for i in range(openvr.k_unMaxTrackedDeviceCount): - if poses[i].bPoseIsValid: - device_class = vr.getTrackedDeviceClass(i) - if device_class == openvr.TrackedDeviceClass_Controller or \ - device_class == openvr.TrackedDeviceClass_GenericTracker: - controllerId = i - break - -if controllerId is None: - print('Cannot find controller or tracker, exiting') - sys.exit(1) - - -def position_callback(timestamp, data, logconf): - x = data['kalman.stateX'] - y = data['kalman.stateY'] - z = data['kalman.stateZ'] - print('pos: ({}, {}, {})'.format(x, y, z)) - - -def start_position_printing(scf): - log_conf = LogConfig(name='Position', period_in_ms=500) - log_conf.add_variable('kalman.stateX', 'float') - log_conf.add_variable('kalman.stateY', 'float') - log_conf.add_variable('kalman.stateZ', 'float') - - scf.cf.log.add_config(log_conf) - log_conf.data_received_cb.add_callback(position_callback) - log_conf.start() - - -def vector_substract(v0, v1): - return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] - - -def vector_add(v0, v1): - return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] - - -def run_sequence(scf): - cf = scf.cf - - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3] - - grabbed = False - grab_controller_start = [0, 0, 0] - grab_setpoint_start = [0, 0, 0] - - while True: - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) - controller_state = vr.getControllerState(controllerId)[1] - - trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - - if not grabbed and trigger: - print('Grab started') - grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - grab_setpoint_start = setpoint - - if grabbed and not trigger: - print('Grab ended') - - grabbed = trigger - - if trigger: - curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - setpoint = vector_add(grab_setpoint_start, - vector_substract(curr, - grab_controller_start)) - - cf.commander.send_position_setpoint(setpoint[0], - setpoint[1], - setpoint[2], - 0) - time.sleep(0.02) - - 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) - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - reset_estimator(scf) - - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - run_sequence(scf) - - openvr.shutdown() diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py deleted file mode 100644 index 64b5a5812..000000000 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ /dev/null @@ -1,163 +0,0 @@ -#!/usr/bin/env python3 -# Demo that makes one Crazyflie take off 30cm above the first controller found -# Using the controller trigger it is then possible to 'grab' the Crazyflie -# and to make it move. -# If the Crazyflie has a ledring attached, the touchpad of the controller can -# be used to change the color of the led-ring LEDs -import colorsys -import math -import sys -import time - -import openvr - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = 'radio://0/80/2M/E7E7E7E7E7' - -print('Openning') -vr = openvr.init(openvr.VRApplication_Other) -print('Oppened') - -# Find first controller or tracker -controllerId = None -poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) -for i in range(openvr.k_unMaxTrackedDeviceCount): - if poses[i].bPoseIsValid: - device_class = vr.getTrackedDeviceClass(i) - if device_class == openvr.TrackedDeviceClass_Controller or\ - device_class == openvr.TrackedDeviceClass_GenericTracker: - controllerId = i - break - -if controllerId is None: - print('Cannot find controller or tracker, exiting') - sys.exit(1) - - -def position_callback(timestamp, data, logconf): - x = data['kalman.stateX'] - y = data['kalman.stateY'] - z = data['kalman.stateZ'] - print('pos: ({}, {}, {})'.format(x, y, z)) - - -def start_position_printing(scf): - log_conf = LogConfig(name='Position', period_in_ms=500) - log_conf.add_variable('kalman.stateX', 'float') - log_conf.add_variable('kalman.stateY', 'float') - log_conf.add_variable('kalman.stateZ', 'float') - - scf.cf.log.add_config(log_conf) - log_conf.data_received_cb.add_callback(position_callback) - log_conf.start() - - -def vector_substract(v0, v1): - return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] - - -def vector_add(v0, v1): - return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] - - -def run_sequence(scf): - cf = scf.cf - - cf.param.set_value('ring.effect', '7') - - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - # setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3] - setpoint = [0, 0, 0.5] - - grabbed = False - grab_controller_start = [0, 0, 0] - grab_setpoint_start = [0, 0, 0] - - while True: - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) - controller_state = vr.getControllerState(controllerId)[1] - - trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0) - trackpad_touched = ((controller_state.ulButtonTouched & 0x100000000) != 0) # noqa - - trackpad = (0, 0) - for i in range(openvr.k_unControllerStateAxisCount): - axis_type = vr.getInt32TrackedDeviceProperty( - controllerId, openvr.Prop_Axis0Type_Int32 + i)[0] - if axis_type == openvr.k_eControllerAxis_TrackPad: - trackpad = (controller_state.rAxis[i].x, - controller_state.rAxis[i].y) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - - if trackpad_touched: - angle = math.atan2(trackpad[1], trackpad[0]) - hue = (angle + math.pi) / (2*math.pi) - rgb = colorsys.hsv_to_rgb(hue, 1, 0.3) - - print(rgb) - - cf.param.set_value('ring.solidRed', - '{}'.format(int(rgb[0] * 255))) - cf.param.set_value('ring.solidGreen', - '{}'.format(int(rgb[1] * 255))) - cf.param.set_value('ring.solidBlue', - '{}'.format(int(rgb[2] * 255))) - - if not grabbed and trigger: - print('Grab started') - grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - grab_setpoint_start = setpoint - - if grabbed and not trigger: - print('Grab ended') - - grabbed = trigger - - if trigger: - curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]] - setpoint = vector_add(grab_setpoint_start, - vector_substract(curr, grab_controller_start) - ) - - cf.commander.send_position_setpoint(setpoint[0], - setpoint[1], - setpoint[2], - 0) - time.sleep(0.02) - - 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) - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - reset_estimator(scf) - # start_position_printing(scf) - - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - run_sequence(scf) - - openvr.shutdown() diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py deleted file mode 100644 index 2c3134ff3..000000000 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python3 -# Demo that makes two Crazyflie take off 30cm above the first controller found -# Using the controller trigger it is then possible to 'grab' the closest -# Crazyflie and to make it move. -import math -import sys -import time - -import openvr - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri0 = 'radio://0/80/2M/E7E7E7E701' -uri1 = 'radio://0/80/2M/E7E7E7E702' - -print('Opening') -vr = openvr.init(openvr.VRApplication_Other) -print('Opened') - -# Find first controller or tracker -controllerId = None -poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) -for i in range(openvr.k_unMaxTrackedDeviceCount): - if poses[i].bPoseIsValid: - device_class = vr.getTrackedDeviceClass(i) - if device_class == openvr.TrackedDeviceClass_Controller or \ - device_class == openvr.TrackedDeviceClass_GenericTracker: - controllerId = i - break - -if controllerId is None: - print('Cannot find controller or tracker, exiting') - sys.exit(1) - - -def position_callback(timestamp, data, logconf): - x = data['kalman.stateX'] - y = data['kalman.stateY'] - z = data['kalman.stateZ'] - print('pos: ({}, {}, {})'.format(x, y, z)) - - -def start_position_printing(scf): - log_conf = LogConfig(name='Position', period_in_ms=500) - log_conf.add_variable('kalman.stateX', 'float') - log_conf.add_variable('kalman.stateY', 'float') - log_conf.add_variable('kalman.stateZ', 'float') - - scf.cf.log.add_config(log_conf) - log_conf.data_received_cb.add_callback(position_callback) - log_conf.start() - - -def vector_subtract(v0, v1): - return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]] - - -def vector_add(v0, v1): - return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]] - - -def vector_norm(v0): - return math.sqrt((v0[0] * v0[0]) + (v0[1] * v0[1]) + (v0[2] * v0[2])) - - -def run_sequence(scf0, scf1): - cf0 = scf0.cf - cf1 = scf1.cf - - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount) - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - setpoints = [[-1 * pose[2][3], -1 * pose[0][3] - 0.5, pose[1][3] + 0.3], - [-1 * pose[2][3], -1 * pose[0][3] + 0.5, pose[1][3] + 0.3]] - - closest = 0 - - grabbed = False - grab_controller_start = [0, 0, 0] - grab_setpoint_start = [0, 0, 0] - - while True: - poses = vr.getDeviceToAbsoluteTrackingPose( - openvr.TrackingUniverseStanding, 0, - openvr.k_unMaxTrackedDeviceCount) - controller_state = vr.getControllerState(controllerId)[1] - - trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0) - - controller_pose = poses[controllerId] - pose = controller_pose.mDeviceToAbsoluteTracking - - if not grabbed and trigger: - print('Grab started') - grab_controller_start = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]] - - dist0 = vector_norm(vector_subtract(grab_controller_start, - setpoints[0])) - dist1 = vector_norm(vector_subtract(grab_controller_start, - setpoints[1])) - - if dist0 < dist1: - closest = 0 - else: - closest = 1 - - grab_setpoint_start = setpoints[closest] - - if grabbed and not trigger: - print('Grab ended') - - grabbed = trigger - - if trigger: - curr = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]] - setpoints[closest] = vector_add( - grab_setpoint_start, vector_subtract(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() - - 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) - - # Arm the Crazyflies - scf0.cf.platform.send_arming_request(True) - scf1.cf.platform.send_arming_request(True) - time.sleep(1.0) - - run_sequence(scf0, scf1) - - openvr.shutdown() diff --git a/examples/lighthouse/multi_bs_geometry_estimation.py b/examples/lighthouse/multi_bs_geometry_estimation.py deleted file mode 100644 index 7bdb7c498..000000000 --- a/examples/lighthouse/multi_bs_geometry_estimation.py +++ /dev/null @@ -1,422 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2022 Bitcraze AB -# -# 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, see . -''' -This functionality is experimental and may not work properly! - -Script to run a full base station geometry estimation of a lighthouse -system. The script records data from a Crazyflie that is moved around in -the flight space and creates a solution that minimizes the error -in the measured positions. - -The execution of the script takes you through a number of steps, please follow -the instructions. - -This script works with 2 or more base stations (if supported by the CF firmware). - -This script is a temporary implementation until similar functionality has been -added to the client. - -REQUIREMENTS: -- Lighthouse v2 base stations are required for this example. The Lighthouse deck - will be set to Lighthouse v2 mode automatically. - -Prerequisites: -1. The base station calibration data must have been -received by the Crazyflie before this script is executed. - -2. 2 or more base stations -''' -from __future__ import annotations - -import logging -import pickle -import time -from threading import Event - -import numpy as np - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.localization.lighthouse_bs_vector import LighthouseBsVectors -from cflib.localization.lighthouse_config_manager import LighthouseConfigWriter -from cflib.localization.lighthouse_geometry_solver import LighthouseGeometrySolver -from cflib.localization.lighthouse_initial_estimator import LighthouseInitialEstimator -from cflib.localization.lighthouse_sample_matcher import LighthouseSampleMatcher -from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleAverageReader -from cflib.localization.lighthouse_sweep_angle_reader import LighthouseSweepAngleReader -from cflib.localization.lighthouse_system_aligner import LighthouseSystemAligner -from cflib.localization.lighthouse_system_scaler import LighthouseSystemScaler -from cflib.localization.lighthouse_types import LhCfPoseSample -from cflib.localization.lighthouse_types import LhDeck4SensorPositions -from cflib.localization.lighthouse_types import LhMeasurement -from cflib.localization.lighthouse_types import Pose -from cflib.utils import uri_helper - -REFERENCE_DIST = 1.0 - - -def record_angles_average(scf: SyncCrazyflie, timeout: float = 5.0) -> LhCfPoseSample: - """Record angles and average over the samples to reduce noise""" - recorded_angles = None - - is_ready = Event() - - def ready_cb(averages): - nonlocal recorded_angles - recorded_angles = averages - is_ready.set() - - reader = LighthouseSweepAngleAverageReader(scf.cf, ready_cb) - reader.start_angle_collection() - - if not is_ready.wait(timeout): - print('Recording timed out.') - return None - - angles_calibrated = {} - for bs_id, data in recorded_angles.items(): - angles_calibrated[bs_id] = data[1] - - result = LhCfPoseSample(angles_calibrated=angles_calibrated) - - visible = ', '.join(map(lambda x: str(x + 1), recorded_angles.keys())) - print(f' Position recorded, base station ids visible: {visible}') - - if len(recorded_angles.keys()) < 2: - print('Received too few base stations, we need at least two. Please try again!') - result = None - - return result - - -def record_angles_sequence(scf: SyncCrazyflie, recording_time_s: float) -> list[LhCfPoseSample]: - """Record angles and return a list of the samples""" - result: list[LhCfPoseSample] = [] - - bs_seen = set() - - def ready_cb(bs_id: int, angles: LighthouseBsVectors): - now = time.time() - measurement = LhMeasurement(timestamp=now, base_station_id=bs_id, angles=angles) - result.append(measurement) - bs_seen.add(str(bs_id + 1)) - - reader = LighthouseSweepAngleReader(scf.cf, ready_cb) - reader.start() - end_time = time.time() + recording_time_s - - while time.time() < end_time: - time_left = int(end_time - time.time()) - visible = ', '.join(sorted(bs_seen)) - print(f'{time_left}s, bs visible: {visible}') - bs_seen = set() - time.sleep(0.5) - - reader.stop() - - return result - - -def parse_recording_time(recording_time: str, default: int) -> int: - """Interpret recording time input by user""" - try: - return int(recording_time) - except ValueError: - return default - - -def print_base_stations_poses(base_stations: dict[int, Pose]): - """Pretty print of base stations pose""" - for bs_id, pose in sorted(base_stations.items()): - pos = pose.translation - print(f' {bs_id + 1}: ({pos[0]}, {pos[1]}, {pos[2]})') - - -def set_axes_equal(ax): - '''Make axes of 3D plot have equal scale so that spheres appear as spheres, - cubes as cubes, etc.. This is one possible solution to Matplotlib's - ax.set_aspect('equal') and ax.axis('equal') not working for 3D. - - Input - ax: a matplotlib axis, e.g., as output from plt.gca(). - ''' - - x_limits = ax.get_xlim3d() - y_limits = ax.get_ylim3d() - z_limits = ax.get_zlim3d() - - x_range = abs(x_limits[1] - x_limits[0]) - x_middle = np.mean(x_limits) - y_range = abs(y_limits[1] - y_limits[0]) - y_middle = np.mean(y_limits) - z_range = abs(z_limits[1] - z_limits[0]) - z_middle = np.mean(z_limits) - - # The plot bounding box is a sphere in the sense of the infinity - # norm, hence I call half the max range the plot radius. - plot_radius = 0.5*max([x_range, y_range, z_range]) - - ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) - ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius]) - ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius]) - - -def visualize(cf_poses: list[Pose], bs_poses: list[Pose]): - """Visualize positions of base stations and Crazyflie positions""" - # Set to True to visualize positions - # Requires PyPlot - visualize_positions = False - if visualize_positions: - import matplotlib.pyplot as plt - - positions = np.array(list(map(lambda x: x.translation, cf_poses))) - - fig = plt.figure() - ax = fig.add_subplot(projection='3d') - - x_cf = positions[:, 0] - y_cf = positions[:, 1] - z_cf = positions[:, 2] - - ax.scatter(x_cf, y_cf, z_cf) - - positions = np.array(list(map(lambda x: x.translation, bs_poses))) - - x_bs = positions[:, 0] - y_bs = positions[:, 1] - z_bs = positions[:, 2] - - ax.scatter(x_bs, y_bs, z_bs, c='red') - - set_axes_equal(ax) - print('Close graph window to continue') - plt.show() - - -def write_to_file(name: str, - origin: LhCfPoseSample, - x_axis: list[LhCfPoseSample], - xy_plane: list[LhCfPoseSample], - samples: list[LhCfPoseSample]): - with open(name, 'wb') as handle: - data = (origin, x_axis, xy_plane, samples) - pickle.dump(data, handle, protocol=pickle.HIGHEST_PROTOCOL) - - -def load_from_file(name: str): - with open(name, 'rb') as handle: - return pickle.load(handle) - - -def estimate_geometry(origin: LhCfPoseSample, - x_axis: list[LhCfPoseSample], - xy_plane: list[LhCfPoseSample], - samples: list[LhCfPoseSample]) -> dict[int, Pose]: - """Estimate the geometry of the system based on samples recorded by a Crazyflie""" - matched_samples = [origin] + x_axis + xy_plane + LighthouseSampleMatcher.match(samples, min_nr_of_bs_in_match=2) - initial_guess, cleaned_matched_samples = LighthouseInitialEstimator.estimate( - matched_samples, LhDeck4SensorPositions.positions) - - print('Initial guess base stations at:') - print_base_stations_poses(initial_guess.bs_poses) - - print(f'{len(cleaned_matched_samples)} samples will be used') - visualize(initial_guess.cf_poses, initial_guess.bs_poses.values()) - - solution = LighthouseGeometrySolver.solve(initial_guess, cleaned_matched_samples, LhDeck4SensorPositions.positions) - if not solution.success: - print('Solution did not converge, it might not be good!') - - start_x_axis = 1 - start_xy_plane = 1 + len(x_axis) - origin_pos = solution.cf_poses[0].translation - x_axis_poses = solution.cf_poses[start_x_axis:start_x_axis + len(x_axis)] - x_axis_pos = list(map(lambda x: x.translation, x_axis_poses)) - xy_plane_poses = solution.cf_poses[start_xy_plane:start_xy_plane + len(xy_plane)] - xy_plane_pos = list(map(lambda x: x.translation, xy_plane_poses)) - - print('Raw solution:') - print(' Base stations at:') - print_base_stations_poses(solution.bs_poses) - print(' Solution match per base station:') - for bs_id, value in solution.error_info['bs'].items(): - print(f' {bs_id + 1}: {value}') - - # Align the solution - bs_aligned_poses, transformation = LighthouseSystemAligner.align( - origin_pos, x_axis_pos, xy_plane_pos, solution.bs_poses) - - cf_aligned_poses = list(map(transformation.rotate_translate_pose, solution.cf_poses)) - - # Scale the solution - bs_scaled_poses, cf_scaled_poses, scale = LighthouseSystemScaler.scale_fixed_point(bs_aligned_poses, - cf_aligned_poses, - [REFERENCE_DIST, 0, 0], - cf_aligned_poses[1]) - - print() - print('Final solution:') - print(' Base stations at:') - print_base_stations_poses(bs_scaled_poses) - - visualize(cf_scaled_poses, bs_scaled_poses.values()) - - return bs_scaled_poses - - -def upload_geometry(scf: SyncCrazyflie, bs_poses: dict[int, Pose]): - """Upload the geometry to the Crazyflie""" - geo_dict = {} - for bs_id, pose in bs_poses.items(): - geo = LighthouseBsGeometry() - geo.origin = pose.translation.tolist() - geo.rotation_matrix = pose.rot_matrix.tolist() - geo.valid = True - geo_dict[bs_id] = geo - - event = Event() - - def data_written(_): - event.set() - - helper = LighthouseConfigWriter(scf.cf) - helper.write_and_store_config(data_written, geos=geo_dict) - event.wait() - - -def estimate_from_file(file_name: str): - origin, x_axis, xy_plane, samples = load_from_file(file_name) - estimate_geometry(origin, x_axis, xy_plane, samples) - - -def get_recording(scf: SyncCrazyflie): - data = None - while True: # Infinite loop, will break on valid measurement - input('Press return when ready. ') - print(' Recording...') - measurement = record_angles_average(scf) - if measurement is not None: - data = measurement - break # Exit the loop if a valid measurement is obtained - else: - time.sleep(1) - print('Invalid measurement, please try again.') - return data - - -def get_multiple_recordings(scf: SyncCrazyflie): - data = [] - first_attempt = True - - while True: - if first_attempt: - user_input = input('Press return to record a measurement: ').lower() - first_attempt = False - else: - user_input = input('Press return to record another measurement, or "q" to continue: ').lower() - - if user_input == 'q' and data: - break - elif user_input == 'q' and not data: - print('You must record at least one measurement.') - continue - - print(' Recording...') - measurement = record_angles_average(scf) - if measurement is not None: - data.append(measurement) - else: - time.sleep(1) - print('Invalid measurement, please try again.') - - return data - - -def connect_and_estimate(uri: str, file_name: str | None = None): - """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(' Setting lighthouse deck to v2 mode...') - scf.cf.param.set_value('lighthouse.systemType', 2) - print(' Lighthouse deck set to v2 mode') - 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 = get_recording(scf) - - 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 = [get_recording(scf)] - - 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.') - xy_plane = get_multiple_recordings(scf) - - 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') - - if file_name: - write_to_file(file_name, origin, x_axis, xy_plane, samples) - print(f'Wrote data to file {file_name}') - - 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') - - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - # Set a file name to write the measurement data to file. Useful for debugging - file_name = None - # file_name = 'lh_geo_estimate_data.pickle' - - connect_and_estimate(uri, file_name=file_name) - - # Run the estimation on data from file instead of live measurements - # estimate_from_file(file_name) diff --git a/examples/lighthouse/read_lighthouse_mem.py b/examples/lighthouse/read_lighthouse_mem.py deleted file mode 100644 index 42b85dcea..000000000 --- a/examples/lighthouse/read_lighthouse_mem.py +++ /dev/null @@ -1,76 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Example of how to read the Lighthouse base station geometry and -calibration memory from a Crazyflie -""" -import logging -from threading import Event - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import LighthouseMemHelper -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class ReadMem: - def __init__(self, uri): - self._event = Event() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - helper = LighthouseMemHelper(scf.cf) - - helper.read_all_geos(self._geo_read_ready) - self._event.wait() - - self._event.clear() - - helper.read_all_calibs(self._calib_read_ready) - self._event.wait() - - def _geo_read_ready(self, geo_data): - for id, data in geo_data.items(): - print('---- Geometry for base station', id + 1) - data.dump() - print() - self._event.set() - - def _calib_read_ready(self, calib_data): - for id, data in calib_data.items(): - print('---- Calibration data for base station', id + 1) - data.dump() - print() - self._event.set() - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - ReadMem(uri) diff --git a/examples/lighthouse/write_lighthouse_mem.py b/examples/lighthouse/write_lighthouse_mem.py deleted file mode 100644 index c35e99651..000000000 --- a/examples/lighthouse/write_lighthouse_mem.py +++ /dev/null @@ -1,130 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Example of how to write to the Lighthouse base station geometry -and calibration memory in a Crazyflie -""" -import logging -from threading import Event - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import LighthouseBsCalibration -from cflib.crazyflie.mem import LighthouseBsGeometry -from cflib.crazyflie.mem import LighthouseMemHelper -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class WriteMem: - def __init__(self, uri, geo_dict, calib_dict): - self._event = Event() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - helper = LighthouseMemHelper(scf.cf) - - helper.write_geos(geo_dict, self._data_written) - self._event.wait() - - self._event.clear() - - helper.write_calibs(calib_dict, self._data_written) - self._event.wait() - - def _data_written(self, success): - if success: - print('Data written') - else: - print('Write failed') - - self._event.set() - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - bs1geo = LighthouseBsGeometry() - bs1geo.origin = [1.0, 2.0, 3.0] - bs1geo.rotation_matrix = [ - [4.0, 5.0, 6.0], - [7.0, 8.0, 9.0], - [10.0, 11.0, 12.0], - ] - bs1geo.valid = True - - bs2geo = LighthouseBsGeometry() - bs2geo.origin = [21.0, 22.0, 23.0] - bs2geo.rotation_matrix = [ - [24.0, 25.0, 26.0], - [27.0, 28.0, 29.0], - [30.0, 31.0, 32.0], - ] - bs2geo.valid = True - - bs1calib = LighthouseBsCalibration() - bs1calib.sweeps[0].phase = 1.0 - bs1calib.sweeps[0].tilt = 2.0 - bs1calib.sweeps[0].curve = 3.0 - bs1calib.sweeps[0].gibmag = 4.0 - bs1calib.sweeps[0].gibphase = 5.0 - bs1calib.sweeps[0].ogeemag = 6.0 - bs1calib.sweeps[0].ogeephase = 7.0 - bs1calib.sweeps[1].phase = 1.1 - bs1calib.sweeps[1].tilt = 2.1 - bs1calib.sweeps[1].curve = 3.1 - bs1calib.sweeps[1].gibmag = 4.1 - bs1calib.sweeps[1].gibphase = 5.1 - bs1calib.sweeps[1].ogeemag = 6.1 - bs1calib.sweeps[1].ogeephase = 7.1 - bs1calib.uid = 1234 - bs1calib.valid = True - - bs2calib = LighthouseBsCalibration() - bs2calib.sweeps[0].phase = 1.5 - bs2calib.sweeps[0].tilt = 2.5 - bs2calib.sweeps[0].curve = 3.5 - bs2calib.sweeps[0].gibmag = 4.5 - bs2calib.sweeps[0].gibphase = 5.5 - bs2calib.sweeps[0].ogeemag = 6.5 - bs2calib.sweeps[0].ogeephase = 7.5 - bs2calib.sweeps[1].phase = 1.51 - bs2calib.sweeps[1].tilt = 2.51 - bs2calib.sweeps[1].curve = 3.51 - bs2calib.sweeps[1].gibmag = 4.51 - bs2calib.sweeps[1].gibphase = 5.51 - bs2calib.sweeps[1].ogeemag = 6.51 - bs2calib.sweeps[1].ogeephase = 7.51 - bs2calib.uid = 9876 - bs2calib.valid = True - - # Note: base station ids (channels) are 0-indexed - geo_dict = {0: bs1geo, 1: bs2geo} - calib_dict = {0: bs1calib, 1: bs2calib} - - WriteMem(uri, geo_dict, calib_dict) diff --git a/examples/link_quality/latency.py b/examples/link_quality/latency.py deleted file mode 100644 index 21faf8aa0..000000000 --- a/examples/link_quality/latency.py +++ /dev/null @@ -1,55 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2024 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -import time - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Reads the CFLIB_URI environment variable for URI or uses default -uri = uri_helper.uri_from_env(default='radio://0/90/2M/E7E7E7E7E7') - - -def latency_callback(latency: float): - """A callback to run when we get an updated latency estimate""" - print(f'Latency: {latency:.3f} ms') - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - # Create Crazyflie object, with cache to avoid re-reading param and log TOC - cf = Crazyflie(rw_cache='./cache') - - # Add a callback to whenever we receive an updated latency estimate - # - # This could also be a Python lambda, something like: - cf.link_statistics.latency.latency_updated.add_callback(latency_callback) - - # This will connect the Crazyflie with the URI specified above. - with SyncCrazyflie(uri, cf=cf) as scf: - print('[host] Connected, use ctrl-c to quit.') - - while True: - time.sleep(1) diff --git a/examples/loco_nodes/lps_reboot_to_bootloader.py b/examples/loco_nodes/lps_reboot_to_bootloader.py deleted file mode 100644 index 10a07066f..000000000 --- a/examples/loco_nodes/lps_reboot_to_bootloader.py +++ /dev/null @@ -1,100 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017 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, see . -""" -Simple example that connects to the first Crazyflie found, and then sends the -reboot signals to 6 anchors ID from 0 to 5. The reset signals is sent -10 times in a row to make sure all anchors are reset to bootloader. -""" -import logging -import time -from threading import Thread - -import cflib -from cflib.crazyflie import Crazyflie -from cflib.utils import uri_helper -from lpslib.lopoanchor import LoPoAnchor - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -logging.basicConfig(level=logging.ERROR) - - -class LpsRebootToBootloader: - """Example that connects to a Crazyflie and ramps the motors up/down and - the disconnects""" - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - self._cf = Crazyflie() - - 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): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - - # Start a separate thread to do the motor test. - # Do not hijack the calling thread! - Thread(target=self._reboot_thread).start() - - 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)) - - 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) - - def _reboot_thread(self): - - anchors = LoPoAnchor(self._cf) - - print('Sending reboot signal to all anchors 10 times in a row ...') - for retry in range(10): - for anchor_id in range(6): - anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER) - time.sleep(0.1) - - self._cf.close_link() - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - le = LpsRebootToBootloader(uri) diff --git a/examples/memory/flash_memory.py b/examples/memory/flash_memory.py deleted file mode 100644 index e08e2784a..000000000 --- a/examples/memory/flash_memory.py +++ /dev/null @@ -1,221 +0,0 @@ -# -*- coding: utf-8 -*- -# -# Copyright (C) 2015 Danilo Bargen -# -# 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, see . -""" -Flash the DS28E05 EEPROM via CRTP. -""" -import datetime -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.utils import uri_helper - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -class NotConnected(RuntimeError): - pass - - -class Flasher(object): - """ - A class that can flash the DS28E05 EEPROM via CRTP. - """ - - def __init__(self, link_uri): - self._cf = Crazyflie() - self._link_uri = link_uri - - # Add 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) - - # Initialize variables - self.connected = False - self.should_disconnect = False - - # Public methods - - def connect(self): - """ - Connect to the crazyflie. - """ - print('Connecting to %s' % self._link_uri) - self._cf.open_link(self._link_uri) - - def disconnect(self): - print('Disconnecting from %s' % self._link_uri) - self._cf.close_link() - - def wait_for_connection(self, timeout=10): - """ - Busy loop until connection is established. - - Will abort after timeout (seconds). Return value is a boolean, whether - connection could be established. - - """ - start_time = datetime.datetime.now() - while True: - if self.connected: - return True - now = datetime.datetime.now() - if (now - start_time).total_seconds() > timeout: - return False - time.sleep(0.5) - - def search_memories(self): - """ - Search and return list of 1-wire memories. - """ - if not self.connected: - raise NotConnected() - return self._cf.mem.get_mems(MemoryElement.TYPE_1W) - - # Callbacks - - def _connected(self, link_uri): - print('Connected to %s' % link_uri) - self.connected = True - - def _disconnected(self, link_uri): - print('Disconnected from %s' % link_uri) - self.connected = False - - def _connection_failed(self, link_uri, msg): - print('Connection to %s failed: %s' % (link_uri, msg)) - self.connected = False - - def _connection_lost(self, link_uri, msg): - print('Connection to %s lost: %s' % (link_uri, msg)) - self.connected = False - - -def choose(items, title_text, question_text): - """ - Interactively choose one of the items. - """ - print(title_text) - - for i, item in enumerate(items, start=1): - print('%d) %s' % (i, item)) - print('%d) Abort' % (i + 1)) - - selected = input(question_text) - try: - index = int(selected) - except ValueError: - index = -1 - if not (index - 1) in range(len(items)): - print('Aborting.') - return None - - return items[index - 1] - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - # Initialize flasher - flasher = Flasher(uri) - - def abort(): - flasher.disconnect() - sys.exit(1) - - # Connect to Crazyflie - flasher.connect() - connected = flasher.wait_for_connection() - if not connected: - print('Connection failed.') - abort() - - # Search for memories - mems = flasher.search_memories() - if not mems: - print('No memories found.') - abort() - mem = choose(mems, 'Available memories:', 'Select memory: ') - if mem is None: - print('Aborting.') - abort() - - # Print information about memory - print('You selected the following memory:') - print(' Name: %s' % mem.name) - print(' Vendor ID: 0x%X' % mem.vid) - print(' Memory ID: 0x%X' % mem.pid) - print(' Pins: 0x%X' % mem.pins) - print(' Elements: %s' % mem.elements) - - # Ask for new information - print('Please specify what information to write. If you just press enter, ' - 'the value will not be changed.') - - # Vendor ID - vid_input = input('New vendor ID: ') - if vid_input != '': - try: - vid = int(vid_input, 0) - if not 0 <= vid <= 0xff: - raise ValueError() - except ValueError: - print('Invalid vendor ID. Please specify a number between 0x00 ' - 'and 0xff.') - abort() - else: - mem.vid = vid - - # Memory ID - pid_input = input('New memory ID: ') - if pid_input != '': - try: - pid = int(pid_input, 0) - if not 0 <= pid <= 0xff: - raise ValueError() - except ValueError: - print('Invalid memory ID. Please specify a number between 0x00 ' - 'and 0xff.') - abort() - else: - mem.pid = pid - - # Callback function when data has been written - def data_written(mem, addr): - print('Data has been written to memory!') - flasher.should_disconnect = True - - # Write data - sure = input('Are you sure? [y/n] ') - if sure != 'y': - print('Better safe than sorry!') - abort() - mem.write_data(data_written) - - # Wait for completion or timeout (10 seconds) - for _ in range(10 * 2): - if flasher.should_disconnect: - flasher.disconnect() - break - time.sleep(0.5) - else: - print('Apparently data could not be written to memory... :(') - flasher.disconnect() diff --git a/examples/memory/read_deck_mem.py b/examples/memory/read_deck_mem.py deleted file mode 100644 index 4969bfe87..000000000 --- a/examples/memory/read_deck_mem.py +++ /dev/null @@ -1,102 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Example of how to read the memory from a deck -""" -import logging -from threading import Event - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class ReadMem: - def __init__(self, uri): - self._event = Event() - self._cf = Crazyflie(rw_cache='./cache') - - with SyncCrazyflie(uri, cf=self._cf) as scf: - mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) - - count = len(mems) - if count != 1: - raise Exception('Unexpected nr of memories found:', count) - - mem = mems[0] - mem.query_decks(self.query_complete_cb) - self._event.wait() - - if len(mem.deck_memories.items()) == 0: - print('No memories to read') - - for id, deck_mem in mem.deck_memories.items(): - print('-----') - print('Deck id:', id) - print('Name:', deck_mem.name) - print('is_started:', deck_mem.is_started) - print('supports_read:', deck_mem.supports_read) - print('supports_write:', deck_mem.supports_write) - - if deck_mem.supports_fw_upgrade: - print('This deck supports FW upgrades') - print( - f' The required FW is: hash={deck_mem.required_hash}, ' - f'length={deck_mem.required_length}, name={deck_mem.name}') - print(' is_fw_upgrade_required:', deck_mem.is_fw_upgrade_required) - if (deck_mem.is_bootloader_active): - print(' In bootloader mode, ready to be flashed') - else: - print(' In FW mode') - print('') - - if deck_mem.supports_read: - print('Reading first 10 bytes of memory') - self._event.clear() - deck_mem.read(0, 10, self.read_complete, self.read_failed) - self._event.wait() - - def query_complete_cb(self, deck_memories): - self._event.set() - - def read_complete(self, addr, data): - print(data) - self._event.set() - - def read_failed(self, addr): - print('Read failed @ {}'.format(addr)) - self._event.set() - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - rm = ReadMem(uri) diff --git a/examples/memory/read_eeprom.py b/examples/memory/read_eeprom.py deleted file mode 100644 index ce8766618..000000000 --- a/examples/memory/read_eeprom.py +++ /dev/null @@ -1,134 +0,0 @@ -# -*- 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, see . -""" -Simple example that connects to the first Crazyflie found, looks for -EEPROM memories and lists its contents. -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.utils import uri_helper - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class EEPROMExample: - """ - Simple example listing the EEPROMs found and lists its contents. - """ - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # 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.should_disconnect = False - - 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) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C) - print('Found {} EEPOM(s)'.format(len(mems))) - self._mems_to_update = len(mems) - for m in mems: - print('Updating id={}'.format(m.id)) - m.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tElements : ') - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self._mems_to_update -= 1 - if self._mems_to_update == 0: - self.should_disconnect = True - - def _stab_log_error(self, logconf, msg): - """Callback from the log API when an error occurs""" - print('Error when logging %s: %s' % (logconf.name, msg)) - - def _stab_log_data(self, timestamp, data, logconf): - """Callback from a the log API when data arrives""" - print('[%d][%s]: %s' % (timestamp, logconf.name, data)) - - 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 - cflib.crtp.init_drivers() - - le = EEPROMExample(uri) - - # 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. - try: - while le.is_connected: - if le.should_disconnect: - le._cf.close_link() - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/memory/read_l5.py b/examples/memory/read_l5.py deleted file mode 100644 index 723145455..000000000 --- a/examples/memory/read_l5.py +++ /dev/null @@ -1,75 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Example of how to read the memory from the multiranger -""" -import logging -import time -from threading import Event - -import matplotlib.pyplot as plt - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class ReadMem: - def __init__(self, uri): - self._event = Event() - self._cf = Crazyflie(rw_cache='./cache') - - with SyncCrazyflie(uri, cf=self._cf) as scf: - mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MULTIRANGER) - - count = len(mems) - if count != 1: - raise Exception('Unexpected nr of memories found:', count) - - mem = mems[0] - - data = [[0 for x in range(8)] for y in range(8)] - im = plt.imshow(data, cmap='gray', vmin=0, vmax=400) - - start_time = time.time() - for frames in range(100): - data = mem.read_data_sync() - im.set_data(data) - plt.pause(0.01) - - end_time = time.time() - print('FPS: {}'.format(100/(end_time - start_time))) - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - rm = ReadMem(uri) diff --git a/examples/memory/read_ow.py b/examples/memory/read_ow.py deleted file mode 100644 index b94f59832..000000000 --- a/examples/memory/read_ow.py +++ /dev/null @@ -1,84 +0,0 @@ -# -*- 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, see . -""" -Simple example that connects to a Crazyflie, looks for -1-wire memories and lists its contents. -""" -import logging -import sys -from threading import Event - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - -update_event = Event() - - -def read_ow_mems(cf): - mems = cf.mem.get_mems(MemoryElement.TYPE_1W) - print(f'Found {len(mems)} 1-wire memories') - - for m in mems: - update_event.clear() - - print(f'Reading id={m.id}') - m.update(data_updated_cb) - success = update_event.wait(timeout=5.0) - if not success: - print(f'Mem read time out for memory {m.id}') - sys.exit(1) - - -def data_updated_cb(mem): - print(f'Got id={mem.id}') - print(f'\tAddr : {mem.addr}') - print(f'\tType : {mem.type}') - print(f'\tSize : {mem.size}') - print(f'\tValid : {mem.valid}') - print(f'\tName : {mem.name}') - print(f'\tVID : 0x{mem.vid:02X}') - print(f'\tPID : 0x{mem.pid:02X}') - print(f'\tPins : 0x{mem.pins:02X}') - print('\tElements : ') - - for key, element in mem.elements.items(): - print(f'\t\t{key}={element}') - - update_event.set() - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - read_ow_mems(scf.cf) diff --git a/examples/memory/read_paa3905.py b/examples/memory/read_paa3905.py deleted file mode 100644 index 96fab14e5..000000000 --- a/examples/memory/read_paa3905.py +++ /dev/null @@ -1,77 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Example of how to read the memory from the multiranger -""" -import logging -import time -from threading import Event - -import matplotlib.pyplot as plt - -import cflib.crtp # noqa -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class ReadMem: - def __init__(self, uri): - self._event = Event() - self._cf = Crazyflie(rw_cache='./cache') - - with SyncCrazyflie(uri, cf=self._cf) as scf: - mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_PAA3905) - - count = len(mems) - if count != 1: - raise Exception('Unexpected nr of memories found:', count) - - mem = mems[0] - - data = [[0 for x in range(35)] for y in range(35)] - im = plt.imshow(data, cmap='gray', vmin=0, vmax=255, origin='upper') - - start_time = time.time() - for frames in range(100): - data = mem.read_data_sync() - im.set_data(data) - plt.pause(0.01) - - end_time = time.time() - print('FPS: {}'.format(100/(end_time - start_time))) - time.sleep(5) - - -if __name__ == '__main__': - # URI to the Crazyflie to connect to - # uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - uri = uri_helper.uri_from_env(default='usb://0') - - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - rm = ReadMem(uri) diff --git a/examples/memory/write_eeprom.py b/examples/memory/write_eeprom.py deleted file mode 100644 index 0a5305e45..000000000 --- a/examples/memory/write_eeprom.py +++ /dev/null @@ -1,146 +0,0 @@ -# -*- 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, see . -""" -Simple example that connects to the first Crazyflie found, looks for -EEPROM memories and writes the default values in it. -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.utils import uri_helper - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class EEPROMExample: - """ - Simple example listing the EEPROMs found and writes the default values - in it. - """ - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # 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.should_disconnect = False - - 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) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C) - print('Found {} EEPOM(s)'.format(len(mems))) - if len(mems) > 0: - print('Writing default configuration to' - ' memory {}'.format(mems[0].id)) - - elems = mems[0].elements - elems['version'] = 1 - elems['pitch_trim'] = 0.0 - elems['roll_trim'] = 0.0 - elems['radio_channel'] = 80 - elems['radio_speed'] = 2 - elems['radio_address'] = 0xE7E7E7E7E7 - - mems[0].write_data(self._data_written) - - def _data_written(self, mem, addr): - print('Data written, reading back...') - mem.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tElements : ') - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self.should_disconnect = True - - def _stab_log_error(self, logconf, msg): - """Callback from the log API when an error occurs""" - print('Error when logging %s: %s' % (logconf.name, msg)) - - def _stab_log_data(self, timestamp, data, logconf): - """Callback from the log API when data arrives""" - print('[%d][%s]: %s' % (timestamp, logconf.name, data)) - - 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 - cflib.crtp.init_drivers() - - le = EEPROMExample(uri) - - # 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. - try: - while le.is_connected: - if le.should_disconnect: - le._cf.close_link() - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/memory/write_ow.py b/examples/memory/write_ow.py deleted file mode 100644 index 36ded0c3b..000000000 --- a/examples/memory/write_ow.py +++ /dev/null @@ -1,149 +0,0 @@ -# -*- 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, see . -""" -This example connects to the first Crazyflie that it finds and writes to the -one wire memory. -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.mem import OWElement -from cflib.utils import uri_helper - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -class WriteOwExample: - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - # Create a Crazyflie object without specifying any cache dirs - self._cf = Crazyflie() - - # 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.should_disconnect = False - - 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) - - mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) - print('Found {} 1-wire memories'.format(len(mems))) - if len(mems) > 0: - print('Writing test configuration to' - ' memory {}'.format(mems[0].id)) - - # Setting VID:PID to 00:00 will make the Crazyflie match driver to the board name - mems[0].vid = 0x00 - mems[0].pid = 0x00 - - board_name_id = OWElement.element_mapping[1] - board_rev_id = OWElement.element_mapping[2] - - mems[0].elements[board_name_id] = 'Hello deck' - mems[0].elements[board_rev_id] = 'A' - - mems[0].write_data(self._data_written) - - def _data_written(self, mem, addr): - print('Data written, reading back...') - mem.update(self._data_updated) - - def _data_updated(self, mem): - print('Updated id={}'.format(mem.id)) - print('\tType : {}'.format(mem.type)) - print('\tSize : {}'.format(mem.size)) - print('\tValid : {}'.format(mem.valid)) - print('\tName : {}'.format(mem.name)) - print('\tVID : 0x{:02X}'.format(mem.vid)) - print('\tPID : 0x{:02X}'.format(mem.pid)) - print('\tPins : 0x{:02X}'.format(mem.pins)) - print('\tElements : ') - - for key in mem.elements: - print('\t\t{}={}'.format(key, mem.elements[key])) - - self.should_disconnect = True - - def _stab_log_error(self, logconf, msg): - """Callback from the log API when an error occurs""" - print('Error when logging %s: %s' % (logconf.name, msg)) - - def _stab_log_data(self, timestamp, data, logconf): - """Callback from the log API when data arrives""" - print('[%d][%s]: %s' % (timestamp, logconf.name, data)) - - 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 - cflib.crtp.init_drivers() - - le = WriteOwExample(uri) - - # 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. - try: - while le.is_connected: - if le.should_disconnect: - le._cf.close_link() - time.sleep(1) - except KeyboardInterrupt: - sys.exit(1) diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py deleted file mode 100644 index 162a8fd0c..000000000 --- a/examples/mocap/mocap_hl_commander.py +++ /dev/null @@ -1,192 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2023 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -""" -Example of how to connect to a motion capture system and feed the position to a -Crazyflie, using the motioncapture library. The motioncapture library supports all major mocap systems and provides -a generalized API regardless of system type. -The script uses the high level commander to upload a trajectory to fly a figure 8. - -Set the uri to the radio settings of the Crazyflie and modify the -mocap setting matching your system. -""" -import time -from threading import Thread - -import motioncapture - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.mem import Poly4D -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# The host name or ip address of the mocap system -host_name = '192.168.5.21' - -# The type of the mocap system -# Valid options are: 'vicon', 'optitrack', 'optitrack_closed_source', 'qualisys', 'nokov', 'vrpn', 'motionanalysis' -mocap_system_type = 'qualisys' - -# The name of the rigid body that represents the Crazyflie -rigid_body_name = 'cf' - -# True: send position and orientation; False: send position only -send_full_pose = True - -# When using full pose, the estimator can be sensitive to noise in the orientation data when yaw is close to +/- 90 -# degrees. If this is a problem, increase orientation_std_dev a bit. The default value in the firmware is 4.5e-3. -orientation_std_dev = 8.0e-3 - -# The trajectory to fly -# See https://github.com/whoenig/uav_trajectories for a tool to generate -# trajectories - -# Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 -figure8 = [ - [1.050000, 0.000000, -0.000000, 0.000000, -0.000000, 0.830443, -0.276140, -0.384219, 0.180493, -0.000000, 0.000000, -0.000000, 0.000000, -1.356107, 0.688430, 0.587426, -0.329106, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.710000, 0.396058, 0.918033, 0.128965, -0.773546, 0.339704, 0.034310, -0.026417, -0.030049, -0.445604, -0.684403, 0.888433, 1.493630, -1.361618, -0.139316, 0.158875, 0.095799, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.620000, 0.922409, 0.405715, -0.582968, -0.092188, -0.114670, 0.101046, 0.075834, -0.037926, -0.291165, 0.967514, 0.421451, -1.086348, 0.545211, 0.030109, -0.050046, -0.068177, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.700000, 0.923174, -0.431533, -0.682975, 0.177173, 0.319468, -0.043852, -0.111269, 0.023166, 0.289869, 0.724722, -0.512011, -0.209623, -0.218710, 0.108797, 0.128756, -0.055461, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.560000, 0.405364, -0.834716, 0.158939, 0.288175, -0.373738, -0.054995, 0.036090, 0.078627, 0.450742, -0.385534, -0.954089, 0.128288, 0.442620, 0.055630, -0.060142, -0.076163, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.560000, 0.001062, -0.646270, -0.012560, -0.324065, 0.125327, 0.119738, 0.034567, -0.063130, 0.001593, -1.031457, 0.015159, 0.820816, -0.152665, -0.130729, -0.045679, 0.080444, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.700000, -0.402804, -0.820508, -0.132914, 0.236278, 0.235164, -0.053551, -0.088687, 0.031253, -0.449354, -0.411507, 0.902946, 0.185335, -0.239125, -0.041696, 0.016857, 0.016709, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.620000, -0.921641, -0.464596, 0.661875, 0.286582, -0.228921, -0.051987, 0.004669, 0.038463, -0.292459, 0.777682, 0.565788, -0.432472, -0.060568, -0.082048, -0.009439, 0.041158, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.710000, -0.923935, 0.447832, 0.627381, -0.259808, -0.042325, -0.032258, 0.001420, 0.005294, 0.288570, 0.873350, -0.515586, -0.730207, -0.026023, 0.288755, 0.215678, -0.148061, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [1.053185, -0.398611, 0.850510, -0.144007, -0.485368, -0.079781, 0.176330, 0.234482, -0.153567, 0.447039, -0.532729, -0.855023, 0.878509, 0.775168, -0.391051, -0.713519, 0.391628, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa -] - - -class MocapWrapper(Thread): - def __init__(self, body_name): - Thread.__init__(self) - - self.body_name = body_name - self.on_pose = None - self._stay_open = True - - self.start() - - def close(self): - self._stay_open = False - - def run(self): - mc = motioncapture.connect(mocap_system_type, {'hostname': host_name}) - while self._stay_open: - mc.waitForNextFrame() - for name, obj in mc.rigidBodies.items(): - if name == self.body_name: - if self.on_pose: - pos = obj.position - self.on_pose([pos[0], pos[1], pos[2], obj.rotation]) - - -def send_extpose_quat(cf, x, y, z, quat): - """ - Send the current Crazyflie X, Y, Z position and attitude as a quaternion. - This is going to be forwarded to the Crazyflie's position estimator. - """ - if send_full_pose: - cf.extpos.send_extpose(x, y, z, quat.x, quat.y, quat.z, quat.w) - else: - cf.extpos.send_extpos(x, y, z) - - -def adjust_orientation_sensitivity(cf): - cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) - - -def activate_kalman_estimator(cf): - cf.param.set_value('stabilizer.estimator', '2') - - # Set the std deviation for the quaternion data pushed into the - # kalman filter. The default value seems to be a bit too low. - cf.param.set_value('locSrv.extQuatStdDev', 0.06) - - -def activate_mellinger_controller(cf): - cf.param.set_value('stabilizer.controller', '2') - - -def upload_trajectory(cf, trajectory_id, trajectory): - trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] - trajectory_mem.trajectory = [] - - total_duration = 0 - for row in trajectory: - duration = row[0] - x = Poly4D.Poly(row[1:9]) - y = Poly4D.Poly(row[9:17]) - z = Poly4D.Poly(row[17:25]) - yaw = Poly4D.Poly(row[25:33]) - trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw)) - total_duration += duration - - trajectory_mem.write_data_sync() - cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory)) - return total_duration - - -def run_sequence(cf, trajectory_id, duration): - commander = cf.high_level_commander - - commander.takeoff(1.0, 2.0) - time.sleep(3.0) - relative = True - commander.start_trajectory(trajectory_id, 1.0, relative) - time.sleep(duration) - commander.land(0.0, 2.0) - time.sleep(2) - commander.stop() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - # Connect to the mocap system - mocap_wrapper = MocapWrapper(rigid_body_name) - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - trajectory_id = 1 - - # Set up a callback to handle data from the mocap system - mocap_wrapper.on_pose = lambda pose: send_extpose_quat(cf, pose[0], pose[1], pose[2], pose[3]) - - adjust_orientation_sensitivity(cf) - activate_kalman_estimator(cf) - # activate_mellinger_controller(cf) - duration = upload_trajectory(cf, trajectory_id, figure8) - print('The sequence is {:.1f} seconds long'.format(duration)) - reset_estimator(cf) - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - run_sequence(cf, trajectory_id, duration) - - mocap_wrapper.close() diff --git a/examples/mocap/mocap_swarm.py b/examples/mocap/mocap_swarm.py deleted file mode 100644 index 134586028..000000000 --- a/examples/mocap/mocap_swarm.py +++ /dev/null @@ -1,144 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -""" -Example of how to connect to a motion capture system and feed positions (only) -to multiple Crazyflies, using the motioncapture library. The swarm takes off and -flies a synchronous square shape before landing. The trajectories are relative to -the starting positions and the Crazyflies can be at any position on the floor when -the script is started. The script uses the high level commander and the Swarm class. - -Set the uri to the radio settings of your Crazyflies, set the rigid body names and -modify the mocap setting matching your system. -""" -import time -from threading import Thread - -import motioncapture - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie - -# The type of the mocap system -# Valid options are: 'vicon', 'optitrack', 'optitrack_closed_source', 'qualisys', 'nokov', 'vrpn', 'motionanalysis' -mocap_system_type = 'optitrack' - -# The host name or ip address of the mocap system -host_name = '10.223.0.31' - -# Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive -swarm_config = [ - ('radio://0/80/2M/E7E7E7E7E7', 'cf1'), - # ('radio://0/80/2M/E7E7E7E7E8', 'cf2'), - # ('radio://0/80/2M/E7E7E7E7E9', 'cf3'), - # Add more URIs if you want more copters in the swarm -] - -uris = [uri for uri, _ in swarm_config] -rbs = {uri: name for uri, name in swarm_config} - - -class MocapWrapper(Thread): - def __init__(self, active_rbs_cfs): - Thread.__init__(self) - self.active_rbs_cfs = active_rbs_cfs - self._stay_open = True - self.counter = 0 - self.start() - - def close(self): - self._stay_open = False - - def run(self): - mc = motioncapture.connect(mocap_system_type, {'hostname': host_name}) - while self._stay_open: - mc.waitForNextFrame() - self.counter += 1 - for name, obj in mc.rigidBodies.items(): - if name in self.active_rbs_cfs: - pos = obj.position - # Only send positions - self.active_rbs_cfs[name].extpos.send_extpos(pos[0], pos[1], pos[2]) - if self.counter == 200: - print(f'Sent pos {pos} for {name}') - if self.counter == 200: - self.counter = 0 - - -def activate_kalman_estimator(scf: SyncCrazyflie): - scf.cf.param.set_value('stabilizer.estimator', '2') - - # Set the std deviation for the quaternion data pushed into the - # kalman filter. The default value seems to be a bit too low. - scf.cf.param.set_value('locSrv.extQuatStdDev', 0.06) - - -def arm(scf: SyncCrazyflie): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -def run_sequence(scf: SyncCrazyflie): - box_size = 1 - flight_time = 2 - - commander = scf.cf.high_level_commander - - commander.takeoff(1.0, 2.0) - time.sleep(3) - - commander.go_to(box_size, 0, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(0, box_size, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(-box_size, 0, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(0, -box_size, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.land(0.0, 2.0) - time.sleep(2) - - commander.stop() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - active_rbs_cfs = {rbs[uri]: scf.cf for uri, scf in swarm._cfs.items()} - mocap_thread = MocapWrapper(active_rbs_cfs) - - swarm.parallel_safe(activate_kalman_estimator) - time.sleep(1) - swarm.reset_estimators() - time.sleep(1) - swarm.parallel_safe(arm) - - swarm.parallel_safe(run_sequence) - - mocap_thread.close() diff --git a/examples/mocap/mocap_swarm_multi_commander.py b/examples/mocap/mocap_swarm_multi_commander.py deleted file mode 100644 index 71ab49778..000000000 --- a/examples/mocap/mocap_swarm_multi_commander.py +++ /dev/null @@ -1,149 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -""" -Example of how to connect to a motion capture system and feed positions (only) -to multiple Crazyflies, using the motioncapture library. - -The script uses the position high level and the motion commander to fly circles and waypoints. - -Set the uri to the radio settings of your Crazyflies, set the rigid body names and -modify the mocap setting matching your system. -""" -import time -from threading import Thread - -import motioncapture - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.motion_commander import MotionCommander -from cflib.positioning.position_hl_commander import PositionHlCommander - -# The type of the mocap system -# Valid options are: 'vicon', 'optitrack', 'optitrack_closed_source', 'qualisys', 'nokov', 'vrpn', 'motionanalysis' -mocap_system_type = 'optitrack' - -# The host name or ip address of the mocap system -host_name = '10.223.0.31' - -# Maps the URIs to the rigid-body names as streamed by, e.g., OptiTrack's Motive -swarm_config = [ - ('radio://0/80/2M/E7E7E7E7E7', 'cf1'), - # ('radio://0/80/2M/E7E7E7E7E8', 'cf2'), - # ('radio://0/80/2M/E7E7E7E7E9', 'cf3'), - # Add more URIs if you want more copters in the swarm -] - -uris = [uri for uri, _ in swarm_config] -rbs = {uri: name for uri, name in swarm_config} - - -class MocapWrapper(Thread): - def __init__(self, active_rbs_cfs): - Thread.__init__(self) - self.active_rbs_cfs = active_rbs_cfs - self._stay_open = True - self.counter = 0 - self.start() - - def close(self): - self._stay_open = False - - def run(self): - mc = motioncapture.connect(mocap_system_type, {'hostname': host_name}) - while self._stay_open: - mc.waitForNextFrame() - self.counter += 1 - for name, obj in mc.rigidBodies.items(): - if name in self.active_rbs_cfs: - pos = obj.position - # Only send positions - self.active_rbs_cfs[name].extpos.send_extpos(pos[0], pos[1], pos[2]) - if self.counter == 200: - print(f'Sent pos {pos} for {name}') - if self.counter == 200: - self.counter = 0 - - -def activate_kalman_estimator(scf: SyncCrazyflie): - scf.cf.param.set_value('stabilizer.estimator', '2') - - # Set the std deviation for the quaternion data pushed into the - # kalman filter. The default value seems to be a bit too low. - scf.cf.param.set_value('locSrv.extQuatStdDev', 0.06) - - -def run_sequence(scf: SyncCrazyflie): - print('This is: ', scf._link_uri) - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - # .takeoff() is automatic when entering the "with PositionHlCommander" context - if rbs[scf._link_uri] == 'cf1': - with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: - pc.set_default_velocity(0.5) - for i in range(3): # fly a triangle with changing altitude - pc.go_to(1.0, 1.0, 1.5) - pc.go_to(1.0, -1.0, 1.5) - pc.go_to(0.5, 0.0, 2.0) - pc.go_to(0.5, 0.0, 0.15) - elif rbs[scf._link_uri] == 'cf2': - with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: - pc.set_default_velocity(0.3) - for i in range(3): # fly side to side - pc.go_to(0.2, 1.0, 0.85) - pc.go_to(0.2, -1.0, 0.85) - pc.go_to(0.0, 0.0, 0.15) - elif rbs[scf._link_uri] == 'cf3': - with MotionCommander(scf) as mc: - # 2 right loops and 2 left loops - mc.back(0.8) - time.sleep(1) - mc.up(0.5) - time.sleep(1) - mc.circle_right(0.5, velocity=0.4, angle_degrees=720) - time.sleep(1) - mc.up(0.5) - time.sleep(1) - mc.circle_left(0.5, velocity=0.4, angle_degrees=720) - time.sleep(1) - mc.down(0.5) - # .land() is automatic when exiting the scope of context "with PositionHlCommander" - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - active_rbs_cfs = {rbs[uri]: scf.cf for uri, scf in swarm._cfs.items()} - mocap_thread = MocapWrapper(active_rbs_cfs) - - swarm.parallel_safe(activate_kalman_estimator) - time.sleep(1) - swarm.reset_estimators() - time.sleep(2) - swarm.parallel_safe(run_sequence) - - mocap_thread.close() diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py deleted file mode 100644 index d109a16ae..000000000 --- a/examples/mocap/qualisys_hl_commander.py +++ /dev/null @@ -1,258 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ,---------, ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, in version 3. -# -# 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, see . -""" -Example of how to connect to a Qualisys QTM system and feed the position to a -Crazyflie. It uses the high level commander to upload a trajectory to fly a -figure 8. - -Set the uri to the radio settings of the Crazyflie and modify the -rigid_body_name to match the name of the Crazyflie in QTM. - -Requires the qualisys optional dependency: - pip install cflib[qualisys] -""" -import asyncio -import math -import time -import xml.etree.cElementTree as ET -from threading import Thread - -import qtm_rt -from scipy.spatial.transform import Rotation - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.mem import Poly4D -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# The name of the rigid body in QTM that represents the Crazyflie -rigid_body_name = 'cf' - -# True: send position and orientation; False: send position only -send_full_pose = True - -# When using full pose, the estimator can be sensitive to noise in the orientation data when yaw is close to +/- 90 -# degrees. If this is a problem, increase orientation_std_dev a bit. The default value in the firmware is 4.5e-3. -orientation_std_dev = 8.0e-3 - -# The trajectory to fly -# See https://github.com/whoenig/uav_trajectories for a tool to generate -# trajectories - -# Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 -figure8 = [ - [1.050000, 0.000000, -0.000000, 0.000000, -0.000000, 0.830443, -0.276140, -0.384219, 0.180493, -0.000000, 0.000000, -0.000000, 0.000000, -1.356107, 0.688430, 0.587426, -0.329106, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.710000, 0.396058, 0.918033, 0.128965, -0.773546, 0.339704, 0.034310, -0.026417, -0.030049, -0.445604, -0.684403, 0.888433, 1.493630, -1.361618, -0.139316, 0.158875, 0.095799, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.620000, 0.922409, 0.405715, -0.582968, -0.092188, -0.114670, 0.101046, 0.075834, -0.037926, -0.291165, 0.967514, 0.421451, -1.086348, 0.545211, 0.030109, -0.050046, -0.068177, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.700000, 0.923174, -0.431533, -0.682975, 0.177173, 0.319468, -0.043852, -0.111269, 0.023166, 0.289869, 0.724722, -0.512011, -0.209623, -0.218710, 0.108797, 0.128756, -0.055461, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.560000, 0.405364, -0.834716, 0.158939, 0.288175, -0.373738, -0.054995, 0.036090, 0.078627, 0.450742, -0.385534, -0.954089, 0.128288, 0.442620, 0.055630, -0.060142, -0.076163, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.560000, 0.001062, -0.646270, -0.012560, -0.324065, 0.125327, 0.119738, 0.034567, -0.063130, 0.001593, -1.031457, 0.015159, 0.820816, -0.152665, -0.130729, -0.045679, 0.080444, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.700000, -0.402804, -0.820508, -0.132914, 0.236278, 0.235164, -0.053551, -0.088687, 0.031253, -0.449354, -0.411507, 0.902946, 0.185335, -0.239125, -0.041696, 0.016857, 0.016709, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.620000, -0.921641, -0.464596, 0.661875, 0.286582, -0.228921, -0.051987, 0.004669, 0.038463, -0.292459, 0.777682, 0.565788, -0.432472, -0.060568, -0.082048, -0.009439, 0.041158, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [0.710000, -0.923935, 0.447832, 0.627381, -0.259808, -0.042325, -0.032258, 0.001420, 0.005294, 0.288570, 0.873350, -0.515586, -0.730207, -0.026023, 0.288755, 0.215678, -0.148061, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa - [1.053185, -0.398611, 0.850510, -0.144007, -0.485368, -0.079781, 0.176330, 0.234482, -0.153567, 0.447039, -0.532729, -0.855023, 0.878509, 0.775168, -0.391051, -0.713519, 0.391628, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa -] - - -class QtmWrapper(Thread): - def __init__(self, body_name): - Thread.__init__(self) - - self.body_name = body_name - self.on_pose = None - self.connection = None - self.qtm_6DoF_labels = [] - self._stay_open = True - - self.start() - - def close(self): - self._stay_open = False - self.join() - - def run(self): - asyncio.run(self._life_cycle()) - - async def _life_cycle(self): - await self._connect() - while (self._stay_open): - await asyncio.sleep(1) - await self._close() - - async def _connect(self): - qtm_instance = await self._discover() - host = qtm_instance.host - print('Connecting to QTM on ' + host) - self.connection = await qtm_rt.connect(host) - - params = await self.connection.get_parameters(parameters=['6d']) - xml = ET.fromstring(params) - self.qtm_6DoF_labels = [label.text.strip() for index, label in enumerate(xml.findall('*/Body/Name'))] - - await self.connection.stream_frames( - components=['6D'], - on_packet=self._on_packet) - - async def _discover(self): - async for qtm_instance in qtm_rt.Discover('0.0.0.0'): - return qtm_instance - - def _on_packet(self, packet): - header, bodies = packet.get_6d() - - if bodies is None: - return - - if self.body_name not in self.qtm_6DoF_labels: - print('Body ' + self.body_name + ' not found.') - else: - index = self.qtm_6DoF_labels.index(self.body_name) - temp_cf_pos = bodies[index] - x = temp_cf_pos[0][0] / 1000 - y = temp_cf_pos[0][1] / 1000 - z = temp_cf_pos[0][2] / 1000 - - r = temp_cf_pos[1].matrix - rot = [ - [r[0], r[3], r[6]], - [r[1], r[4], r[7]], - [r[2], r[5], r[8]], - ] - - if self.on_pose: - # Make sure we got a position - if math.isnan(x): - return - - self.on_pose([x, y, z, rot]) - - async def _close(self): - await self.connection.stream_frames_stop() - self.connection.disconnect() - - -def _sqrt(a): - """ - There might be rounding errors making 'a' slightly negative. - Make sure we don't throw an exception. - """ - if a < 0.0: - return 0.0 - return math.sqrt(a) - - -def send_extpose_rot_matrix(cf, x, y, z, rot): - """ - Send the current Crazyflie X, Y, Z position and attitude as a (3x3) - rotaton matrix. This is going to be forwarded to the Crazyflie's - position estimator. - """ - quat = Rotation.from_matrix(rot).as_quat() - - if send_full_pose: - cf.extpos.send_extpose(x, y, z, quat[0], quat[1], quat[2], quat[3]) - else: - cf.extpos.send_extpos(x, y, z) - - -def adjust_orientation_sensitivity(cf): - cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) - - -def activate_kalman_estimator(cf): - cf.param.set_value('stabilizer.estimator', '2') - - # Set the std deviation for the quaternion data pushed into the - # kalman filter. The default value seems to be a bit too low. - cf.param.set_value('locSrv.extQuatStdDev', 0.06) - - -def activate_mellinger_controller(cf): - cf.param.set_value('stabilizer.controller', '2') - - -def upload_trajectory(cf, trajectory_id, trajectory): - trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] - trajectory_mem.trajectory = [] - - total_duration = 0 - for row in trajectory: - duration = row[0] - x = Poly4D.Poly(row[1:9]) - y = Poly4D.Poly(row[9:17]) - z = Poly4D.Poly(row[17:25]) - yaw = Poly4D.Poly(row[25:33]) - trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw)) - total_duration += duration - - trajectory_mem.write_data_sync() - cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory)) - return total_duration - - -def run_sequence(cf, trajectory_id, duration): - commander = cf.high_level_commander - - commander.takeoff(1.0, 2.0) - time.sleep(3.0) - relative = True - commander.start_trajectory(trajectory_id, 1.0, relative) - time.sleep(duration) - commander.land(0.0, 2.0) - time.sleep(2) - commander.stop() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - # Connect to QTM - qtm_wrapper = QtmWrapper(rigid_body_name) - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - trajectory_id = 1 - - # Set up a callback to handle data from QTM - qtm_wrapper.on_pose = lambda pose: send_extpose_rot_matrix( - cf, pose[0], pose[1], pose[2], pose[3]) - - adjust_orientation_sensitivity(cf) - activate_kalman_estimator(cf) - # activate_mellinger_controller(cf) - duration = upload_trajectory(cf, trajectory_id, figure8) - print('The sequence is {:.1f} seconds long'.format(duration)) - reset_estimator(cf) - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - run_sequence(cf, trajectory_id, duration) - - qtm_wrapper.close() diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py deleted file mode 100644 index 8c3e9340f..000000000 --- a/examples/motors/multiramp.py +++ /dev/null @@ -1,122 +0,0 @@ -# -*- 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, see . -""" -Simple example that connects 2 Crazyflies, ramp up-down the motors and -disconnects. -""" -import logging -import time -from threading import Thread - -import cflib -from cflib.crazyflie import Crazyflie - -logging.basicConfig(level=logging.ERROR) - - -class MotorRampExample: - """Example that connects to a Crazyflie and ramps the motors up/down and - the disconnects""" - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - self._cf = Crazyflie(rw_cache='./cache') - - 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) - - self.connected = True - - print('Connecting to %s' % link_uri) - - 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.""" - - # Arm the Crazyflie - self._cf.platform.send_arming_request(True) - time.sleep(1.0) - - # Start a separate thread to do the motor test. - # Do not hijack the calling thread! - Thread(target=self._ramp_motors).start() - - 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.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)) - self.connected = False - - def _disconnected(self, link_uri): - """Callback when the Crazyflie is disconnected (called in all cases)""" - print('Disconnected from %s' % link_uri) - self.connected = False - - def _ramp_motors(self): - thrust_mult = 1 - thrust_step = 500 - thrust = 20000 - pitch = 0 - roll = 0 - yawrate = 0 - - # Unlock startup thrust protection - self._cf.commander.send_setpoint(0, 0, 0, 0) - - while thrust >= 20000: - self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) - time.sleep(0.1) - if thrust >= 25000: - thrust_mult = -1 - thrust += thrust_step * thrust_mult - for _ in range(30): - # Continuously send the zero setpoint until the drone is recognized as landed - # to prevent the supervisor from intervening due to missing regular setpoints - self._cf.commander.send_setpoint(0, 0, 0, 0) - time.sleep(0.1) - # Sleeping before closing the link makes sure the last - # packet leaves before the link is closed, since the - # message queue is not flushed before closing - self._cf.close_link() - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - # Connect the two Crazyflies and ramps them up-down - le0 = MotorRampExample('radio://0/70/2M') - le1 = MotorRampExample('radio://1/80/250K') - while (le0.connected or le1.connected): - time.sleep(0.1) diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py deleted file mode 100644 index 3fe556388..000000000 --- a/examples/motors/ramp.py +++ /dev/null @@ -1,118 +0,0 @@ -# -*- 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, see . -""" -Simple example that connects to the first Crazyflie found, ramps up/down -the motors and disconnects. -""" -import logging -import time - -import cflib -from cflib.crazyflie import Crazyflie -from cflib.utils import uri_helper - -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -logging.basicConfig(level=logging.ERROR) - - -class MotorRampExample: - """Example that connects to a Crazyflie and ramps the motors up/down and - the disconnects""" - - def __init__(self, link_uri): - """ Initialize and run the example with the specified link_uri """ - - self._cf = Crazyflie(rw_cache='./cache') - - 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): - """ This callback is called form the Crazyflie API when a Crazyflie - has been connected and the TOCs have been downloaded.""" - - # Arm the Crazyflie - self._cf.platform.send_arming_request(True) - time.sleep(1.0) - - 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)) - - 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) - - def ramp_motors(self): - thrust_mult = 1 - thrust_step = 500 - thrust = 20000 - pitch = 0 - roll = 0 - yawrate = 0 - - # Unlock startup thrust protection - self._cf.commander.send_setpoint(0, 0, 0, 0) - - while thrust >= 20000: - self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) - time.sleep(0.1) - if thrust >= 25000: - thrust_mult = -1 - thrust += thrust_step * thrust_mult - for _ in range(30): - # Continuously send the zero setpoint until the drone is recognized as landed - # to prevent the supervisor from intervening due to missing regular setpoints - self._cf.commander.send_setpoint(0, 0, 0, 0) - # Sleeping before closing the link makes sure the last - # packet leaves before the link is closed, since the - # message queue is not flushed before closing - time.sleep(0.1) - - def disconnect(self): - self._cf.close_link() - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - me = MotorRampExample(uri) - - me.ramp_motors() - - me.disconnect() diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py deleted file mode 100644 index 837c7f570..000000000 --- a/examples/multiranger/multiranger_pointcloud.py +++ /dev/null @@ -1,369 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018 Bitcraze AB -# -# Crazyflie Python Library -# -# 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, see . -""" -Example script that plots the output ranges from the Multiranger and Flow -deck in a 3D plot. - -When the application is started the Crazyflie will hover at 0.3 m. The -Crazyflie can then be controlled by using keyboard input: - * Move by using the arrow keys (left/right/forward/backwards) - * Adjust the right with w/s (0.1 m for each keypress) - * Yaw slowly using a/d (CCW/CW) - * Yaw fast using z/x (CCW/CW) - -There's additional setting for (see constants below): - * Plotting the downwards sensor - * Plotting the estimated Crazyflie position - * Max threshold for sensors - * Speed factor that set's how fast the Crazyflie moves - -The demo is ended by either closing the graph window. - -For the example to run the following hardware is needed: - * Crazyflie 2.0 - * Crazyradio PA - * Flow deck - * Multiranger deck -""" -import logging -import math -import sys -from time import time - -import numpy as np -from vispy import scene -from vispy.scene import visuals -from vispy.scene.cameras import TurntableCamera - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.utils import uri_helper - -try: - from sip import setapi - setapi('QVariant', 2) - setapi('QString', 2) -except ImportError: - pass - -from PyQt6 import QtCore, QtWidgets - -logging.basicConfig(level=logging.INFO) - -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -if len(sys.argv) > 1: - URI = sys.argv[1] - -# Enable plotting of Crazyflie -PLOT_CF = False -# Enable plotting of down sensor -PLOT_SENSOR_DOWN = False -# Set the sensor threshold (in mm) -SENSOR_TH = 2000 -# Set the speed factor for moving and rotating -SPEED_FACTOR = 0.3 - - -class MainWindow(QtWidgets.QMainWindow): - - def __init__(self, URI): - QtWidgets.QMainWindow.__init__(self) - - self.resize(700, 500) - self.setWindowTitle('Multi-ranger point cloud') - - self.canvas = Canvas(self.updateHover) - self.canvas.create_native() - self.canvas.native.setParent(self) - - self.setCentralWidget(self.canvas.native) - - cflib.crtp.init_drivers() - self.cf = Crazyflie(ro_cache=None, rw_cache='cache') - - # Connect callbacks from the Crazyflie API - self.cf.connected.add_callback(self.connected) - self.cf.disconnected.add_callback(self.disconnected) - - # Connect to the Crazyflie - self.cf.open_link(URI) - - # Arm the Crazyflie - self.cf.platform.send_arming_request(True) - time.sleep(1.0) - - self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} - - self.hoverTimer = QtCore.QTimer() - self.hoverTimer.timeout.connect(self.sendHoverCommand) - self.hoverTimer.setInterval(100) - self.hoverTimer.start() - - def sendHoverCommand(self): - self.cf.commander.send_hover_setpoint( - self.hover['x'], self.hover['y'], self.hover['yaw'], - self.hover['height']) - - def updateHover(self, k, v): - if (k != 'height'): - self.hover[k] = v * SPEED_FACTOR - else: - self.hover[k] += v - - def disconnected(self, URI): - print('Disconnected') - - def connected(self, URI): - print('We are now connected to {}'.format(URI)) - - # The definition of the logconfig can be made before connecting - lpos = LogConfig(name='Position', period_in_ms=100) - lpos.add_variable('stateEstimate.x') - lpos.add_variable('stateEstimate.y') - lpos.add_variable('stateEstimate.z') - - try: - self.cf.log.add_config(lpos) - lpos.data_received_cb.add_callback(self.pos_data) - lpos.start() - except KeyError as e: - print('Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - print('Could not add Position log config, bad configuration.') - - lmeas = LogConfig(name='Meas', period_in_ms=100) - lmeas.add_variable('range.front') - lmeas.add_variable('range.back') - lmeas.add_variable('range.up') - lmeas.add_variable('range.left') - lmeas.add_variable('range.right') - lmeas.add_variable('range.zrange') - lmeas.add_variable('stabilizer.roll') - lmeas.add_variable('stabilizer.pitch') - lmeas.add_variable('stabilizer.yaw') - - try: - self.cf.log.add_config(lmeas) - lmeas.data_received_cb.add_callback(self.meas_data) - lmeas.start() - except KeyError as e: - print('Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - print('Could not add Measurement log config, bad configuration.') - - def pos_data(self, timestamp, data, logconf): - position = [ - data['stateEstimate.x'], - data['stateEstimate.y'], - data['stateEstimate.z'] - ] - self.canvas.set_position(position) - - def meas_data(self, timestamp, data, logconf): - measurement = { - 'roll': data['stabilizer.roll'], - 'pitch': data['stabilizer.pitch'], - 'yaw': data['stabilizer.yaw'], - 'front': data['range.front'], - 'back': data['range.back'], - 'up': data['range.up'], - 'down': data['range.zrange'], - 'left': data['range.left'], - 'right': data['range.right'] - } - self.canvas.set_measurement(measurement) - - def closeEvent(self, event): - if (self.cf is not None): - self.cf.close_link() - - -class Canvas(scene.SceneCanvas): - def __init__(self, keyupdateCB): - scene.SceneCanvas.__init__(self, keys=None) - self.size = 800, 600 - self.unfreeze() - self.view = self.central_widget.add_view() - self.view.bgcolor = '#ffffff' - self.view.camera = TurntableCamera( - fov=10.0, distance=30.0, up='+z', center=(0.0, 0.0, 0.0)) - self.last_pos = [0, 0, 0] - self.pos_markers = visuals.Markers() - self.meas_markers = visuals.Markers() - self.pos_data = np.array([0, 0, 0], ndmin=2) - self.meas_data = np.array([0, 0, 0], ndmin=2) - self.lines = [] - - self.view.add(self.pos_markers) - self.view.add(self.meas_markers) - for i in range(6): - line = visuals.Line() - self.lines.append(line) - self.view.add(line) - - self.keyCB = keyupdateCB - - self.freeze() - - scene.visuals.XYZAxis(parent=self.view.scene) - - def on_key_press(self, event): - if (not event.native.isAutoRepeat()): - if (event.native.key() == QtCore.Qt.Key.Key_Left): - self.keyCB('y', 1) - if (event.native.key() == QtCore.Qt.Key.Key_Right): - self.keyCB('y', -1) - if (event.native.key() == QtCore.Qt.Key.Key_Up): - self.keyCB('x', 1) - if (event.native.key() == QtCore.Qt.Key.Key_Down): - self.keyCB('x', -1) - if (event.native.key() == QtCore.Qt.Key.Key_A): - self.keyCB('yaw', -70) - if (event.native.key() == QtCore.Qt.Key.Key_D): - self.keyCB('yaw', 70) - if (event.native.key() == QtCore.Qt.Key.Key_Z): - self.keyCB('yaw', -200) - if (event.native.key() == QtCore.Qt.Key.Key_X): - self.keyCB('yaw', 200) - if (event.native.key() == QtCore.Qt.Key.Key_W): - self.keyCB('height', 0.1) - if (event.native.key() == QtCore.Qt.Key.Key_S): - self.keyCB('height', -0.1) - - def on_key_release(self, event): - if (not event.native.isAutoRepeat()): - if (event.native.key() == QtCore.Qt.Key.Key_Left): - self.keyCB('y', 0) - if (event.native.key() == QtCore.Qt.Key.Key_Right): - self.keyCB('y', 0) - if (event.native.key() == QtCore.Qt.Key.Key_Up): - self.keyCB('x', 0) - if (event.native.key() == QtCore.Qt.Key.Key_Down): - self.keyCB('x', 0) - if (event.native.key() == QtCore.Qt.Key.Key_A): - self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key.Key_D): - self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key.Key_W): - self.keyCB('height', 0) - if (event.native.key() == QtCore.Qt.Key.Key_S): - self.keyCB('height', 0) - if (event.native.key() == QtCore.Qt.Key.Key_Z): - self.keyCB('yaw', 0) - if (event.native.key() == QtCore.Qt.Key.Key_X): - self.keyCB('yaw', 0) - - def set_position(self, pos): - self.last_pos = pos - if (PLOT_CF): - self.pos_data = np.append(self.pos_data, [pos], axis=0) - self.pos_markers.set_data(self.pos_data, face_color='red', size=5) - - def rot(self, roll, pitch, yaw, origin, point): - cosr = math.cos(math.radians(roll)) - cosp = math.cos(math.radians(pitch)) - cosy = math.cos(math.radians(yaw)) - - sinr = math.sin(math.radians(roll)) - sinp = math.sin(math.radians(pitch)) - siny = math.sin(math.radians(yaw)) - - roty = np.array([[cosy, -siny, 0], - [siny, cosy, 0], - [0, 0, 1]]) - - rotp = np.array([[cosp, 0, sinp], - [0, 1, 0], - [-sinp, 0, cosp]]) - - rotr = np.array([[1, 0, 0], - [0, cosr, -sinr], - [0, sinr, cosr]]) - - rotFirst = np.dot(rotr, rotp) - - rot = np.array(np.dot(rotFirst, roty)) - - tmp = np.subtract(point, origin) - tmp2 = np.dot(rot, tmp) - return np.add(tmp2, origin) - - def rotate_and_create_points(self, m): - data = [] - o = self.last_pos - roll = m['roll'] - pitch = -m['pitch'] - yaw = m['yaw'] - - if (m['up'] < SENSOR_TH): - up = [o[0], o[1], o[2] + m['up'] / 1000.0] - data.append(self.rot(roll, pitch, yaw, o, up)) - - if (m['down'] < SENSOR_TH and PLOT_SENSOR_DOWN): - down = [o[0], o[1], o[2] - m['down'] / 1000.0] - data.append(self.rot(roll, pitch, yaw, o, down)) - - if (m['left'] < SENSOR_TH): - left = [o[0], o[1] + m['left'] / 1000.0, o[2]] - data.append(self.rot(roll, pitch, yaw, o, left)) - - if (m['right'] < SENSOR_TH): - right = [o[0], o[1] - m['right'] / 1000.0, o[2]] - data.append(self.rot(roll, pitch, yaw, o, right)) - - if (m['front'] < SENSOR_TH): - front = [o[0] + m['front'] / 1000.0, o[1], o[2]] - data.append(self.rot(roll, pitch, yaw, o, front)) - - if (m['back'] < SENSOR_TH): - back = [o[0] - m['back'] / 1000.0, o[1], o[2]] - data.append(self.rot(roll, pitch, yaw, o, back)) - - return data - - def set_measurement(self, measurements): - data = self.rotate_and_create_points(measurements) - o = self.last_pos - for i in range(6): - if (i < len(data)): - o = self.last_pos - self.lines[i].set_data(np.array([o, data[i]])) - else: - self.lines[i].set_data(np.array([o, o])) - - if (len(data) > 0): - self.meas_data = np.append(self.meas_data, data, axis=0) - self.meas_markers.set_data(self.meas_data, face_color='blue', size=5) - - -if __name__ == '__main__': - appQt = QtWidgets.QApplication(sys.argv) - win = MainWindow(URI) - win.show() - appQt.exec() diff --git a/examples/multiranger/multiranger_push.py b/examples/multiranger/multiranger_push.py deleted file mode 100755 index 491f30038..000000000 --- a/examples/multiranger/multiranger_push.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017 Bitcraze AB -# -# Crazyflie Python Library -# -# 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, see . -""" -Example scripts that allows a user to "push" the Crazyflie 2.0 around -using your hands while it's hovering. - -This examples uses the Flow and Multi-ranger decks to measure distances -in all directions and tries to keep away from anything that comes closer -than 0.2m by setting a velocity in the opposite direction. - -The demo is ended by either pressing Ctrl-C or by holding your hand above the -Crazyflie. - -For the example to run the following hardware is needed: - * Crazyflie 2.0 - * Crazyradio PA - * Flow deck - * Multiranger deck -""" -import logging -import sys -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.motion_commander import MotionCommander -from cflib.utils import uri_helper -from cflib.utils.multiranger import Multiranger - -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -if len(sys.argv) > 1: - URI = sys.argv[1] - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -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 - cflib.crtp.init_drivers() - - cf = Crazyflie(rw_cache='./cache') - with SyncCrazyflie(URI, cf=cf) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - with MotionCommander(scf) as motion_commander: - with Multiranger(scf) as multiranger: - keep_flying = True - - while keep_flying: - VELOCITY = 0.5 - velocity_x = 0.0 - velocity_y = 0.0 - - if is_close(multiranger.front): - velocity_x -= VELOCITY - if is_close(multiranger.back): - velocity_x += VELOCITY - - if is_close(multiranger.left): - velocity_y -= VELOCITY - if is_close(multiranger.right): - velocity_y += VELOCITY - - if is_close(multiranger.up): - keep_flying = False - - motion_commander.start_linear_motion( - velocity_x, velocity_y, 0) - - time.sleep(0.1) - - print('Demo terminated!') diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py deleted file mode 100644 index 7e06a59c3..000000000 --- a/examples/multiranger/multiranger_wall_following.py +++ /dev/null @@ -1,132 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2023 Bitcraze AB -# -# Crazyflie Python Library -# -# 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, see . -""" -Example script that makes the Crazyflie follow a wall - -This examples uses the Flow and Multi-ranger decks to measure distances -in all directions and do wall following. Straight walls with corners -are advised to have in the test environment. -This is a python port of c-based app layer example from the Crazyflie-firmware -found here https://github.com/bitcraze/crazyflie-firmware/tree/master/examples/ -demos/app_wall_following_demo - -For the example to run the following hardware is needed: - * Crazyflie 2.0 - * Crazyradio PA - * Flow deck - * Multiranger deck -""" -import logging -import time -from math import degrees -from math import radians - -from wall_following.wall_following import WallFollowing - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.crazyflie.syncLogger import SyncLogger -from cflib.positioning.motion_commander import MotionCommander -from cflib.utils import uri_helper -from cflib.utils.multiranger import Multiranger - -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -def handle_range_measurement(range): - if range is None: - range = 999 - return range - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - # Only output errors from the logging framework - logging.basicConfig(level=logging.ERROR) - - keep_flying = True - - wall_following = WallFollowing( - angle_value_buffer=0.1, reference_distance_from_wall=0.3, - max_forward_speed=0.3, init_state=WallFollowing.StateWallFollowing.FORWARD) - - # Setup logging to get the yaw data - lg_stab = LogConfig(name='Stabilizer', period_in_ms=100) - lg_stab.add_variable('stabilizer.yaw', 'float') - - cf = Crazyflie(rw_cache='./cache') - with SyncCrazyflie(URI, cf=cf) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - with MotionCommander(scf) as motion_commander: - with Multiranger(scf) as multiranger: - with SyncLogger(scf, lg_stab) as logger: - - while keep_flying: - - # initialize variables - velocity_x = 0.0 - velocity_y = 0.0 - yaw_rate = 0.0 - state_wf = WallFollowing.StateWallFollowing.HOVER - - # Get Yaw - log_entry = logger.next() - data = log_entry[1] - actual_yaw = data['stabilizer.yaw'] - actual_yaw_rad = radians(actual_yaw) - - # get front range in meters - front_range = handle_range_measurement(multiranger.front) - top_range = handle_range_measurement(multiranger.up) - left_range = handle_range_measurement(multiranger.left) - right_range = handle_range_measurement(multiranger.right) - - # choose here the direction that you want the wall following to turn to - wall_following_direction = WallFollowing.WallFollowingDirection.RIGHT - side_range = left_range - - # get velocity commands and current state from wall following state machine - velocity_x, velocity_y, yaw_rate, state_wf = wall_following.wall_follower( - front_range, side_range, actual_yaw_rad, wall_following_direction, time.time()) - - print('velocity_x', velocity_x, 'velocity_y', velocity_y, - 'yaw_rate', yaw_rate, 'state_wf', state_wf) - - # convert yaw_rate from rad to deg - yaw_rate_deg = degrees(yaw_rate) - - motion_commander.start_linear_motion( - velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg) - - # if top_range is activated, stop the demo - if top_range < 0.2: - keep_flying = False diff --git a/examples/multiranger/wall_following/wall_following.py b/examples/multiranger/wall_following/wall_following.py deleted file mode 100644 index deabf629f..000000000 --- a/examples/multiranger/wall_following/wall_following.py +++ /dev/null @@ -1,383 +0,0 @@ -# -*- coding: utf-8 -*- -# -# ........... ____ _ __ -# | ,-^-, | / __ )(_) /_______________ _____ ___ -# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# | / ,..´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# +....... /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# GNU general public license v3.0 -# -# Copyright (C) 2023 Bitcraze AB -# -# Crazyflie Python Library -# -# 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, see . -""" -file: wall_following.py - -Class for the wall following demo - -This is a python port of c-based app layer example from the Crazyflie-firmware -found here https://github.com/bitcraze/crazyflie-firmware/tree/master/examples/ -demos/app_wall_following_demo - -Author: Kimberly McGuire (Bitcraze AB) -""" -import math -from enum import Enum - - -class WallFollowing(): - class StateWallFollowing(Enum): - FORWARD = 1 - HOVER = 2 - TURN_TO_FIND_WALL = 3 - TURN_TO_ALIGN_TO_WALL = 4 - FORWARD_ALONG_WALL = 5 - ROTATE_AROUND_WALL = 6 - ROTATE_IN_CORNER = 7 - FIND_CORNER = 8 - - class WallFollowingDirection(Enum): - LEFT = 1 - RIGHT = -1 - - def __init__(self, reference_distance_from_wall=0.0, - max_forward_speed=0.2, - max_turn_rate=0.5, - wall_following_direction=WallFollowingDirection.LEFT, - first_run=False, - prev_heading=0.0, - wall_angle=0.0, - around_corner_back_track=False, - state_start_time=0.0, - ranger_value_buffer=0.2, - angle_value_buffer=0.1, - range_lost_threshold=0.3, - in_corner_angle=0.8, - wait_for_measurement_seconds=1.0, - init_state=StateWallFollowing.FORWARD): - """ - __init__ function for the WallFollowing class - - reference_distance_from_wall is the distance from the wall that the Crazyflie - should try to keep - max_forward_speed is the maximum speed the Crazyflie should fly forward - max_turn_rate is the maximum turn rate the Crazyflie should turn with - wall_following_direction is the direction the Crazyflie should follow the wall - (WallFollowingDirection Enum) - first_run is a boolean that is True if the Crazyflie is in the first run of the - wall following demo - prev_heading is the heading of the Crazyflie in the previous state (in rad) - wall_angle is the angle of the wall in the previous state (in rad) - around_corner_back_track is a boolean that is True if the Crazyflie is in the - around corner state and should back track - state_start_time is the time when the Crazyflie entered the current state (in s) - ranger_value_buffer is the buffer value for the ranger measurements (in m) - angle_value_buffer is the buffer value for the angle measurements (in rad) - range_lost_threshold is the threshold for when the Crazyflie should stop - following the wall (in m) - in_corner_angle is the angle the Crazyflie should turn when it is in the corner (in rad) - wait_for_measurement_seconds is the time the Crazyflie should wait for a - measurement before it starts the wall following demo (in s) - init_state is the initial state of the Crazyflie (StateWallFollowing Enum) - self.state is a shared state variable that is used to keep track of the current - state of the Crazyflie's wall following - self.time_now is a shared state variable that is used to keep track of the current (in s) - """ - - self.reference_distance_from_wall = reference_distance_from_wall - self.max_forward_speed = max_forward_speed - self.max_turn_rate = max_turn_rate - self.wall_following_direction_value = float(wall_following_direction.value) - self.first_run = first_run - self.prev_heading = prev_heading - self.wall_angle = wall_angle - self.around_corner_back_track = around_corner_back_track - self.state_start_time = state_start_time - self.ranger_value_buffer = ranger_value_buffer - self.angle_value_buffer = angle_value_buffer - self.range_threshold_lost = range_lost_threshold - self.in_corner_angle = in_corner_angle - self.wait_for_measurement_seconds = wait_for_measurement_seconds - - self.first_run = True - self.state = init_state - self.time_now = 0.0 - self.speed_redux_corner = 3.0 - self.speed_redux_straight = 2.0 - - # Helper function - def value_is_close_to(self, real_value, checked_value, margin): - if real_value > checked_value - margin and real_value < checked_value + margin: - return True - else: - return False - - def wrap_to_pi(self, number): - if number > math.pi: - return number - 2 * math.pi - elif number < -math.pi: - return number + 2 * math.pi - else: - return number - - # Command functions - def command_turn(self, reference_rate): - """ - Command the Crazyflie to turn around its yaw axis - - reference_rate and rate_yaw is defined in rad/s - velocity_x is defined in m/s - """ - velocity_x = 0.0 - rate_yaw = self.wall_following_direction_value * reference_rate - return velocity_x, rate_yaw - - def command_align_corner(self, reference_rate, side_range, wanted_distance_from_corner): - """ - Command the Crazyflie to align itself to the outer corner - and make sure it's at a certain distance from it - - side_range and wanted_distance_from_corner is defined in m - reference_rate and rate_yaw is defined in rad/s - velocity_x is defined in m/s - """ - if side_range > wanted_distance_from_corner + self.range_threshold_lost: - rate_yaw = self.wall_following_direction_value * reference_rate - velocity_y = 0.0 - else: - if side_range > wanted_distance_from_corner: - velocity_y = self.wall_following_direction_value * \ - (-1.0 * self.max_forward_speed / self.speed_redux_corner) - else: - velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_corner) - rate_yaw = 0.0 - return velocity_y, rate_yaw - - def command_hover(self): - """ - Command the Crazyflie to hover in place - """ - velocity_x = 0.0 - velocity_y = 0.0 - rate_yaw = 0.0 - return velocity_x, velocity_y, rate_yaw - - def command_forward_along_wall(self, side_range): - """ - Command the Crazyflie to fly forward along the wall - while controlling it's distance to it - - side_range is defined in m - velocity_x and velocity_y is defined in m/s - """ - velocity_x = self.max_forward_speed - velocity_y = 0.0 - check_distance_wall = self.value_is_close_to( - self.reference_distance_from_wall, side_range, self.ranger_value_buffer) - if not check_distance_wall: - if side_range > self.reference_distance_from_wall: - velocity_y = self.wall_following_direction_value * \ - (-1.0 * self.max_forward_speed / self.speed_redux_straight) - else: - velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_straight) - return velocity_x, velocity_y - - def command_turn_around_corner_and_adjust(self, radius, side_range): - """ - Command the Crazyflie to turn around the corner - and adjust it's distance to the corner - - radius is defined in m - side_range is defined in m - velocity_x and velocity_y is defined in m/s - """ - velocity_x = self.max_forward_speed - rate_yaw = self.wall_following_direction_value * (-1 * velocity_x / radius) - velocity_y = 0.0 - check_distance_wall = self.value_is_close_to( - self.reference_distance_from_wall, side_range, self.ranger_value_buffer) - if not check_distance_wall: - if side_range > self.reference_distance_from_wall: - velocity_y = self.wall_following_direction_value * \ - (-1.0 * self.max_forward_speed / self.speed_redux_corner) - else: - velocity_y = self.wall_following_direction_value * (self.max_forward_speed / self.speed_redux_corner) - return velocity_x, velocity_y, rate_yaw - - # state machine helper functions - def state_transition(self, new_state): - """ - Transition to a new state and reset the state timer - - new_state is defined in the StateWallFollowing enum - """ - self.state_start_time = self.time_now - return new_state - - def adjust_reference_distance_wall(self, reference_distance_wall_new): - """ - Adjust the reference distance to the wall - """ - self.reference_distance_from_wall = reference_distance_wall_new - - # Wall following State machine - def wall_follower(self, front_range, side_range, current_heading, - wall_following_direction, time_outer_loop): - """ - wall_follower is the main function of the wall following state machine. - It takes the current range measurements of the front range and side range - sensors, the current heading of the Crazyflie, the wall following direction - and the current time of the outer loop (the real time or the simulation time) - as input, and handles the state transitions and commands the Crazyflie to - to do the wall following. - - front_range and side_range is defined in m - current_heading is defined in rad - wall_following_direction is defined as WallFollowingDirection enum - time_outer_loop is defined in seconds (double) - command_velocity_x, command_velocity_ y is defined in m/s - command_rate_yaw is defined in rad/s - self.state is defined as StateWallFollowing enum - """ - - self.wall_following_direction_value = float(wall_following_direction.value) - self.time_now = time_outer_loop - - if self.first_run: - self.prev_heading = current_heading - self.around_corner_back_track = False - self.first_run = False - - # -------------- Handle state transitions ---------------- # - if self.state == self.StateWallFollowing.FORWARD: - if front_range < self.reference_distance_from_wall + self.ranger_value_buffer: - self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL) - elif self.state == self.StateWallFollowing.HOVER: - print('hover') - elif self.state == self.StateWallFollowing.TURN_TO_FIND_WALL: - # Turn until 45 degrees from wall such that the front and side range sensors - # can detect the wall - side_range_check = side_range < (self.reference_distance_from_wall / - math.cos(math.pi/4) + self.ranger_value_buffer) - front_range_check = front_range < (self.reference_distance_from_wall / - math.cos(math.pi/4) + self.ranger_value_buffer) - if side_range_check and front_range_check: - self.prev_heading = current_heading - # Calculate the angle to the wall - self.wall_angle = self.wall_following_direction_value * \ - (math.pi/2 - math.atan(front_range / side_range) + self.angle_value_buffer) - self.state = self.state_transition(self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL) - # If went too far in heading and lost the wall, go to find corner. - if side_range < self.reference_distance_from_wall + self.ranger_value_buffer and \ - front_range > self.reference_distance_from_wall + self.range_threshold_lost: - self.around_corner_back_track = False - self.prev_heading = current_heading - self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER) - elif self.state == self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL: - align_wall_check = self.value_is_close_to( - self.wrap_to_pi(current_heading - self.prev_heading), self.wall_angle, self.angle_value_buffer) - if align_wall_check: - self.state = self.state_transition(self.StateWallFollowing.FORWARD_ALONG_WALL) - elif self.state == self.StateWallFollowing.FORWARD_ALONG_WALL: - # If side range is out of reach, - # end of the wall is reached - if side_range > self.reference_distance_from_wall + self.range_threshold_lost: - self.state = self.state_transition(self.StateWallFollowing.FIND_CORNER) - # If front range is small - # then corner is reached - if front_range < self.reference_distance_from_wall + self.ranger_value_buffer: - self.prev_heading = current_heading - self.state = self.state_transition(self.StateWallFollowing.ROTATE_IN_CORNER) - elif self.state == self.StateWallFollowing.ROTATE_AROUND_WALL: - if front_range < self.reference_distance_from_wall + self.ranger_value_buffer: - self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL) - elif self.state == self.StateWallFollowing.ROTATE_IN_CORNER: - check_heading_corner = self.value_is_close_to( - math.fabs(self.wrap_to_pi(current_heading-self.prev_heading)), - self.in_corner_angle, self.angle_value_buffer) - if check_heading_corner: - self.state = self.state_transition(self.StateWallFollowing.TURN_TO_FIND_WALL) - elif self.state == self.StateWallFollowing.FIND_CORNER: - if side_range <= self.reference_distance_from_wall: - self.state = self.state_transition(self.StateWallFollowing.ROTATE_AROUND_WALL) - else: - self.state = self.state_transition(self.StateWallFollowing.HOVER) - - # -------------- Handle state actions ---------------- # - command_velocity_x_temp = 0.0 - command_velocity_y_temp = 0.0 - command_angle_rate_temp = 0.0 - - if self.state == self.StateWallFollowing.FORWARD: - command_velocity_x_temp = self.max_forward_speed - command_velocity_y_temp = 0.0 - command_angle_rate_temp = 0.0 - elif self.state == self.StateWallFollowing.HOVER: - command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() - elif self.state == self.StateWallFollowing.TURN_TO_FIND_WALL: - command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) - command_velocity_y_temp = 0.0 - elif self.state == self.StateWallFollowing.TURN_TO_ALIGN_TO_WALL: - if self.time_now - self.state_start_time < self.wait_for_measurement_seconds: - command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() - else: - command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) - command_velocity_y_temp = 0.0 - elif self.state == self.StateWallFollowing.FORWARD_ALONG_WALL: - command_velocity_x_temp, command_velocity_y_temp = self.command_forward_along_wall(side_range) - command_angle_rate_temp = 0.0 - elif self.state == self.StateWallFollowing.ROTATE_AROUND_WALL: - # If first time around corner - # first try to find the wall again - # if side range is larger than preffered distance from wall - if side_range > self.reference_distance_from_wall + self.range_threshold_lost: - # check if scanning already occured - if self.wrap_to_pi(math.fabs(current_heading - self.prev_heading)) > \ - self.in_corner_angle: - self.around_corner_back_track = True - # turn and adjust distance to corner from that point - if self.around_corner_back_track: - # rotate back if it already went into one direction - command_velocity_y_temp, command_angle_rate_temp = self.command_turn( - -1 * self.max_turn_rate) - command_velocity_x_temp = 0.0 - else: - command_velocity_y_temp, command_angle_rate_temp = self.command_turn( - self.max_turn_rate) - command_velocity_x_temp = 0.0 - else: - # continue to turn around corner - self.prev_heading = current_heading - self.around_corner_back_track = False - command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = \ - self.command_turn_around_corner_and_adjust( - self.reference_distance_from_wall, side_range) - elif self.state == self.StateWallFollowing.ROTATE_IN_CORNER: - command_velocity_x_temp, command_angle_rate_temp = self.command_turn(self.max_turn_rate) - command_velocity_y_temp = 0.0 - elif self.state == self.StateWallFollowing.FIND_CORNER: - command_velocity_y_temp, command_angle_rate_temp = self.command_align_corner( - -1 * self.max_turn_rate, side_range, self.reference_distance_from_wall) - command_velocity_x_temp = 0.0 - else: - # state does not exist, so hover! - command_velocity_x_temp, command_velocity_y_temp, command_angle_rate_temp = self.command_hover() - - command_velocity_x = command_velocity_x_temp - command_velocity_y = command_velocity_y_temp - command_yaw_rate = command_angle_rate_temp - - return command_velocity_x, command_velocity_y, command_yaw_rate, self.state diff --git a/examples/positioning/flowsequence_sync.py b/examples/positioning/flowsequence_sync.py deleted file mode 100644 index 4f62db71b..000000000 --- a/examples/positioning/flowsequence_sync.py +++ /dev/null @@ -1,86 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2016 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, see . -""" -Simple example that connects to the crazyflie at `URI` and runs a figure 8 -sequence. This script requires some kind of location system, it has been -tested with the flow deck and the lighthouse positioning system. - -Change the URI variable to your Crazyflie configuration. -""" -import logging -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper -from cflib.utils.reset_estimator import reset_estimator - -URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Only output errors from the logging framework -logging.basicConfig(level=logging.ERROR) - - -if __name__ == '__main__': - # Initialize the low-level drivers - cflib.crtp.init_drivers() - - with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - cf = scf.cf - - reset_estimator(scf) - time.sleep(1) - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - for y in range(10): - cf.commander.send_hover_setpoint(0, 0, 0, y / 25) - time.sleep(0.1) - - for _ in range(20): - cf.commander.send_hover_setpoint(0, 0, 0, 0.4) - time.sleep(0.1) - - for _ in range(50): - cf.commander.send_hover_setpoint(0.5, 0, 36 * 2, 0.4) - time.sleep(0.1) - - for _ in range(50): - cf.commander.send_hover_setpoint(0.5, 0, -36 * 2, 0.4) - time.sleep(0.1) - - for _ in range(20): - cf.commander.send_hover_setpoint(0, 0, 0, 0.4) - time.sleep(0.1) - - for y in range(10): - cf.commander.send_hover_setpoint(0, 0, 0, (10 - y) / 25) - time.sleep(0.1) - - cf.commander.send_stop_setpoint() - # Hand control over to the high level commander to avoid timeout and locking of the Crazyflie - cf.commander.send_notify_setpoint_stop() diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py deleted file mode 100644 index 79ff83634..000000000 --- a/examples/positioning/initial_position.py +++ /dev/null @@ -1,112 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 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, see . -""" -Simple example that connects to one crazyflie, sets the initial position/yaw -and flies a trajectory. - -The initial pose (x, y, z, yaw) is configured in a number of variables and -the trajectory is flown relative to this position, using the initial yaw. - -This example is intended to work with any absolute positioning system. -It aims at documenting how to take off with the Crazyflie in an orientation -that is different from the standard positive X orientation and how to set the -initial position of the kalman estimator. -""" -import math -import time - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.utils import uri_helper -from cflib.utils.reset_estimator import reset_estimator - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - -# Change the sequence according to your setup -# x y z -sequence = [ - (0, 0, 0.7), - (-0.7, 0, 0.7), - (0, 0, 0.7), - (0, 0, 0.2), -] - - -def set_initial_position(scf, x, y, z, yaw_deg): - scf.cf.param.set_value('kalman.initialX', x) - scf.cf.param.set_value('kalman.initialY', y) - scf.cf.param.set_value('kalman.initialZ', z) - - yaw_radians = math.radians(yaw_deg) - scf.cf.param.set_value('kalman.initialYaw', yaw_radians) - - -def run_sequence(scf, sequence, base_x, base_y, base_z, yaw): - cf = scf.cf - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - for position in sequence: - print('Setting position {}'.format(position)) - - x = position[0] + base_x - y = position[1] + base_y - z = position[2] + base_z - - for i in range(50): - cf.commander.send_position_setpoint(x, y, z, yaw) - time.sleep(0.1) - - cf.commander.send_stop_setpoint() - # Hand control over to the high level commander to avoid timeout and locking of the Crazyflie - cf.commander.send_notify_setpoint_stop() - - # 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() - - # Set these to the position and yaw based on how your Crazyflie is placed - # on the floor - initial_x = 1.0 - initial_y = 1.0 - initial_z = 0.0 - initial_yaw = 90 # In degrees - # 0: positive X direction - # 90: positive Y direction - # 180: negative X direction - # 270: negative Y direction - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - set_initial_position(scf, initial_x, initial_y, initial_z, initial_yaw) - reset_estimator(scf) - run_sequence(scf, sequence, - initial_x, initial_y, initial_z, initial_yaw) diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py deleted file mode 100644 index 4de1d7f02..000000000 --- a/examples/positioning/matrix_light_printer.py +++ /dev/null @@ -1,118 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2018 Bitcraze AB -# -# 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, see . -""" -This script implements a simple matrix light printer to be used with a -camera with open shutter in a dark room. - -It requires a Crazyflie capable of controlling its position and with -a Led ring attached to it. A piece of sticky paper can be attached on -the led ring to orient the ring light toward the front. - -To control it position, Crazyflie requires an absolute positioning -system such as the Lighthouse. -""" -import time - -import matplotlib.pyplot as plt - -import cflib.crtp -from cflib.crazyflie import Crazyflie -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie -from cflib.positioning.position_hl_commander import PositionHlCommander -from cflib.utils import uri_helper - -# URI to the Crazyflie to connect to -uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') - - -class ImageDef: - def __init__(self, file_name): - self._image = plt.imread(file_name) - - self.x_pixels = self._image.shape[1] - self.y_pixels = self._image.shape[0] - - width = 1.0 - height = self.y_pixels * width / self.x_pixels - - self.x_start = -width / 2.0 + 0.5 - self.y_start = 0.7 - - self.x_step = width / self.x_pixels - self.y_step = height / self.y_pixels - - def get_color(self, x_index, y_index): - rgba = self._image[self.y_pixels - y_index - 1, x_index] - rgb = [int(rgba[0] * 90), int(rgba[1] * 90), int(rgba[2] * 90)] - return rgb - - -BLACK = [0, 0, 0] - - -def set_led_ring_solid(cf, rgb): - cf.param.set_value('ring.effect', 7) - print(rgb[0], rgb[1], rgb[2]) - cf.param.set_value('ring.solidRed', rgb[0]) - cf.param.set_value('ring.solidGreen', rgb[1]) - cf.param.set_value('ring.solidBlue', rgb[2]) - - -def matrix_print(cf, pc, image_def): - set_led_ring_solid(cf, BLACK) - time.sleep(3) - - for y_index in range(image_def.y_pixels): - y = image_def.y_start + image_def.y_step * y_index - - pc.go_to(0, image_def.x_start - 0.1, y) - time.sleep(1.0) - - scan_range = range(image_def.x_pixels) - - for x_index in scan_range: - x = image_def.x_start + image_def.x_step * x_index - - color = image_def.get_color(x_index, y_index) - - print(x, y, color) - - pc.go_to(0, x, y) - set_led_ring_solid(cf, color) - - set_led_ring_solid(cf, BLACK) - - print('---') - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - image_def = ImageDef('monalisa.png') - - with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - with PositionHlCommander(scf, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: - matrix_print(scf.cf, pc, image_def) diff --git a/examples/positioning/monalisa.png b/examples/positioning/monalisa.png deleted file mode 100644 index f39be376ec492505861b2e6b2d93d3b31fa54e17..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4886 zcmY*dcQ_o*)?QYLUP7#HSCFWyvmx5*y+jSsMU=&^7Flg|RxiVGtqiFsuZM5qyPYbLR}4RaI=cs7~~ew&CI$J z!+W#fAq`X&0ksn>J2x8=4>dC+06=#8ui*i*azHl#GiM`Hl&Q8B%nsouXk(ACbrAG( z^SD6+05X2C8`sSNWy9g;=IV}w`N=~5LBMYOzhNN=$3GC1i!8)cTaQBt;pM;~E+{4l zg*+tX;NXz)vUh|Tz*YW}ezTK>JVl{AU_wH^zP^IKqJju7Cm~^JX=x#-h>(bgzzsqG z>FyNINfQ50o>)o#QXBjV;0(B@2Q475eY^cbzC_$NyDwNB*a+ zn+Apco(KsGLWTazeUmElHwx4Ba(1{;{>%SRSmqz(|BL-cM@Hzc`2Q*9-<|#wy=nC! zsf^Hnuk9hJRzHs!06=4`4p%TTB@xIRO?bTJIXH=44LtkVHT>BFzH1X}Ku6qCLSSZ3 zvk+s)p#Wf3yAJtHuK=iP9DmOzgsJW#rv=hbgP^WF>E*5VF=8X&E!@oE!O~go6H9Z^ zfrB3O;Tigo_wH;(^6^H{MPvuRNP8aLd0yp5{rw>Wv+o&Ir}*tU0wXDg3|*_0=~W%o zegc~vcy%(PzRFgkWRN*hBjddHsA9-C*Si#kqpo+k@x`f!-*9JWD_3l#e@hE;mg};0 zA@)Q%_I6nH#D3tt*7trJM>$oMS|u^Z=gWJ8N0yU@EX{4yuT8-`x>85RT#Q`;_TCtpuKksBhJv^@2X(}QAI3hAMCPrW$}{bMRk&!S!(AfY2D{d3J;B_x6k!=-~8E1vTC21@6%6^ zU7(6IPFq1lc<52n61>fk9gA*2s8~m@gv{)d6KP|*M%R0YhhVvYF&b`6(mJ<&Kc|A5 z)L8rUot8+;9Hbv`^E#07FdCAj{9lnQpodNpD$&(ZFjVf%b$Q$YI5-5e#IY@9FM3T{W_41SLDe=lx57-%{m3UZnv z#g8MHp{G9jVQfZhIvD^p!&xYSi9mw>EMvGqvIIgrEBuVFBmK^MKdvUlK7@a|tZMZ> z{u*Mtrk5Nwx~Vunk32D7{`13}D$fLB{n1^@`Yr_y7j*UQ_SH7KFC8ipE8N24D@oKf z%)o)gRFQ5oli-DBoxMKJ#4ux`bwHl-3mgOQp>en`YE^c=NROTYAREKJB?+^C#QiwQ zAY7pCh<)Yc>K}Qz-`SlV-h1-FzEJNTY2Cw+*_OYRbo>hvcftsTQNhjIw@J;$E*Zl=_E#s>8UC0m1y zo{}3s=pDo}kkKLtnKve!hGsQJ(Q}Dyx6miXr9|pBcbk{bpfg!D>6ac2sN^Mlc6Cf0 z%beXK{>Ys4lWbd(uX(Ua`5Dc@UTi}C-YK8zct?$KHNRP-?%O`BS2lldeU4Ig5_SA5 zG3nGTGvxSIcYeKTWOnl1nHDX26`%}BEJ#VA(PsM?5S{%>o?NK!_+gbQtLo_JyXQam zbiF;COu*Uhh9C-t4(Nge6?E7z3t#tWf6XKKM=wY%=jxCsy;4D;hNk5ET5&`Cf$YpQ z9(n6g>VU?BPBr7yh4al9dWBdosiEE9Q9*Ts0=;~>g+YVPd@vhTr*69 zZ+tHL7lxJMqmuHz4^n;<3ovfL$e;rYSFa$r{HvAPj$A=^kvqi;KggE&bPbAF8zO}A z!AJ@7UQMG~d^PjOj+$q?=%OKs^opH7@=x`% zu0-Ps0E=Ez4nq}LlYHa`v!^B+(m3uV%kI-$Eu zU|k8~z^~)aQnb#b7i;d@kE;|oQqg9$MyBmeW9tgCJ%`$I_MJtCT5!M+tL}!da`K7K zJxJ@Rx!S$*)wM9s65=iI<}f$AqoFerht9!J{%6ZZ)fIE{>1D6Jl=^1j2wIHHU?s`ryG63(wV0+&e29t?8nJ%-t zyR3m-77-ZCZ?G7Vbv8~M&f`arHKjU)4k++wMimlGBx)M#>6R<|zb>qGZL%VaA(l;n znr0NaKrc{l6^#Cf_qF&oSFU%P-7@Lkl zowZoKbTSWtPp`hzQw`XdlD{a3O}2GOS2XX)a?>N~P1yeI>_(cw@>x33$bjwlp75sw zv%)H)nh+`~9O%P-5}C7xB~-fCR(>2ciON*W21!ohGW(tp$Fs>zRIJ~%>6#{0{TT`5 z+H=FK+P~b0!%hgc!rr_WS9Y>tbYb$)syt>&=A5bN3G}P+Ax>w86x%&pVE<_hr;*?a zZ0P6mrp)0L*kV3vn|fMTM)+z#GR*>EU6f9h>a{?M@Q4c2F47^6XkfD7xNY@?^cHaP zF*u<*&+W(gdeUOIv`n=EXYAZ-)EC6}Ap zM(&~B__VgXqp-S~t<}~x=yomkqe=m7;R>EC<%s>)Rwnaw<57*WjZBs~FS6?jzn89| zf1;Uj3&nc$Sng2$Z?JpEHg_=UeZJp;NysLMh(5U?S6*#dg(a4tG#mGc- zOH)+Jpq2@hx{LCE z{9r%{F#0{IpBE+TH|mNJrgvE^k@Uv~nqH)IYXqQV+3NB~EN`9dd+6nkFt-?Mh>Yd$ zlUdOjg3#dw#j@Y}>O(|8?n*jk784jbFVMK;58O7B#Oo?4Y{8lXXrRe}aQ3K_O!So* zr!(GB*PEK}J#gD9HI&q29%DZ*40Lr&zNmhBA&B+r zd#WQw8V4}dw%G${y9o0_?tmQyM(%Uo6)g!)VjS}c`Q?Fa4$f0pRaHqkE`P$lmu~TD zu1g+c1&{AFrH}CUE8Usj$<{vUsCLXjbfY^Bo!4t<(raN*^F8RS?zLOx9qFT1Ilo5( z3e`eApuNQljk3u(Bkz|)M33Fg0FiWhytVGTsW@7@;hZSnC<0ZzI_;+OOyGXZ<4RW_ z`ThWw-aQWPi*Fp!2HCgQ(8;+zVq+waDx2b(0^~1mU3+S9p7X3t@*`{d z{c*@1!5P&0IBZ6B^^su+hy-#}hO znXlh)9ZR}ui@c4d@xPto3>M_;tC^>`@6k)eJz}=JAaGYQ@Rw-iFTR|lVS1xo78MbL z9h)3gv0nlvTl39xy^z)x+{@UvPk3HQO!(5Uc!*@&4tW2{{Y_1beSAoW z|1zE;XHXTiJ=lT#vJJSCcS==Y0Sbk{7vVKJ6+?bu;k+H1--4xsLDIjm2$78<)$bnt z`x{9SY8U-kVsz!>1uz=rBS^@Pa%T9*FUm+6DT}r&#hbVLHGUKfzV#K{Ei=J3)A;*B zR<-qc)1BjZ?$mI+lV>f%O<$znL{y>Mo|A1pzxyPEvNkQRB2?jp+Um1lrI+k4zEh_; z14W_F{2U9j3Lh&ZM>rBrHR$Y*o1nX|axJ@2XVl_=OEBk0KOAYf_AR%{MPhv9*T-dL z%%-#n$(Z@!1n_QAD1mK?+IZIpx5RLf^g4}stqXMUu?>kx0srokYEp-swAnuEp2W^~ z+{|M0&uUf#S+K%0E(R|>T6*jc6`Ju6?QP+S+?&zRE0xUl>gO_tseDxsq$@3DLU zd#-`8=QQPm)8`gqBwcQ;>sn=ex|p%sXJzBicfEEYSqxFzzEU?>ZS`kV`B9zbu_qSr z)taRq4iVT9cBL8(7f4AT2x537?==EKJ_|l z`W1CcBKRRq{RBB9Fl>P!Zo($x*7-M7FaOym7CgD1lW|{~^Ozr?KGgmE%B)00&KVa8 z1v#waAK~BW=U})#b6ud2W$KD^b*pzgUXIJ-0nU-dI4~>eoJOd`ng;DW z81d`MOnN5N>nSgeMCxDon{IEnX(sF|fh5uhMC)Vx2UH5d1o##hU*_e{fa`QJ?z>Wg z@9GEfL3Rr(oVt=V#Z3*M09p7WIXQ2fib9iF6$ekzr{Em^*#<^l0@?m4imM0SB_$y? z_-eu;B;POVMD|w{f3iX+RCyQl5D^M&iDdUx&L&@27WzEEoT$RB z4kK*~T5SoDUO8Ooz}UN$87(`3DFZ=LiaLW0QC|r;KLdR}J0C zJ;(a9-ILH9brgV%@)VX;XL%AFq%GK9|LP4eRfBXIK;|hy+G-zf;o`L}3J!}i=RjYc zRp1xD{d=x_nuC@EyR_!L_L>2U zu~GMe7%;@Xn~Sa~%2(^z;S^O4QDVD}+b 2 and \ - pk.data[0] & 0xf3 == 0xf3 and pk.data[1] == 0x01: - # append rssi data - temp.append(pk.data[2]) - - ack_rate = count / TRY - if len(temp) > 0: - rssi_avg = np.mean(temp) - std = np.std(temp) - else: - rssi_avg = np.nan - std = np.nan - - rssi.append(rssi_avg) - ack.append(ack_rate) - rssi_std.append(std) - - print('Channel', channel, 'ack_rate:', ack_rate, - 'rssi average:', rssi_avg, 'rssi std:', std) - -# change channel back to default -for x in range(50): - radio.send_packet((0xff, 0x03, SET_RADIO_CHANNEL, init_channel)) - -# divide each std by 2 for plotting convenience -rssi_std = [x / 2 for x in rssi_std] -rssi_std = np.array(rssi_std) -rssi = np.array(rssi) -ack = np.array(ack) - -rssi_rank = [] -ack_rank = [] - -# suggestion for rssi -order = rssi.argsort() -ranks = order.argsort() -for x in range(int(125 * Fraction)): - for y in range(126): - if ranks[y] == x: - rssi_rank.append(y) - -# suggestion for ack -order = ack.argsort() -ranks = order.argsort() -for x in range(126, 126 - int(125 * Fraction) - 1, -1): - for y in range(126): - if ranks[y] == x: - ack_rank.append(y) - -rssi_set = set(rssi_rank[0:int(125 * Fraction)]) -ack_set = set(ack_rank[0:int(125 * Fraction)]) -final_rank = rssi_set.intersection(ack_rank) -print('\nSuggested Channels:') -for x in final_rank: - print('\t', x) - -# graph 1 for ack -x = np.arange(0, 126, 1) -fig, ax1 = plt.subplots() -ax1.axis([0, 125, 0, 1.25]) -ax1.plot(x, ack, 'b') -ax1.set_xlabel('Channel') -ax1.set_ylabel('Ack Rate', color='b') -for tl in ax1.get_yticklabels(): - tl.set_color('b') - -# graph 2 for rssi & rssi_std -ax2 = ax1.twinx() -ax2.grid(True) -ax2.errorbar(x, rssi, yerr=rssi_std, fmt='r-') -ax2.fill_between(x, rssi + rssi_std, rssi - rssi_std, - facecolor='orange', edgecolor='k') -ax2.axis([0, 125, 0, 90]) -plt.ylabel('RSSI Average', color='r') -for tl in ax2.get_yticklabels(): - tl.set_color('r') -points = np.ones(100) -for x in final_rank: - ax2.plot((x, x), (0, 100), linestyle='-', - color='cornflowerblue', linewidth=1) - -plt.show() diff --git a/examples/radio/scan.py b/examples/radio/scan.py deleted file mode 100644 index 055feab3e..000000000 --- a/examples/radio/scan.py +++ /dev/null @@ -1,37 +0,0 @@ -# -*- 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, see . -""" -Simple example that scans for available Crazyflies with a certain address and lists them. -""" -import cflib.crtp - -# Initiate the low level drivers -cflib.crtp.init_drivers() - -print('Scanning interfaces for Crazyflies...') -available = cflib.crtp.scan_interfaces(address=int('E7E7E7E7E7', 16) - ) -print('Crazyflies found:') -for i in available: - print(i[0]) diff --git a/examples/swarm/asynchronized_swarm.py b/examples/swarm/asynchronized_swarm.py deleted file mode 100644 index 3d9b3d591..000000000 --- a/examples/swarm/asynchronized_swarm.py +++ /dev/null @@ -1,159 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017-2018 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, see . -""" -Simple example of an asynchronized swarm choreography using the motion -commander. - -The swarm takes off and flies an asynchronous choreography before landing. -All movements are relative to the starting position. -During the flight, the position of each Crazyflie is printed. - -This example is intended to work with any kind of location system, it has -been tested with the flow deck v2 and the lighthouse positioning system. -Not using an absolute positioning system makes every Crazyflie start its -positioning printing with (0,0,0) as its initial position. - -This example aims at documenting how to use the motion commander together -with the Swarm class to achieve asynchronized sequences. -""" -import time - -import cflib.crtp -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm -from cflib.positioning.motion_commander import MotionCommander - -# Change uris according to your setup -# URIs in a swarm using the same radio must also be on the same channel -URI1 = 'radio://0/80/2M/E7E7E7E7E7' -URI2 = 'radio://0/80/2M/E7E7E7E7E8' - -DEFAULT_HEIGHT = 0.5 -DEFAULT_VELOCITY = 0.2 -pos1 = [0, 0, 0] -pos2 = [0, 0, 0] - -# List of URIs -uris = { - URI1, - URI2, -} - - -def wait_for_param_download(scf): - while not scf.cf.param.is_updated: - time.sleep(1.0) - print('Parameters downloaded for', scf.cf.link_uri) - - -def arm(scf): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -def position_callback(uri, data): - if uri == URI1: - pos1[0] = data['stateEstimate.x'] - pos1[1] = data['stateEstimate.y'] - pos1[2] = data['stateEstimate.z'] - print(f'Uri1 position: x={pos1[0]}, y={pos1[1]}, z={pos1[2]}') - elif uri == URI2: - pos2[0] = data['stateEstimate.x'] - pos2[1] = data['stateEstimate.y'] - pos2[2] = data['stateEstimate.z'] - print(f'Uri2 position: x={pos2[0]}, y={pos2[1]}, z={pos2[2]}') - - -def start_position_printing(scf): - log_conf1 = LogConfig(name='Position', period_in_ms=500) - log_conf1.add_variable('stateEstimate.x', 'float') - log_conf1.add_variable('stateEstimate.y', 'float') - log_conf1.add_variable('stateEstimate.z', 'float') - scf.cf.log.add_config(log_conf1) - log_conf1.data_received_cb.add_callback(lambda _timestamp, data, _logconf: position_callback(scf.cf.link_uri, data)) - log_conf1.start() - - -def async_flight(scf): - with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: - time.sleep(1) - - start_time = time.time() - end_time = time.time() + 12 - - while time.time() < end_time: - - if scf.__dict__['_link_uri'] == URI1: - if time.time() - start_time < 5: - mc.start_up(DEFAULT_VELOCITY) - elif time.time() - start_time < 7: - mc.stop() - elif time.time() - start_time < 12: - mc.start_down(DEFAULT_VELOCITY) - else: - mc.stop() - - elif scf.__dict__['_link_uri'] == URI2: - if time.time() - start_time < 2: - mc.start_left(DEFAULT_VELOCITY) - elif time.time() - start_time < 4: - mc.start_right(DEFAULT_VELOCITY) - elif time.time() - start_time < 6: - mc.start_left(DEFAULT_VELOCITY) - elif time.time() - start_time < 8: - mc.start_right(DEFAULT_VELOCITY) - elif time.time() - start_time < 10: - mc.start_left(DEFAULT_VELOCITY) - elif time.time() - start_time < 12: - mc.start_right(DEFAULT_VELOCITY) - else: - mc.stop() - - time.sleep(0.01) - mc.land() - - -if __name__ == '__main__': - # logging.basicConfig(level=logging.DEBUG) - cflib.crtp.init_drivers() - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - - swarm.reset_estimators() - - print('Waiting for parameters to be downloaded...') - swarm.parallel_safe(wait_for_param_download) - - time.sleep(1) - - swarm.parallel_safe(start_position_printing) - time.sleep(0.1) - - swarm.parallel_safe(arm) - time.sleep(0.5) - - swarm.parallel_safe(async_flight) - time.sleep(1) diff --git a/examples/swarm/christmas_tree.py b/examples/swarm/christmas_tree.py deleted file mode 100644 index 7404defcd..000000000 --- a/examples/swarm/christmas_tree.py +++ /dev/null @@ -1,185 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2025 Bitcraze AB -# -# 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, see . -""" -Script for flying a swarm of 8 Crazyflies performing a coordinated spiral choreography -resembling a Christmas tree outline in 3D space. Each drone takes off to a different -height, flies in spiraling circular layers, and changes radius as it rises and descends, -forming the visual structure of a cone when viewed from outside. - -The script is using the high level commanded and has been tested with 3 Crazyradios 2.0 -and the Lighthouse positioning system. -""" -import math -import time - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm -from cflib.crazyflie.syncCrazyflie import SyncCrazyflie - -uri1 = 'radio://0/30/2M/E7E7E7E701' -uri2 = 'radio://0/30/2M/E7E7E7E702' -uri3 = 'radio://0/30/2M/E7E7E7E703' - -uri4 = 'radio://1/55/2M/E7E7E7E704' -uri5 = 'radio://1/55/2M/E7E7E7E705' -uri6 = 'radio://1/55/2M/E7E7E7E706' - -uri7 = 'radio://2/70/2M/E7E7E7E707' -uri8 = 'radio://2/70/2M/E7E7E7E708' - -uris = [ - uri1, - uri2, - uri3, - uri4, - uri5, - uri6, - uri7, - uri8, - # Add more URIs if you want more copters in the swarm -] - - -def arm(scf: SyncCrazyflie): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -# center of the spiral -x0 = 0 -y0 = 0 -z0 = 0.5 - -x_offset = 0.4 # Vertical distance between 2 layers -z_offset = 0.5 # Radius difference between 2 layers - -starting_x = { - uri1: x0 + x_offset, - uri2: x0 + 2*x_offset, - uri3: x0 + 3*x_offset, - uri4: x0 + 4*x_offset, - uri5: x0 - x_offset, - uri6: x0 - 2*x_offset, - uri7: x0 - 3*x_offset, - uri8: x0 - 4*x_offset, -} - -starting_z = { - uri1: z0 + 3*z_offset, - uri2: z0 + 2*z_offset, - uri3: z0 + z_offset, - uri4: z0, - uri5: z0 + 3*z_offset, - uri6: z0 + 2*z_offset, - uri7: z0 + z_offset, - uri8: z0, -} - -starting_yaw = { - uri1: math.pi/2, - uri2: -math.pi/2, - uri3: math.pi/2, - uri4: -math.pi/2, - uri5: -math.pi/2, - uri6: math.pi/2, - uri7: -math.pi/2, - uri8: math.pi/2, - -} - -rotate_clockwise = { - uri1: False, - uri2: True, - uri3: False, - uri4: True, - uri5: False, - uri6: True, - uri7: False, - uri8: True, -} - -takeoff_dur = { - uri: value / 0.4 - for uri, value in starting_z.items() -} - - -def x_from_z(z): - cone_width = 4 # m - cone_height = 5 # m - """ - Returns the radius of the tree with a given z. - """ - return cone_width/2 - (cone_width/cone_height) * z - - -def run_shared_sequence(scf: SyncCrazyflie): - circle_duration = 5 # Duration of a full-circle - commander = scf.cf.high_level_commander - uri = scf._link_uri - - commander.takeoff(starting_z[uri], takeoff_dur[uri]) - time.sleep(max(takeoff_dur.values())+1) - - # Go to the starting position - commander.go_to(starting_x[uri], y0, starting_z[uri], starting_yaw[uri], 4) - time.sleep(5) - - # Full circle with ascent=0 - commander.spiral(2*math.pi, abs(starting_x[uri]), abs(starting_x[uri]), - ascent=0, duration_s=circle_duration, sideways=False, clockwise=rotate_clockwise[uri]) - time.sleep(circle_duration+1) - - # Half circle with ascent=-0.5m - commander.spiral(math.pi, abs(starting_x[uri]), x_from_z(starting_z[uri]-0.5*z_offset), - ascent=-0.5*z_offset, duration_s=0.5*circle_duration, sideways=False, - clockwise=rotate_clockwise[uri]) - time.sleep(0.5*circle_duration+0.5) - - # Full circle with ascent=+1.0m - commander.spiral(2*math.pi, x_from_z(starting_z[uri]-0.5*z_offset), x_from_z(starting_z[uri]+0.5*z_offset), - ascent=z_offset, duration_s=circle_duration, sideways=False, - clockwise=rotate_clockwise[uri]) - time.sleep(circle_duration+1) - - # Half circle with ascent=-0.5m - commander.spiral(math.pi, x_from_z(starting_z[uri]+0.5*z_offset), x_from_z(starting_z[uri]), - ascent=-0.5*z_offset, duration_s=0.5*circle_duration, sideways=False, - clockwise=rotate_clockwise[uri]) - time.sleep(0.5*circle_duration+1) - - commander.land(0, takeoff_dur[uri]) - time.sleep(max(takeoff_dur.values())+3) - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - # swarm.reset_estimators() - time.sleep(0.5) - print('arming...') - swarm.parallel_safe(arm) - print('starting sequence...') - swarm.parallel_safe(run_shared_sequence) - time.sleep(1) diff --git a/examples/swarm/hl_commander_swarm.py b/examples/swarm/hl_commander_swarm.py deleted file mode 100644 index a3f4ddf27..000000000 --- a/examples/swarm/hl_commander_swarm.py +++ /dev/null @@ -1,94 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Simple example of a swarm using the High level commander. - -The swarm takes off and flies a synchronous square shape before landing. -The trajectories are relative to the starting positions and the Crazyflies can -be at any position on the floor when the script is started. - -This example is intended to work with any absolute positioning system. -It aims at documenting how to use the High Level Commander together with -the Swarm class. -""" -import time - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm - - -def activate_mellinger_controller(scf, use_mellinger): - controller = 1 - if use_mellinger: - controller = 2 - scf.cf.param.set_value('stabilizer.controller', controller) - - -def arm(scf): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -def run_shared_sequence(scf): - activate_mellinger_controller(scf, False) - - box_size = 1 - flight_time = 2 - - commander = scf.cf.high_level_commander - - commander.takeoff(1.0, 2.0) - time.sleep(3) - - commander.go_to(box_size, 0, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(0, box_size, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(-box_size, 0, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.go_to(0, -box_size, 0, 0, flight_time, relative=True) - time.sleep(flight_time) - - commander.land(0.0, 2.0) - time.sleep(2) - - commander.stop() - - -uris = { - 'radio://0/30/2M/E7E7E7E711', - 'radio://0/30/2M/E7E7E7E712', - # Add more URIs if you want more copters in the swarm - # URIs in a swarm using the same radio must also be on the same channel -} - -if __name__ == '__main__': - cflib.crtp.init_drivers() - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - swarm.reset_estimators() - swarm.parallel_safe(arm) - swarm.parallel_safe(run_shared_sequence) diff --git a/examples/swarm/leader_follower.py b/examples/swarm/leader_follower.py deleted file mode 100644 index 552133af3..000000000 --- a/examples/swarm/leader_follower.py +++ /dev/null @@ -1,207 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017-2018 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, see . -''' -Example of a swarm sharing data and performing a leader-follower scenario -using the motion commander. - -The swarm takes off and the drones hover until the follower's local coordinate -system is aligned with the global one. Then, the leader performs its own -trajectory based on commands from the motion commander. The follower is -constantly commanded to keep a defined distance from the leader, meaning that -it is moving towards the leader when their current distance is larger than the -defined one and away from the leader in the opposite scenario. -All movements refer to the local coordinate system of each drone. - -This example is intended to work with an absolute positioning system, it has -been tested with the lighthouse positioning system. - -This example aims at documenting how to use the collected data to define new -trajectories in real-time. It also indicates how to use the swarm class to -feed the Crazyflies completely different asynchronized trajectories in parallel. -''' -import math -import time - -import cflib.crtp -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm -from cflib.positioning.motion_commander import MotionCommander - -# Change uris according to your setup -# URIs in a swarm using the same radio must also be on the same channel -URI1 = 'radio://0/80/2M/E7E7E7E7E7' # Follower -URI2 = 'radio://0/80/2M/E7E7E7E7E8' # Leader - - -DEFAULT_HEIGHT = 0.75 -DEFAULT_VELOCITY = 0.5 -x1 = [0] -y1 = [0] -z1 = [0] -x2 = [0] -y2 = [0] -z2 = [0] -yaw1 = [0] -yaw2 = [0] -d = 0 - -# List of URIs -uris = { - URI1, - URI2, -} - - -def wait_for_param_download(scf): - while not scf.cf.param.is_updated: - time.sleep(1.0) - print('Parameters downloaded for', scf.cf.link_uri) - - -def arm(scf): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -def pos_to_vel(x1, y1, x2, y2, dist): - ''' - This function takes two points on the x-y plane and outputs - two components of the velocity vector: one along the x-axis - and one along the y-axis. The combined vector represents the - total velocity, pointing from point 1 to point 2, with a - magnitude equal to the DEFAULT_VELOCITY. These 2 velocity - vectors are meant to be used by the motion commander. - The distance between them is taken as an argument because it - is constanlty calculated by position_callback(). - ''' - if dist == 0: - Vx = 0 - Vy = 0 - else: - Vx = DEFAULT_VELOCITY * (x2-x1)/dist - Vy = DEFAULT_VELOCITY * (y2-y1)/dist - return Vx, Vy - - -def position_callback(uri, data): - global d - if uri == URI1: # Follower - x1.append(data['stateEstimate.x']) - y1.append(data['stateEstimate.y']) - z1.append(data['stateEstimate.z']) - yaw1.append(data['stateEstimate.yaw']) - elif uri == URI2: # Leader - x2.append(data['stateEstimate.x']) - y2.append(data['stateEstimate.y']) - z2.append(data['stateEstimate.z']) - yaw2.append(data['stateEstimate.yaw']) - - d = math.sqrt(pow((x1[-1]-x2[-1]), 2)+pow((y1[-1]-y2[-1]), 2)) - - -def start_position_printing(scf): - log_conf1 = LogConfig(name='Position', period_in_ms=10) - log_conf1.add_variable('stateEstimate.x', 'float') - log_conf1.add_variable('stateEstimate.y', 'float') - log_conf1.add_variable('stateEstimate.z', 'float') - log_conf1.add_variable('stateEstimate.yaw', 'float') - scf.cf.log.add_config(log_conf1) - log_conf1.data_received_cb.add_callback(lambda _timestamp, data, _logconf: position_callback(scf.cf.link_uri, data)) - log_conf1.start() - - -def leader_follower(scf): - r_min = 0.8 # The minimum distance between the 2 drones - r_max = 1.0 # The maximum distance between the 2 drones - with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: - - # The follower turns until it is aligned with the global coordinate system - while abs(yaw1[-1]) > 2: - if scf.__dict__['_link_uri'] == URI1: # Follower - if yaw1[-1] > 0: - mc.start_turn_right(36 if abs(yaw1[-1]) > 15 else 9) - elif yaw1[-1] < 0: - mc.start_turn_left(36 if abs(yaw1[-1]) > 15 else 9) - - elif scf.__dict__['_link_uri'] == URI2: # Leader - mc.stop() - time.sleep(0.005) - - time.sleep(0.5) - - start_time = time.time() - # Define the flight time after the follower is aligned - end_time = time.time() + 20 - - while time.time() < end_time: - - if scf.__dict__['_link_uri'] == URI1: # Follower - if d > r_max: - cmd_vel_x, cmd_vel_y = pos_to_vel(x1[-1], y1[-1], x2[-1], y2[-1], d) - elif d >= r_min and d <= r_max: - cmd_vel_x = 0 - cmd_vel_y = 0 - elif d < r_min: - opp_cmd_vel_x, opp_cmd_vel_y = pos_to_vel(x1[-1], y1[-1], x2[-1], y2[-1], d) - cmd_vel_x = -opp_cmd_vel_x - cmd_vel_y = -opp_cmd_vel_y - - mc.start_linear_motion(cmd_vel_x, cmd_vel_y, 0) - - elif scf.__dict__['_link_uri'] == URI2: # Leader - # Define the sequence of the leader - if time.time() - start_time < 3: - mc.start_forward(DEFAULT_VELOCITY) - elif time.time() - start_time < 6: - mc.start_back(DEFAULT_VELOCITY) - elif time.time() - start_time < 20: - mc.start_circle_right(0.9, DEFAULT_VELOCITY) - else: - mc.stop() - - time.sleep(0.005) - mc.land() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - - swarm.reset_estimators() - - swarm.parallel_safe(arm) - - print('Waiting for parameters to be downloaded...') - swarm.parallel_safe(wait_for_param_download) - - time.sleep(1) - - swarm.parallel_safe(start_position_printing) - time.sleep(0.5) - - swarm.parallel_safe(leader_follower) - time.sleep(1) diff --git a/examples/swarm/swarm_sequence.py b/examples/swarm/swarm_sequence.py deleted file mode 100644 index 132dfb81c..000000000 --- a/examples/swarm/swarm_sequence.py +++ /dev/null @@ -1,253 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017-2018 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, see . -""" -Version of the AutonomousSequence.py example connecting to 10 Crazyflies. -The Crazyflies go straight up, hover a while and land but the code is fairly -generic and each Crazyflie has its own sequence of setpoints that it files -to. - -The layout of the positions: - x2 x1 x0 - -y3 10 4 - - ^ Y - | -y2 9 6 3 - | - +------> X - -y1 8 5 2 - - - -y0 7 1 - -""" -import time - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm - -# Change uris and sequences according to your setup -# URIs in a swarm using the same radio must also be on the same channel -URI1 = 'radio://0/70/2M/E7E7E7E701' -URI2 = 'radio://0/70/2M/E7E7E7E702' -URI3 = 'radio://0/70/2M/E7E7E7E703' -URI4 = 'radio://0/70/2M/E7E7E7E704' -URI5 = 'radio://0/70/2M/E7E7E7E705' -URI6 = 'radio://0/70/2M/E7E7E7E706' -URI7 = 'radio://0/70/2M/E7E7E7E707' -URI8 = 'radio://0/70/2M/E7E7E7E708' -URI9 = 'radio://0/70/2M/E7E7E7E709' -URI10 = 'radio://0/70/2M/E7E7E7E70A' - - -z0 = 0.4 -z = 1.0 - -x0 = 0.7 -x1 = 0 -x2 = -0.7 - -y0 = -1.0 -y1 = -0.4 -y2 = 0.4 -y3 = 1.0 - -# x y z time -sequence1 = [ - (x0, y0, z0, 3.0), - (x0, y0, z, 30.0), - (x0, y0, z0, 3.0), -] - -sequence2 = [ - (x0, y1, z0, 3.0), - (x0, y1, z, 30.0), - (x0, y1, z0, 3.0), -] - -sequence3 = [ - (x0, y2, z0, 3.0), - (x0, y2, z, 30.0), - (x0, y2, z0, 3.0), -] - -sequence4 = [ - (x0, y3, z0, 3.0), - (x0, y3, z, 30.0), - (x0, y3, z0, 3.0), -] - -sequence5 = [ - (x1, y1, z0, 3.0), - (x1, y1, z, 30.0), - (x1, y1, z0, 3.0), -] - -sequence6 = [ - (x1, y2, z0, 3.0), - (x1, y2, z, 30.0), - (x1, y2, z0, 3.0), -] - -sequence7 = [ - (x2, y0, z0, 3.0), - (x2, y0, z, 30.0), - (x2, y0, z0, 3.0), -] - -sequence8 = [ - (x2, y1, z0, 3.0), - (x2, y1, z, 30.0), - (x2, y1, z0, 3.0), -] - -sequence9 = [ - (x2, y2, z0, 3.0), - (x2, y2, z, 30.0), - (x2, y2, z0, 3.0), -] - -sequence10 = [ - (x2, y3, z0, 3.0), - (x2, y3, z, 30.0), - (x2, y3, z0, 3.0), -] - -seq_args = { - URI1: [sequence1], - URI2: [sequence2], - URI3: [sequence3], - URI4: [sequence4], - URI5: [sequence5], - URI6: [sequence6], - URI7: [sequence7], - URI8: [sequence8], - URI9: [sequence9], - URI10: [sequence10], -} - -# List of URIs, comment the one you do not want to fly -uris = { - URI1, - URI2, - URI3, - URI4, - URI5, - URI6, - URI7, - URI8, - URI9, - URI10 -} - - -def wait_for_param_download(scf): - while not scf.cf.param.is_updated: - time.sleep(1.0) - print('Parameters downloaded for', scf.cf.link_uri) - - -def arm(scf): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -def take_off(cf, position): - take_off_time = 1.0 - sleep_time = 0.1 - steps = int(take_off_time / sleep_time) - vz = position[2] / take_off_time - - print(vz) - - for i in range(steps): - cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) - time.sleep(sleep_time) - - -def land(cf, position): - landing_time = 1.0 - sleep_time = 0.1 - steps = int(landing_time / sleep_time) - vz = -position[2] / landing_time - - print(vz) - - for _ in range(steps): - cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) - time.sleep(sleep_time) - - cf.commander.send_stop_setpoint() - # Hand control over to the high level commander to avoid timeout and locking of the Crazyflie - cf.commander.send_notify_setpoint_stop() - - # 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) - - -def run_sequence(scf, sequence): - try: - cf = scf.cf - - take_off(cf, sequence[0]) - for position in sequence: - print('Setting position {}'.format(position)) - end_time = time.time() + position[3] - while time.time() < end_time: - cf.commander.send_position_setpoint(position[0], - position[1], - position[2], 0) - time.sleep(0.1) - land(cf, sequence[-1]) - except Exception as e: - print(e) - - -if __name__ == '__main__': - # logging.basicConfig(level=logging.DEBUG) - cflib.crtp.init_drivers() - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - # If the copters are started in their correct positions this is - # probably not needed. The Kalman filter will have time to converge - # any way since it takes a while to start them all up and connect. We - # keep the code here to illustrate how to do it. - # swarm.reset_estimators() - - # The current values of all parameters are downloaded as a part of the - # connections sequence. Since we have 10 copters this is clogging up - # communication and we have to wait for it to finish before we start - # flying. - print('Waiting for parameters to be downloaded...') - swarm.parallel(wait_for_param_download) - - swarm.parallel(arm) - - swarm.parallel(run_sequence, args_dict=seq_args) diff --git a/examples/swarm/swarm_sequence_circle.py b/examples/swarm/swarm_sequence_circle.py deleted file mode 100644 index 90ecd7bbb..000000000 --- a/examples/swarm/swarm_sequence_circle.py +++ /dev/null @@ -1,147 +0,0 @@ -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2017 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, see . -""" -A script to fly 5 Crazyflies in formation. One stays in the center and the -other four fly around it in a circle. Mainly intended to be used with the -Flow deck. -The starting positions are vital and should be oriented like this - - > - -^ + v - - < - -The distance from the center to the perimeter of the circle is around 0.5 m - -""" -import math -import time - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm - -# Change uris according to your setup -# URIs in a swarm using the same radio must also be on the same channel -URI0 = 'radio://0/70/2M/E7E7E7E7E7' -URI1 = 'radio://0/110/2M/E7E7E7E702' -URI2 = 'radio://0/94/2M/E7E7E7E7E7' -URI3 = 'radio://0/5/2M/E7E7E7E702' -URI4 = 'radio://0/110/2M/E7E7E7E703' - -# d: diameter of circle -# z: altitude -params0 = {'d': 1.0, 'z': 0.3} -params1 = {'d': 1.0, 'z': 0.3} -params2 = {'d': 0.0, 'z': 0.5} -params3 = {'d': 1.0, 'z': 0.3} -params4 = {'d': 1.0, 'z': 0.3} - - -uris = { - URI0, - URI1, - URI2, - URI3, - URI4, -} - -params = { - URI0: [params0], - URI1: [params1], - URI2: [params2], - URI3: [params3], - URI4: [params4], -} - - -def poshold(cf, t, z): - steps = t * 10 - - for r in range(steps): - cf.commander.send_hover_setpoint(0, 0, 0, z) - time.sleep(0.1) - - -def run_sequence(scf, params): - cf = scf.cf - - # Arm the Crazyflie - cf.platform.send_arming_request(True) - time.sleep(1.0) - - # Number of setpoints sent per second - fs = 4 - fsi = 1.0 / fs - - # Compensation for unknown error :-( - comp = 1.3 - - # Base altitude in meters - base = 0.15 - - d = params['d'] - z = params['z'] - - poshold(cf, 2, base) - - ramp = fs * 2 - for r in range(ramp): - cf.commander.send_hover_setpoint(0, 0, 0, base + r * (z - base) / ramp) - time.sleep(fsi) - - poshold(cf, 2, z) - - for _ in range(2): - # The time for one revolution - circle_time = 8 - - steps = circle_time * fs - for _ in range(steps): - cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time, - 0, 360.0 / circle_time, z) - time.sleep(fsi) - - poshold(cf, 2, z) - - for r in range(ramp): - cf.commander.send_hover_setpoint(0, 0, 0, - base + (ramp - r) * (z - base) / ramp) - time.sleep(fsi) - - poshold(cf, 1, base) - - cf.commander.send_stop_setpoint() - # Hand control over to the high level commander to avoid timeout and locking of the Crazyflie - cf.commander.send_notify_setpoint_stop() - - -if __name__ == '__main__': - cflib.crtp.init_drivers() - - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - swarm.reset_estimators() - swarm.parallel(run_sequence, args_dict=params) diff --git a/examples/swarm/synchronized_sequence.py b/examples/swarm/synchronized_sequence.py deleted file mode 100755 index f6483f4b4..000000000 --- a/examples/swarm/synchronized_sequence.py +++ /dev/null @@ -1,205 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2019 Bitcraze AB -# -# 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, see . -""" -Simple example of a synchronized swarm choreography using the High level -commander. - -The swarm takes off and flies a synchronous choreography before landing. -The take-of is relative to the start position but the Goto are absolute. -The sequence contains a list of commands to be executed at each step. - -This example is intended to work with any absolute positioning system. -It aims at documenting how to use the High Level Commander together with -the Swarm class to achieve synchronous sequences. -""" -import threading -import time -from collections import namedtuple -from queue import Queue - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm - -# Time for one step in second -STEP_TIME = 1 - -# Possible commands, all times are in seconds -Arm = namedtuple('Arm', []) -Takeoff = namedtuple('Takeoff', ['height', 'time']) -Land = namedtuple('Land', ['time']) -Goto = namedtuple('Goto', ['x', 'y', 'z', 'time']) -# RGB [0-255], Intensity [0.0-1.0] -Ring = namedtuple('Ring', ['r', 'g', 'b', 'intensity', 'time']) -# Reserved for the control loop, do not use in sequence -Quit = namedtuple('Quit', []) - -uris = [ - 'radio://0/10/2M/E7E7E7E701', # cf_id 0, startup position [-0.5, -0.5] - 'radio://0/10/2M/E7E7E7E702', # cf_id 1, startup position [ 0, 0] - 'radio://0/10/2M/E7E7E7E703', # cf_id 3, startup position [0.5, 0.5] - # Add more URIs if you want more copters in the swarm - # URIs in a swarm using the same radio must also be on the same channel -] - -sequence = [ - # Step, CF_id, action - (0, 0, Arm()), - (0, 1, Arm()), - (0, 2, Arm()), - - (0, 0, Takeoff(0.5, 2)), - (0, 2, Takeoff(0.5, 2)), - - (1, 1, Takeoff(1.0, 2)), - - (2, 0, Goto(-0.5, -0.5, 0.5, 1)), - (2, 2, Goto(0.5, 0.5, 0.5, 1)), - - (3, 1, Goto(0, 0, 1, 1)), - - (4, 0, Ring(255, 255, 255, 0.2, 0)), - (4, 1, Ring(255, 0, 0, 0.2, 0)), - (4, 2, Ring(255, 255, 255, 0.2, 0)), - - (5, 0, Goto(0.5, -0.5, 0.5, 2)), - (5, 2, Goto(-0.5, 0.5, 0.5, 2)), - - (7, 0, Goto(0.5, 0.5, 0.5, 2)), - (7, 2, Goto(-0.5, -0.5, 0.5, 2)), - - (9, 0, Goto(-0.5, 0.5, 0.5, 2)), - (9, 2, Goto(0.5, -0.5, 0.5, 2)), - - (11, 0, Goto(-0.5, -0.5, 0.5, 2)), - (11, 2, Goto(0.5, 0.5, 0.5, 2)), - - (13, 0, Land(2)), - (13, 1, Land(2)), - (13, 2, Land(2)), - - (15, 0, Ring(0, 0, 0, 0, 5)), - (15, 1, Ring(0, 0, 0, 0, 5)), - (15, 2, Ring(0, 0, 0, 0, 5)), -] - - -def activate_mellinger_controller(scf, use_mellinger): - controller = 1 - if use_mellinger: - controller = 2 - scf.cf.param.set_value('stabilizer.controller', str(controller)) - - -def arm(scf): - scf.cf.platform.send_arming_request(True) - time.sleep(1.0) - - -def set_ring_color(cf, r, g, b, intensity, time): - cf.param.set_value('ring.fadeTime', str(time)) - - r *= intensity - g *= intensity - b *= intensity - - color = (int(r) << 16) | (int(g) << 8) | int(b) - - cf.param.set_value('ring.fadeColor', str(color)) - - -def crazyflie_control(scf): - cf = scf.cf - control = controlQueues[uris.index(cf.link_uri)] - - activate_mellinger_controller(scf, False) - - commander = scf.cf.high_level_commander - - # Set fade to color effect and reset to Led-ring OFF - set_ring_color(cf, 0, 0, 0, 0, 0) - cf.param.set_value('ring.effect', '14') - - while True: - command = control.get() - if type(command) is Quit: - return - elif type(command) is Arm: - arm(scf) - elif type(command) is Takeoff: - commander.takeoff(command.height, command.time) - elif type(command) is Land: - commander.land(0.0, command.time) - elif type(command) is Goto: - commander.go_to(command.x, command.y, command.z, 0, command.time) - elif type(command) is Ring: - set_ring_color(cf, command.r, command.g, command.b, - command.intensity, command.time) - pass - else: - print('Warning! unknown command {} for uri {}'.format(command, - cf.uri)) - - -def control_thread(): - pointer = 0 - step = 0 - stop = False - - while not stop: - print('Step {}:'.format(step)) - while sequence[pointer][0] <= step: - cf_id = sequence[pointer][1] - command = sequence[pointer][2] - - print(' - Running: {} on {}'.format(command, cf_id)) - controlQueues[cf_id].put(command) - pointer += 1 - - if pointer >= len(sequence): - print('Reaching the end of the sequence, stopping!') - stop = True - break - - step += 1 - time.sleep(STEP_TIME) - - for ctrl in controlQueues: - ctrl.put(Quit()) - - -if __name__ == '__main__': - controlQueues = [Queue() for _ in range(len(uris))] - - cflib.crtp.init_drivers() - factory = CachedCfFactory(rw_cache='./cache') - with Swarm(uris, factory=factory) as swarm: - swarm.reset_estimators() - - print('Starting sequence!') - - threading.Thread(target=control_thread).start() - - swarm.parallel_safe(crazyflie_control) - - time.sleep(1) From e4482567b68d617f32a3a483c60505bcd75083bf Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Tue, 7 Apr 2026 12:50:59 +0200 Subject: [PATCH 2/2] Removed links to specific examples --- docs/development/wireshark.md | 4 ++-- docs/user-guides/python_api.md | 2 +- docs/user-guides/sbs_swarm_interface.md | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/development/wireshark.md b/docs/development/wireshark.md index d181e20c6..05ce220e9 100644 --- a/docs/development/wireshark.md +++ b/docs/development/wireshark.md @@ -34,14 +34,14 @@ The de facto standard network packet capture format is libpcap (pcap), which is To tell the CFLIB to generate a PCAP file of what it thinks the CRTP traffic looks like you can go: ```bash -$ CRTP_PCAP_LOG=filename.pcap python3 examples/swarm/hl_commander_swarm.py +$ CRTP_PCAP_LOG=filename.pcap python3 .py $ wireshark filename.pcap ``` and on Windows based computers in a shell window: ```bash > set CRTP_PCAP_LOG=filename.pcap -> python3 examples/swarm/hl_commander_swarm.py +> python3 .py > wireshark filename.pcap ``` To generate a PCAP file and open it with Wireshark. You can also use the text based `tshark` tool, and you can add a filter, for instance, only shoow CRTP port 8 (Highlevel setpoint): diff --git a/docs/user-guides/python_api.md b/docs/user-guides/python_api.md index a81b2da7f..0b5887525 100644 --- a/docs/user-guides/python_api.md +++ b/docs/user-guides/python_api.md @@ -14,7 +14,7 @@ If you are interested in more details look in the PyDoc in the code or: [logging](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_log/) or [parameters](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_parameters/) - [Automated documentation for Python API](https://www.bitcraze.io/documentation/repository/crazyflie-lib-python/master/api/cflib/) -- Examples: See the [example folder of the repository](https://github.com/bitcraze/crazyflie-lib-python/tree/master/examples). +- Examples: See the [example folder of the repository](https://github.com/bitcraze/crazyflie-lib-python/tree/master/examples) for basic examples, or [crazyflie-demos](https://github.com/bitcraze/crazyflie-demos) for more advanced demos. ## Structure of the library diff --git a/docs/user-guides/sbs_swarm_interface.md b/docs/user-guides/sbs_swarm_interface.md index eb94f032a..3be574e97 100644 --- a/docs/user-guides/sbs_swarm_interface.md +++ b/docs/user-guides/sbs_swarm_interface.md @@ -474,4 +474,4 @@ if __name__ == '__main__': You’re done! The full code of this tutorial can be found in the `example/step-by-step/` folder. ## What is next? -Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality, simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications! For more examples and inspiration on the Swarm functionality, you can check out the `examples/swarm/` folder of the cflib. +Now you are able to control a swarm of Crazyflies and you can experiment with different behaviors for each one of them while maintaining the functionality, simplicity of working with just one since the parallelism is handled internally and you can just focus on creating awesome applications! For more examples and inspiration on the Swarm functionality, you can check out the [crazyflie-demos](https://github.com/bitcraze/crazyflie-demos) repository.