diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index 5e09b906d..89bfedf1c 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -51,6 +51,7 @@ from .mem import Memory from .param import Param from .platformservice import PlatformService +from .supervisor import Supervisor from .toccache import TocCache from cflib.crazyflie.high_level_commander import HighLevelCommander from cflib.utils.callbacks import Caller @@ -124,6 +125,7 @@ def __init__(self, link=None, ro_cache=None, rw_cache=None): self.platform = PlatformService(self) self.appchannel = Appchannel(self) self.link_statistics = LinkStatistics(self) + self.supervisor = Supervisor(self) self.link_uri = '' diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 1d50d81d9..f032f1144 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -28,6 +28,7 @@ import collections import logging import struct +import warnings from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort @@ -58,8 +59,8 @@ class Localization(): RANGE_STREAM_REPORT = 0 RANGE_STREAM_REPORT_FP16 = 1 LPS_SHORT_LPP_PACKET = 2 - EMERGENCY_STOP = 3 - EMERGENCY_STOP_WATCHDOG = 4 + EMERGENCY_STOP = 3 # Deprecated: use supervisor.send_emergency_stop() + EMERGENCY_STOP_WATCHDOG = 4 # Deprecated: use supervisor.send_emergency_stop_watchdog() COMM_GNSS_NMEA = 6 COMM_GNSS_PROPRIETARY = 7 EXT_POSE = 8 @@ -169,25 +170,53 @@ def send_short_lpp_packet(self, dest_id, data): def send_emergency_stop(self): """ - Send emergency stop - """ - - pk = CRTPPacket() - pk.port = CRTPPort.LOCALIZATION - pk.channel = self.GENERIC_CH - pk.data = struct.pack('= 12: + self._cf.supervisor.send_emergency_stop() + else: + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack('= 12: + self._cf.supervisor.send_emergency_stop_watchdog() + else: + pk = CRTPPacket() + pk.port = CRTPPort.LOCALIZATION + pk.channel = self.GENERIC_CH + pk.data = struct.pack('. +""" +Provides access to the supervisor module of the Crazyflie platform. +""" +import logging +import threading +import time +import warnings + +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.crtpstack import CRTPPort + +__author__ = 'Bitcraze AB' +__all__ = ['Supervisor'] + +logger = logging.getLogger(__name__) + +SUPERVISOR_CH_INFO = 0 +SUPERVISOR_CH_COMMAND = 1 + +# Bit positions +BIT_CAN_BE_ARMED = 0 +BIT_IS_ARMED = 1 +BIT_IS_AUTO_ARMED = 2 +BIT_CAN_FLY = 3 +BIT_IS_FLYING = 4 +BIT_IS_TUMBLED = 5 +BIT_IS_LOCKED = 6 +BIT_IS_CRASHED = 7 +BIT_HL_CONTROL_ACTIVE = 8 +BIT_HL_TRAJ_FINISHED = 9 +BIT_HL_CONTROL_DISABLED = 10 + +CMD_GET_STATE_BITFIELD = 0x0C + +CMD_ARM_SYSTEM = 0x01 +CMD_RECOVER_SYSTEM = 0x02 +CMD_EMERGENCY_STOP = 0x03 +CMD_EMERGENCY_STOP_WATCHDOG = 0x04 + + +class Supervisor: + """ + This class provides two main functionalities: + + 1. **Reading the system state** + Accesses the Crazyflie's supervisor bitfield to determine the current state, + such as whether it can be armed, is flying or crashed. + + 2. **Sending system commands** + Sends arming/disarming requests and crash recovery commands to the Crazyflie. + """ + STATES = [ + 'Can be armed', + 'Is armed', + 'Is auto armed', + 'Can fly', + 'Is flying', + 'Is tumbled', + 'Is locked', + 'Is crashed', + 'HL control is active', + 'Finished HL trajectory', + 'HL control is disabled' + ] + + def __init__(self, crazyflie): + self._cf = crazyflie + + self._cache_timeout = 0.1 # seconds + self._last_fetch_time = 0 + self._bitfield = None + self._cf.add_port_callback(CRTPPort.SUPERVISOR, self._supervisor_callback) + self._bitfield_received = threading.Event() + + def _check_protocol_version(self): + """Returns True if the protocol version is supported, False otherwise.""" + version = self._cf.platform.get_protocol_version() + if version < 12: + warnings.warn( + 'The supervisor subsystem requires CRTP protocol version 12 or later. ' + f'Connected Crazyflie reports version {version}. ' + 'Update your Crazyflie firmware.', + stacklevel=3, + ) + return False + return True + + def _supervisor_callback(self, pk: CRTPPacket): + """ + Called when a packet is received. + """ + if len(pk.data) < 1: + return + + cmd = pk.data[0] + if cmd & 0x80: # high bit = response + orig_cmd = cmd & 0x7F + if orig_cmd == CMD_GET_STATE_BITFIELD: + self._bitfield = int.from_bytes(pk.data[1:], byteorder='little') + self._bitfield_received.set() + logger.info(f'Supervisor bitfield received: 0x{self._bitfield:04X}') + + elif orig_cmd == CMD_ARM_SYSTEM: + success = bool(pk.data[1]) + is_armed = bool(pk.data[2]) + logger.info(f'Arm response: success={success}, is_armed={is_armed}') + + elif orig_cmd == CMD_RECOVER_SYSTEM: + success = bool(pk.data[1]) + recovered = bool(pk.data[2]) + logger.info(f'Recovery response: success={success}, recovered={recovered}') + + def _fetch_bitfield(self, timeout=0.2): + """ + Request the bitfield and wait for response (blocking). + Uses time-based cache to avoid sending packages too frequently. + """ + if not self._check_protocol_version(): + return 0 + now = time.time() + + # Return cached value if it's recent enough + if self._bitfield is not None and (now - self._last_fetch_time) < self._cache_timeout: + return self._bitfield + + # Send a new request + self._bitfield_received.clear() + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_INFO) + pk.data = [CMD_GET_STATE_BITFIELD] + self._cf.send_packet(pk) + + # Wait for response + if not self._bitfield_received.wait(timeout): + logger.warning('Timeout waiting for supervisor bitfield response') + return self._bitfield or 0 # still return last known value + + # Update timestamp + self._last_fetch_time = time.time() + return self._bitfield or 0 + + def _bit(self, position): + bitfield = self._fetch_bitfield() + return bool((bitfield >> position) & 0x01) + + def read_bitfield(self): + """ + Directly get the full bitfield value. + """ + return self._fetch_bitfield() + + def read_state_list(self): + """ + Reads the bitfield and returns the list of all active states. + """ + bitfield = self.read_bitfield() + state_list = self.decode_bitfield(bitfield) + return state_list + + def decode_bitfield(self, value): + """ + Given a bitfield integer `value` and a list of `self.STATES`, + returns the names of all states whose bits are set. + Bit 0 corresponds to states[0], Bit 1 to states[1], etc. + * Bit 0 = Can be armed - the system can be armed and will accept an arming command. + * Bit 1 = Is armed - the system is armed. + * Bit 2 = Is auto armed - the system is configured to automatically arm. + * Bit 3 = Can fly - the Crazyflie is ready to fly. + * Bit 4 = Is flying - the Crazyflie is flying. + * Bit 5 = Is tumbled - the Crazyflie is up side down. + * Bit 6 = Is locked - the Crazyflie is in the locked state and must be restarted. + * Bit 7 = Is crashed - the Crazyflie has crashed. + * Bit 8 = High level control is actively flying the drone. + * Bit 9 = High level trajectory has finished. + * Bit 10 = High level control is disabled and not producing setpoints. + """ + if value < 0: + raise ValueError('value must be >= 0') + + result = [] + for bit_index, name in enumerate(self.STATES): + if value & (1 << bit_index): + result.append(name) + + return result + + @property + def can_be_armed(self): + return self._bit(BIT_CAN_BE_ARMED) + + @property + def is_armed(self): + return self._bit(BIT_IS_ARMED) + + @property + def is_auto_armed(self): + return self._bit(BIT_IS_AUTO_ARMED) + + @property + def can_fly(self): + return self._bit(BIT_CAN_FLY) + + @property + def is_flying(self): + return self._bit(BIT_IS_FLYING) + + @property + def is_tumbled(self): + return self._bit(BIT_IS_TUMBLED) + + @property + def is_locked(self): + return self._bit(BIT_IS_LOCKED) + + @property + def is_crashed(self): + return self._bit(BIT_IS_CRASHED) + + @property + def hl_control_active(self): + return self._bit(BIT_HL_CONTROL_ACTIVE) + + @property + def hl_traj_finished(self): + return self._bit(BIT_HL_TRAJ_FINISHED) + + @property + def hl_control_disabled(self): + return self._bit(BIT_HL_CONTROL_DISABLED) + + def send_arming_request(self, do_arm: bool): + """ + Send system arm/disarm request. + + Args: + do_arm (bool): True = arm the system, False = disarm the system + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_ARM_SYSTEM, do_arm) + self._cf.send_packet(pk) + logger.debug(f'Sent arming request: do_arm={do_arm}') + + def send_crash_recovery_request(self): + """ + Send crash recovery request. + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_RECOVER_SYSTEM,) + self._cf.send_packet(pk) + logger.debug('Sent crash recovery request') + + def send_emergency_stop(self): + """ + Send emergency stop. The Crazyflie will immediately stop all motors. + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_EMERGENCY_STOP,) + self._cf.send_packet(pk) + logger.debug('Sent emergency stop') + + def send_emergency_stop_watchdog(self): + """ + Send emergency stop watchdog. The Crazyflie will stop all motors + unless this command is repeated at regular intervals. + """ + if not self._check_protocol_version(): + return + pk = CRTPPacket() + pk.set_header(CRTPPort.SUPERVISOR, SUPERVISOR_CH_COMMAND) + pk.data = (CMD_EMERGENCY_STOP_WATCHDOG,) + self._cf.send_packet(pk) + logger.debug('Sent emergency stop watchdog') diff --git a/cflib/crtp/crtpstack.py b/cflib/crtp/crtpstack.py index 2cb9118e7..8204764cd 100644 --- a/cflib/crtp/crtpstack.py +++ b/cflib/crtp/crtpstack.py @@ -47,6 +47,7 @@ class CRTPPort: LOCALIZATION = 0x06 COMMANDER_GENERIC = 0x07 SETPOINT_HL = 0x08 + SUPERVISOR = 0x09 PLATFORM = 0x0D LINKCTRL = 0x0F ALL = 0xFF diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 303f49536..eef11b70c 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -139,7 +139,7 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) take_off_simple(scf) @@ -230,7 +230,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) take_off_simple(scf) @@ -323,7 +323,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) move_linear_simple(scf) @@ -436,7 +436,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() @@ -553,7 +553,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() @@ -684,7 +684,7 @@ if __name__ == '__main__': sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() 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/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py index da86b55b9..abb6194a6 100644 --- a/examples/autonomy/motion_commander_demo.py +++ b/examples/autonomy/motion_commander_demo.py @@ -53,7 +53,7 @@ with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) # We take off when the commander is created 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/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/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/erase_ow.py b/examples/memory/erase_ow.py deleted file mode 100644 index f79508d6b..000000000 --- a/examples/memory/erase_ow.py +++ /dev/null @@ -1,145 +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 . -""" -Warning: you will have to write new data to the memory to make it -usable again. Use with caution. - -Simple example that connects to the first Crazyflie found, looks for -EEPROM memories and erases 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.INFO) - - -class EEPROMExample: - """ - Simple example listing the EEPROMs found and erases 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_1W) - print('Found {} 1-wire memories'.format(len(mems))) - if len(mems) > 0: - print('Erasing memory {}'.format(mems[0].id)) - mems[0].erase(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 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__': - input('Warning, this will erase EEPROM memory, press enter to continue...') - - # 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/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 f39be376e..000000000 Binary files a/examples/positioning/monalisa.png and /dev/null differ diff --git a/examples/radio/radio_test.py b/examples/radio/radio_test.py deleted file mode 100644 index 4fe51a3e6..000000000 --- a/examples/radio/radio_test.py +++ /dev/null @@ -1,160 +0,0 @@ -# Eric Yihan Chen -# The Automatic Coordination of Teams (ACT) Lab -# University of Southern California -# ericyihc@usc.edu -''' - Simple example that connects to the first Crazyflie found, triggers - reading of rssi data and acknowledgement rate for every channel (0 to 125). - It finally sets the Crazyflie channel back to default, plots link - quality data, and offers good channel suggestion. - - This script should be used on a Crazyflie with bluetooth disabled and RSSI - ack packet enabled to get RSSI feedback. To configure the Crazyflie in this - mode build the crazyflie2-nrf-firmware with `CFLAGS += -DRSSI_ACK_PACKET=1` - in `config.mk`. Additionally, the Crazyflie must be using the default address - 0xE7E7E7E7E7. See https://github.com/bitcraze/crazyflie-lib-python/issues/131 - for more information. -''' -import argparse - -import matplotlib.pyplot as plt -import numpy as np - -import cflib.drivers.crazyradio as crazyradio - -radio = crazyradio.Crazyradio() - -# optional user input -parser = argparse.ArgumentParser(description='Key variables') -parser.add_argument( - '-try', '--try', dest='TRY', type=int, default=100, - help='the time to send data for each channel' -) -# by default my crazyflie uses channel 80 -parser.add_argument( - '-channel', '--channel', dest='channel', type=int, - default=80, help='the default channel in crazyflie' -) -# by default my crazyflie uses datarate 2M -parser.add_argument( - '-rate', '--rate', dest='rate', type=int, default=2, - help='the default datarate in crazyflie' -) -parser.add_argument( - '-frac', '--fraction', dest='fraction', type=float, - default=0.25, help='top fraction of suggested channels' -) -args = parser.parse_args() - -init_channel = args.channel -TRY = args.TRY -Fraction = args.fraction -data_rate = args.rate - -radio.set_channel(init_channel) -radio.set_data_rate(data_rate) -SET_RADIO_CHANNEL = 1 - -rssi_std = [] -rssi = [] -ack = [] -radio.set_arc(0) - -for channel in range(0, 126, 1): - - # change Crazyflie channel - for x in range(50): - radio.send_packet((0xff, 0x03, SET_RADIO_CHANNEL, channel)) - - count = 0 - temp = [] - - # change radio channel - radio.set_channel(channel) - - for i in range(TRY): - pk = radio.send_packet((0xff, )) - if pk.ack: - count += 1 - if pk.ack and len(pk.data) > 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/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index b557cce2d..58ce5891d 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -125,7 +125,7 @@ def param_deck_flow(_, value_str): sys.exit(1) # Arm the Crazyflie - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.0) logconf.start() diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index b562ac58b..ae098e0f0 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -45,7 +45,7 @@ def light_check(scf: SyncCrazyflie): def arm(scf: SyncCrazyflie): - scf.cf.platform.send_arming_request(True) + scf.cf.supervisor.send_arming_request(True) time.sleep(1.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/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)